libvisiontransfer  10.8.0
datachannel-imu-bno080.cpp
1 /*******************************************************************************
2  * Copyright (c) 2024 Allied Vision Technologies 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/internal/datachannel-imu-bno080.h>
16 #include <visiontransfer/internal/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((int) (myTime/1000000), (int) (myTime%1000000), status, x, z, -y);
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  case SH2Constants::SENSOR_GAME_ROTATION_VECTOR://0x08
65  case SH2Constants::SENSOR_GEOMAGNETIC_ROTATION://0x09
66  {
67  double x, y, z, w;
68  double accuracy = -1.0;
69  auto q = sh2GetSensorQPoint(sensorid);
70  x = sh2ConvertFixedQ16(sh2GetU16(data+4), q);
71  y = sh2ConvertFixedQ16(sh2GetU16(data+6), q);
72  z = sh2ConvertFixedQ16(sh2GetU16(data+8), q);
73  w = sh2ConvertFixedQ16(sh2GetU16(data+10), q);
74  if (sensorid!=SH2Constants::SENSOR_GAME_ROTATION_VECTOR) {
75  // The BNO080 'game rotation vectors' to not provide an accuracy estimate
76  // (since they do not estimate yaw in a fixed geomagnetic system).
77  accuracy = (double) ((signed short) sh2GetU16(data+12)) / (double) (1 << 12); // accuracy Q point is 12
78  }
79  lastRotationQuaternion = TimestampedQuaternion((int) (myTime/1000000), (int) (myTime%1000000), status, x, z, -y, w, accuracy);
80  ringbufRotationQuaternion.pushData(lastRotationQuaternion);
81  break;
82  }
83  // the misc. sensors are 1D floats (32b or 16b)
84  case SH2Constants::SENSOR_PRESSURE: // 0x0a
85  case SH2Constants::SENSOR_AMBIENT_LIGHT: // 0x0b
86  {
87  signed short svalue = sh2GetU32(data+4);
88  double value = (double) svalue / (double)(1 << sh2GetSensorQPoint(sensorid));
89  lastScalar[sensorid - 0x0a] = TimestampedScalar((int) (myTime/1000000), (int) (myTime%1000000), status, value);
90  ringbufScalar[sensorid - 0x0a].pushData(lastScalar[sensorid - 0x0a]);
91  break;
92  }
93  case SH2Constants::SENSOR_HUMIDITY: // 0x0c
94  case SH2Constants::SENSOR_PROXIMITY: // 0x0d
95  case SH2Constants::SENSOR_TEMPERATURE: // 0x0e
96  {
97  signed short svalue = sh2GetU16(data+4);
98  double value = (double) svalue / (double)(1 << sh2GetSensorQPoint(sensorid));
99  lastScalar[sensorid - 0x0a] = TimestampedScalar((int) (myTime/1000000), (int) (myTime%1000000), status, value);
100  ringbufScalar[sensorid - 0x0a].pushData(lastScalar[sensorid - 0x0a]);
101  break;
102  }
103  default:
104  break;
105  }
106  int recordlen = sh2GetSensorReportLength(sensorid);
107  return recordlen;
108 }
109 
110 void ClientSideDataChannelIMUBNO080::handleChunk(unsigned char* data, int datalen) {
111  if (datalen < 5) return;
112  auto cargobase = reinterpret_cast<SH2CargoBase*>(data);
113  static uint64_t interruptTime = 0; // will always be reported first, below
114  switch (cargobase->getReportType()) {
115  case 0xff: { // Our own interrupt-synchronized timestamp
116  auto report = reinterpret_cast<SH2CargoBodyScenescanTimestamp*>(data);
117  interruptTime = report->getUSecSinceEpoch();
118  break;
119  }
120  case 0xfb: { // SH-2 Time Base (followed by sensor reports)
121  auto report = reinterpret_cast<SH2CargoBodyTimeBase*>(data);
122  long basetimeOfs = report->getTimeBase();
123  uint64_t localBase = interruptTime - basetimeOfs;
124  data += sizeof(SH2CargoBodyTimeBase); datalen -= sizeof(SH2CargoBodyTimeBase);
125  // The (variable-length) remainder of this packet are concatenated SH2 sensor input reports.
126  // They must be parsed in order since they are of differing sizes, depending on the sensor type.
127  int recordlen;
128  while (datalen > 0) {
129  recordlen = handleSensorInputRecord(data, datalen, localBase);
130  if (recordlen<1) break; // record type unknown -> size unknown -> cannot proceed
131  data += recordlen; datalen -= recordlen;
132  }
133  break;
134  }
135  case 0xfa: // SH-2 Timestamp Rebase
136  // Required for BNO batch reports that span >1.6s.
137  // This is not relevant here, since we set the batch delay to intervals
138  // considerably shorter than that (the server stores those batches
139  // immediately with integrated base timestamps).
140  default: {
141  }
142  }
143 }
144 
145 int ClientSideDataChannelIMUBNO080::handleMessage(DataChannelMessage& message, sockaddr_in* sender) {
146  unsigned char* data = message.payload;
147  int datalen = message.header.payloadSize;
148  while (datalen > 0) {
149  int elemlen = sh2GetU16(data) & 0x7fff;
150  handleChunk(data, elemlen);
151  data += elemlen; datalen -= elemlen;
152  }
153  return 1;
154 };
155 
156 }} // namespaces
157 
visiontransfer::TimestampedQuaternion
Encapsulate a 4D (quaternion) sensor report, containing X, Y, Z, W, as well as timestamp and status f...
Definition: sensordata.h:80
visiontransfer::TimestampedVector
Encapsulate a 3D sensor report, containing X, Y, Z, as well as timestamp and status fields.
Definition: sensordata.h:64
visiontransfer::TimestampedScalar
Encapsulate a scalar sensor measurement, containing the value, as well as timestamp and status fields...
Definition: sensordata.h:51
visiontransfer::internal::ClientSideDataChannelIMUBNO080::handleMessage
int handleMessage(DataChannelMessage &message, sockaddr_in *sender) override
Channel-dependent message handlers in respective channel implementations.
Definition: datachannel-imu-bno080.cpp:169
Allied Vision