15 #include <visiontransfer/datachannel-imu-bno080.h> 16 #include <visiontransfer/protocol-sh2-imu-bno080.h> 21 ClientSideDataChannelIMUBNO080::ClientSideDataChannelIMUBNO080()
23 infoString =
"Receiver for the BNO080 IMU sensor";
25 lastXYZ[0x01 - 1] = {0, 0, 0, 0, 0, 10};
26 lastXYZ[0x02 - 1] = {0, 0, 0, 0, 0, 0};
27 lastXYZ[0x03 - 1] = {0, 0, 0, 0, 0, 0};
28 lastXYZ[0x04 - 1] = {0, 0, 0, 0, 0, 0};
29 lastXYZ[0x05 - 1] = {0, 0, 0, 0, 0, 0};
30 lastXYZ[0x06 - 1] = {0, 0, 0, 0, 0, 10};
31 lastScalar[0x0a - 0x0a] = {0, 0, 0, 0};
32 lastScalar[0x0b - 0x0a] = {0, 0, 0, 0};
33 lastScalar[0x0d - 0x0a] = {0, 0, 0, 0};
34 lastScalar[0x0d - 0x0a] = {0, 0, 0, 0};
35 lastScalar[0x0e - 0x0a] = {0, 0, 0, 0};
36 lastRotationQuaternion = {0, 0, 0, 0.0, 0.0, 0.0, 1.0, 0};
39 int ClientSideDataChannelIMUBNO080::handleSensorInputRecord(
unsigned char* data,
int datalen, uint64_t baseTime) {
40 int sensorid = data[0];
41 int status = data[2] & 3;
42 int delay = ((data[2] & 0xfc) << 6) | data[3];
43 uint64_t myTime = baseTime + delay;
46 case SH2Constants::SENSOR_ACCELEROMETER:
47 case SH2Constants::SENSOR_GYROSCOPE:
48 case SH2Constants::SENSOR_MAGNETOMETER:
49 case SH2Constants::SENSOR_LINEAR_ACCELERATION:
50 case SH2Constants::SENSOR_GRAVITY:
53 auto q = sh2GetSensorQPoint(sensorid);
54 x = sh2ConvertFixedQ16(sh2GetU16(data+4), q);
55 y = sh2ConvertFixedQ16(sh2GetU16(data+6), q);
56 z = sh2ConvertFixedQ16(sh2GetU16(data+8), q);
58 lastXYZ[sensorid-1] =
TimestampedVector((
int) (myTime/1000000), (
int) (myTime%1000000), status, x, y, z);
59 ringbufXYZ[sensorid-1].pushData(lastXYZ[sensorid-1]);
63 case SH2Constants::SENSOR_ROTATION_VECTOR:
67 auto q = sh2GetSensorQPoint(sensorid);
68 i = sh2ConvertFixedQ16(sh2GetU16(data+4), q);
69 j = sh2ConvertFixedQ16(sh2GetU16(data+6), q);
70 k = sh2ConvertFixedQ16(sh2GetU16(data+8), q);
71 r = sh2ConvertFixedQ16(sh2GetU16(data+10), q);
72 accuracy = (double) ((
signed short) sh2GetU16(data+12)) / (
double) (1 << 12);
73 lastRotationQuaternion =
TimestampedQuaternion((
int) (myTime/1000000), (
int) (myTime%1000000), status, i, j, k, r, accuracy);
74 ringbufRotationQuaternion.pushData(lastRotationQuaternion);
78 case SH2Constants::SENSOR_PRESSURE:
79 case SH2Constants::SENSOR_AMBIENT_LIGHT:
81 signed short svalue = sh2GetU32(data+4);
82 double value = (double) svalue / (
double)(1 << sh2GetSensorQPoint(sensorid));
83 lastScalar[sensorid - 0x0a] =
TimestampedScalar((
int) (myTime/1000000), (
int) (myTime%1000000), status, value);
84 ringbufScalar[sensorid - 0x0a].pushData(lastScalar[sensorid - 0x0a]);
87 case SH2Constants::SENSOR_HUMIDITY:
88 case SH2Constants::SENSOR_PROXIMITY:
89 case SH2Constants::SENSOR_TEMPERATURE:
91 signed short svalue = sh2GetU16(data+4);
92 double value = (double) svalue / (
double)(1 << sh2GetSensorQPoint(sensorid));
93 lastScalar[sensorid - 0x0a] =
TimestampedScalar((
int) (myTime/1000000), (
int) (myTime%1000000), status, value);
94 ringbufScalar[sensorid - 0x0a].pushData(lastScalar[sensorid - 0x0a]);
100 int recordlen = sh2GetSensorReportLength(sensorid);
104 void ClientSideDataChannelIMUBNO080::handleChunk(
unsigned char* data,
int datalen) {
105 if (datalen < 5)
return;
106 auto cargobase =
reinterpret_cast<SH2CargoBase*
>(data);
107 static uint64_t interruptTime = 0;
108 switch (cargobase->getReportType()) {
110 auto report =
reinterpret_cast<SH2CargoBodyScenescanTimestamp*
>(data);
111 interruptTime = report->getUSecSinceEpoch();
115 auto report =
reinterpret_cast<SH2CargoBodyTimeBase*
>(data);
116 long basetimeOfs = report->getTimeBase();
117 uint64_t localBase = interruptTime - basetimeOfs;
118 data +=
sizeof(SH2CargoBodyTimeBase); datalen -=
sizeof(SH2CargoBodyTimeBase);
122 while (datalen > 0) {
123 recordlen = handleSensorInputRecord(data, datalen, localBase);
124 if (recordlen<1)
break;
125 data += recordlen; datalen -= recordlen;
140 unsigned char* data = message.payload;
141 int datalen = message.header.payloadSize;
142 while (datalen > 0) {
143 int elemlen = sh2GetU16(data) & 0x7fff;
144 handleChunk(data, elemlen);
145 data += elemlen; datalen -= elemlen;
Encapsulate a 4D (quaternion) sensor report, containing i, j, k, r, as well as timestamp and status f...
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields...
int handleMessage(DataChannelMessage &message, sockaddr_in *sender) override
Channel-dependent message handlers in respective channel implementations.