libvisiontransfer  7.2.0
datachannel-imu-bno080.cpp
1 /*******************************************************************************
2  * Copyright (c) 2019 Nerian Vision GmbH
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a copy
5  * of this software and associated documentation files (the "Software"), to deal
6  * in the Software without restriction, including without limitation the rights
7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8  * copies of the Software, and to permit persons to whom the Software is
9  * furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *******************************************************************************/
14 
15 #include <visiontransfer/datachannel-imu-bno080.h>
16 #include <visiontransfer/protocol-sh2-imu-bno080.h>
17 
18 namespace visiontransfer {
19 namespace internal {
20 
21 ClientSideDataChannelIMUBNO080::ClientSideDataChannelIMUBNO080()
22 : DataChannel() {
23  infoString = "Receiver for the BNO080 IMU sensor";
24  // Sane defaults for orientation etc. if values are queried despite lack of 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}; // unused, cf. the quaternion below
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}; // unused / sensor not present
33  lastScalar[0x0d - 0x0a] = {0, 0, 0, 0};
34  lastScalar[0x0d - 0x0a] = {0, 0, 0, 0}; // unused / sensor not present
35  lastScalar[0x0e - 0x0a] = {0, 0, 0, 0};
36  lastRotationQuaternion = {0, 0, 0, 0.0, 0.0, 0.0, 1.0, 0}; // channel 0x05
37 }
38 
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;
44  switch (sensorid) {
45  // these have identical format, 3D vector
46  case SH2Constants::SENSOR_ACCELEROMETER: //0x01
47  case SH2Constants::SENSOR_GYROSCOPE: //0x02
48  case SH2Constants::SENSOR_MAGNETOMETER: //0x03
49  case SH2Constants::SENSOR_LINEAR_ACCELERATION: //0x04
50  case SH2Constants::SENSOR_GRAVITY: //0x06
51  {
52  double x, y, z;
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);
57  // sensorid-1 is in range [0..5]
58  lastXYZ[sensorid-1] = TimestampedVector(myTime/1000000, myTime%1000000, status, x, y, z);
59  ringbufXYZ[sensorid-1].pushData(lastXYZ[sensorid-1]);
60  break;
61  }
62  // this one is 4D (quaternion data), plus accuracy field
63  case SH2Constants::SENSOR_ROTATION_VECTOR: //0x05
64  {
65  double i, j, k, r;
66  double accuracy;
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) (2 << 12); // accuracy Q point is 12
73  lastRotationQuaternion = TimestampedQuaternion(myTime/1000000, myTime%1000000, status, i, j, k, r, accuracy);
74  ringbufRotationQuaternion.pushData(lastRotationQuaternion);
75  break;
76  }
77  // the misc. sensors are 1D floats (32b or 16b)
78  case SH2Constants::SENSOR_PRESSURE: // 0x0a
79  case SH2Constants::SENSOR_AMBIENT_LIGHT: // 0x0b
80  {
81  signed short svalue = sh2GetU32(data+4);
82  double value = (double) svalue / (double)(1 << sh2GetSensorQPoint(sensorid));
83  lastScalar[sensorid - 0x0a] = TimestampedScalar(myTime/1000000, myTime%1000000, status, value);
84  ringbufScalar[sensorid - 0x0a].pushData(lastScalar[sensorid - 0x0a]);
85  break;
86  }
87  case SH2Constants::SENSOR_HUMIDITY: // 0x0c
88  case SH2Constants::SENSOR_PROXIMITY: // 0x0d
89  case SH2Constants::SENSOR_TEMPERATURE: // 0x0e
90  {
91  signed short svalue = sh2GetU16(data+4);
92  double value = (double) svalue / (double)(1 << sh2GetSensorQPoint(sensorid));
93  lastScalar[sensorid - 0x0a] = TimestampedScalar(myTime/1000000, myTime%1000000, status, value);
94  ringbufScalar[sensorid - 0x0a].pushData(lastScalar[sensorid - 0x0a]);
95  break;
96  }
97  default:
98  break;
99  }
100  int recordlen = sh2GetSensorReportLength(sensorid);
101  return recordlen;
102 }
103 
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; // will always be reported first, below
108  switch (cargobase->getReportType()) {
109  case 0xff: { // Our own interrupt-synchronized timestamp
110  auto report = reinterpret_cast<SH2CargoBodyScenescanTimestamp*>(data);
111  interruptTime = report->getUSecSinceEpoch();
112  break;
113  }
114  case 0xfb: { // SH-2 Time Base (followed by sensor reports)
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);
119  // The (variable-length) remainder of this packet are concatenated SH2 sensor input reports.
120  // They must be parsed in order since they are of differing sizes, depending on the sensor type.
121  int recordlen;
122  while (datalen > 0) {
123  recordlen = handleSensorInputRecord(data, datalen, localBase);
124  if (recordlen<1) break; // record type unknown -> size unknown -> cannot proceed
125  data += recordlen; datalen -= recordlen;
126  }
127  break;
128  }
129  case 0xfa: // SH-2 Timestamp Rebase
130  // Required for BNO batch reports that span >1.6s.
131  // This is not relevant here, since we set the batch delay to intervals
132  // considerably shorter than that (the server stores those batches
133  // immediately with integrated base timestamps).
134  default: {
135  }
136  }
137 }
138 
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;
146  }
147  return 1;
148 };
149 
150 }} // namespaces
151 
Encapsulate a 4D (quaternion) sensor report, containing i, j, k, r, as well as timestamp and status f...
Definition: sensordata.h:66
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
Definition: sensordata.h:37
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields...
Definition: sensordata.h:50
int handleMessage(DataChannelMessage &message, sockaddr_in *sender) override
Channel-dependent message handlers in respective channel implementations.
Nerian Vision Technologies