public static void printStationaryBeaconsPositionsFromMarvelmindHedge(MarvelmindHedge hedge, bool onlyNew) { StationaryBeaconsPositions positions = new StationaryBeaconsPositions(); double xm, ym, zm; positions = getStationaryBeaconsPositionsFromMarvelmindHedge(hedge); if (positions.updated || (!onlyNew)) { byte i; byte n = hedge.positionsBeacons.numBeacons; StationaryBeaconPosition b; for (i = 0; i < n; i++) { b = positions.beacons[i]; xm = ((double)b.x) / 1000.0; ym = ((double)b.y) / 1000.0; zm = ((double)b.z) / 1000.0; if (positions.beacons[i].highResolution) { Console.WriteLine("Stationary beacon: address: " + b.address + ", X: " + xm + ", Y: " + ym + ", Z: " + zm + " \n"); } else { Console.WriteLine("Stationary beacon: address: " + b.address + ", X: " + xm + ", Y: " + ym + ", Z: " + zm + " \n"); } } hedge.positionsBeacons.updated = false; } }
static void process_beacons_positions_highres_datagram(MarvelmindHedge hedge, byte[] buffer) { byte n = buffer[5];// number of beacons in packet byte i, ofs; byte address; Int32 x, y, z; StationaryBeaconPosition b; if ((1 + n * 14) != buffer[4]) { return;// incorrect size } for (i = 0; i < n; i++) { ofs = Convert.ToByte(6 + i * 14); address = buffer[ofs + 0]; x = Convert.ToInt32(buffer[ofs + 1] | (((UInt32)buffer[ofs + 2]) << 8) | (((UInt32)buffer[ofs + 3]) << 16) | (((UInt32)buffer[ofs + 4]) << 24)); y = Convert.ToInt32(buffer[ofs + 5] | (((UInt32)buffer[ofs + 6]) << 8) | (((UInt32)buffer[ofs + 7]) << 16) | (((UInt32)buffer[ofs + 8]) << 24)); z = Convert.ToInt32(buffer[ofs + 9] | (((UInt32)buffer[ofs + 10]) << 8) | (((UInt32)buffer[ofs + 11]) << 16) | (((UInt32)buffer[ofs + 12]) << 24)); b = getOrAllocBeacon(hedge, address); if (b != null) { b.address = address; b.x = x; b.y = y; b.z = z; b.highResolution = true; hedge.positionsBeacons.updated = true; } } }
static PositionValue process_position_highres_datagram(MarvelmindHedge hedge, byte[] buffer) { byte ind = hedge.lastValues_next; hedge.positionBuffer[ind].address = buffer[22]; hedge.positionBuffer[ind].timestamp = buffer[5] | (((UInt32)buffer[6]) << 8) | (((UInt32)buffer[7]) << 16) | (((UInt32)buffer[8]) << 24); Int32 vx = (Int32)(buffer[9] | (((UInt32)buffer[10]) << 8) | (((UInt32)buffer[11]) << 16) | (((UInt32)buffer[12]) << 24)); hedge.positionBuffer[ind].x = vx; Int32 vy = (Int32)(buffer[13] | (((UInt32)buffer[14]) << 8) | (((UInt32)buffer[15]) << 16) | (((UInt32)buffer[16]) << 24)); hedge.positionBuffer[ind].y = vy; Int32 vz = (Int32)(buffer[17] | (((UInt32)buffer[18]) << 8) | (((UInt32)buffer[19]) << 16) | (((UInt32)buffer[20]) << 24)); hedge.positionBuffer[ind].z = vz; UInt16 vang = (UInt16)(buffer[23] | ((buffer[24]) << 8)); hedge.positionBuffer[ind].angle = ((float)(vang & 0x0fff)) / 10.0f; hedge.positionBuffer[ind].highResolution = true; ind = markPositionReady(hedge); return(hedge.positionBuffer[ind]); }
static byte markPositionReady(MarvelmindHedge hedge) { byte ind = hedge.lastValues_next; byte indCur = ind; hedge.positionBuffer[ind].ready = true; hedge.positionBuffer[ind].processed = false; ind++; if (ind >= (byte)Command.MAX_BUFFERED_POSITIONS) { ind = 0; } if (hedge.lastValuesCount_ < (byte)Command.MAX_BUFFERED_POSITIONS) { hedge.lastValuesCount_++; } hedge.haveNewValues_ = true; hedge.lastValues_next = ind; return(indCur); }
public indoor() { SerialPort mySerialPort = new SerialPort("COM6"); mySerialPort.BaudRate = 9600; mySerialPort.Parity = Parity.None; mySerialPort.StopBits = StopBits.One; mySerialPort.DataBits = 8; mySerialPort.Handshake = Handshake.None; mySerialPort.RtsEnable = true; mySerialPort.DataReceived += new SerialDataReceivedEventHandler(DataReceivedHandler); mySerialPort.Open(); hedge = new MarvelmindHedge(); Console.WriteLine("Press any key to continue..."); Console.WriteLine(); Console.ReadKey(); mySerialPort.Close(); }
static void process_beacons_positions_datagram(MarvelmindHedge hedge, byte[] buffer) { byte n = buffer[5];// number of beacons in packet byte i, ofs; byte address; Int16 x, y, z; StationaryBeaconPosition b; if ((1 + n * 8) != buffer[4]) { return;// incorrect size } for (i = 0; i < n; i++) { ofs = Convert.ToByte(6 + i * 8); address = buffer[ofs + 0]; x = (Int16)(buffer[ofs + 1] | ((buffer[ofs + 2]) << 8)); y = (Int16)(buffer[ofs + 3] | ((buffer[ofs + 4]) << 8)); z = (Int16)(buffer[ofs + 5] | ((buffer[ofs + 6]) << 8)); b = getOrAllocBeacon(hedge, address); if (b != null) { b.address = address; b.x = x * 10; // millimeters b.y = y * 10; // millimeters b.z = z * 10; // millimeters b.highResolution = false; hedge.positionsBeacons.updated = true; } } }
static PositionValue process_position_datagram(MarvelmindHedge hedge, byte[] buffer) { byte ind = hedge.lastValues_next; hedge.positionBuffer[ind].address = buffer[16]; hedge.positionBuffer[ind].timestamp = buffer[5] | (((UInt32)buffer[6]) << 8) | (((UInt32)buffer[7]) << 16) | (((UInt32)buffer[8]) << 24); Int16 vx = (Int16)(buffer[9] | (((UInt16)buffer[10]) << 8)); hedge.positionBuffer[ind].x = vx * 10;// millimeters Int16 vy = (Int16)(buffer[11] | (((UInt16)buffer[12]) << 8)); hedge.positionBuffer[ind].y = vy * 10;// millimeters Int16 vz = (Int16)(buffer[13] | (((UInt16)buffer[14]) << 8)); hedge.positionBuffer[ind].z = vz * 10;// millimeters UInt16 vang = (UInt16)(buffer[17] | (((UInt16)buffer[18]) << 8)); hedge.positionBuffer[ind].angle = ((float)(vang & 0x0fff)) / 10.0f; hedge.positionBuffer[ind].highResolution = false; ind = markPositionReady(hedge); return(hedge.positionBuffer[ind]); }
static StationaryBeaconPosition getOrAllocBeacon(MarvelmindHedge hedge, byte address) { byte i; byte n_used = hedge.positionsBeacons.numBeacons; if (n_used != 0) { for (i = 0; i < n_used; i++) { if (hedge.positionsBeacons.beacons[i] != null && hedge.positionsBeacons.beacons[i].address == address) { return(hedge.positionsBeacons.beacons[i]); } } } if (n_used >= (Convert.ToByte(Command.MAX_STATIONARY_BEACONS) - 1)) { return(null); } hedge.positionsBeacons.numBeacons = Convert.ToByte(n_used + Convert.ToByte(1)); return(hedge.positionsBeacons.beacons[n_used]); }
public static StationaryBeaconsPositions getStationaryBeaconsPositionsFromMarvelmindHedge(MarvelmindHedge hedge) { return(hedge.positionsBeacons); }