demo version
This commit is contained in:
Josip Milovac
parent
fbb282a801
commit
672d6daa8e
125 changed files with 17918 additions and 1481 deletions
|
|
@ -0,0 +1,443 @@
|
|||
/*******************************************************************************
|
||||
* Copyright (c) 2021 Nerian Vision GmbH
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*******************************************************************************/
|
||||
|
||||
#ifndef VISIONTRANSFER_PROTOCOL_SH2_IMU_BNO080
|
||||
#define VISIONTRANSFER_PROTOCOL_SH2_IMU_BNO080
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace visiontransfer {
|
||||
namespace internal {
|
||||
|
||||
struct SH2Constants {
|
||||
static constexpr uint8_t CHANNEL_COMMAND = 0;
|
||||
static constexpr uint8_t CHANNEL_EXECUTABLE = 1;
|
||||
static constexpr uint8_t CHANNEL_CONTROL = 2;
|
||||
static constexpr uint8_t CHANNEL_REPORTS = 3;
|
||||
static constexpr uint8_t CHANNEL_WAKE_REPORTS= 4;
|
||||
static constexpr uint8_t CHANNEL_GYRO = 5;
|
||||
|
||||
static constexpr uint8_t REPORT_COMMAND_RESPONSE = 0xf1;
|
||||
static constexpr uint8_t REPORT_COMMAND_REQUEST = 0xf2;
|
||||
static constexpr uint8_t REPORT_FRS_READ_RESPONSE = 0xf3;
|
||||
static constexpr uint8_t REPORT_FRS_READ_REQUEST = 0xf4;
|
||||
static constexpr uint8_t REPORT_FRS_WRITE_RESPONSE = 0xf5;
|
||||
static constexpr uint8_t REPORT_FRS_WRITE_DATA = 0xf6;
|
||||
static constexpr uint8_t REPORT_FRS_WRITE_REQUEST = 0xf7;
|
||||
static constexpr uint8_t REPORT_PRODUCT_ID_RESPONSE = 0xf8;
|
||||
static constexpr uint8_t REPORT_PRODUCT_ID_REQUEST = 0xf9;
|
||||
static constexpr uint8_t REPORT_TIMESTAMP_REBASE = 0xfa;
|
||||
static constexpr uint8_t REPORT_BASE_TIMESTAMP_REFERENCE = 0xfb;
|
||||
static constexpr uint8_t REPORT_GET_FEATURE_RESPONSE = 0xfc;
|
||||
static constexpr uint8_t REPORT_SET_FEATURE_COMMAND = 0xfd;
|
||||
static constexpr uint8_t REPORT_GET_FEATURE_REQUEST = 0xfe;
|
||||
|
||||
// Commands supported by report 0xf2 / 0xf1
|
||||
static constexpr uint8_t COMMAND_REPORT_ERRORS = 0x01;
|
||||
static constexpr uint8_t COMMAND_COUNTS = 0x02;
|
||||
static constexpr uint8_t COMMAND_TARE = 0x03;
|
||||
static constexpr uint8_t COMMAND_INITIALIZE = 0x04;
|
||||
static constexpr uint8_t COMMAND_RESERVED_05 = 0x05;
|
||||
static constexpr uint8_t COMMAND_SAVE_DCD = 0x06;
|
||||
static constexpr uint8_t COMMAND_ME_CALIBRATION = 0x07;
|
||||
static constexpr uint8_t COMMAND_RESERVED_08 = 0x08;
|
||||
static constexpr uint8_t COMMAND_PERIODIC_DCD_SAVE = 0x09;
|
||||
static constexpr uint8_t COMMAND_GET_OSCILLATOR_TYPE = 0x0a;
|
||||
static constexpr uint8_t COMMAND_CLEAR_DCD_AND_RESET = 0x0b;
|
||||
static constexpr uint8_t COMMAND_CALIBRATION = 0x0c;
|
||||
static constexpr uint8_t COMMAND_BOOTLOADER = 0x0d;
|
||||
static constexpr uint8_t COMMAND_INTERACTIVE_CALIBRATION = 0x0e;
|
||||
|
||||
// Subcommands, for certain commands only
|
||||
// DCD / ME / Bootloader not considered yet, here
|
||||
static constexpr uint8_t SUBCOMMAND_COUNTS__GET_COUNTS = 0x00;
|
||||
static constexpr uint8_t SUBCOMMAND_COUNTS__CLEAR_COUNTS = 0x01;
|
||||
static constexpr uint8_t SUBCOMMAND_TARE__TARE_NOW = 0x00;
|
||||
static constexpr uint8_t SUBCOMMAND_TARE__PERSIST_TARE = 0x01;
|
||||
static constexpr uint8_t SUBCOMMAND_TARE__SET_REORIENTATION = 0x02;
|
||||
static constexpr uint8_t SUBCOMMAND_CALIBRATION__START_CALIBRATION = 0x00;
|
||||
static constexpr uint8_t SUBCOMMAND_CALIBRATION__FINISH_CALIBRATION = 0x01;
|
||||
|
||||
// What to tare (can be ORed)
|
||||
static constexpr uint8_t TARE_AXIS_X = 1;
|
||||
static constexpr uint8_t TARE_AXIS_Y = 2;
|
||||
static constexpr uint8_t TARE_AXIS_Z = 4;
|
||||
|
||||
// Reference for tare operation
|
||||
static constexpr uint8_t TARE_BASE_ROTATION_VECTOR = 0;
|
||||
static constexpr uint8_t TARE_BASE_GAMING_ROTATION_VECTOR = 1;
|
||||
static constexpr uint8_t TARE_BASE_GEOMAGNETIC_ROTATION_VECTOR = 2;
|
||||
static constexpr uint8_t TARE_BASE_GYRO_INTEGRATED_ROTATION_VECTOR = 3;
|
||||
static constexpr uint8_t TARE_BASE_ARVR_STABILIZED_ROTATION_VECTOR = 4;
|
||||
static constexpr uint8_t TARE_BASE_ARVR_STABILIZED_GAME_ROTATION_VECTOR= 5;
|
||||
|
||||
// Sensor types (= sensor input report ID)
|
||||
static constexpr uint8_t SENSOR_ACCELEROMETER = 0x01;
|
||||
static constexpr uint8_t SENSOR_GYROSCOPE = 0x02;
|
||||
static constexpr uint8_t SENSOR_MAGNETOMETER = 0x03;
|
||||
static constexpr uint8_t SENSOR_LINEAR_ACCELERATION = 0x04;
|
||||
static constexpr uint8_t SENSOR_ROTATION_VECTOR = 0x05;
|
||||
static constexpr uint8_t SENSOR_GRAVITY = 0x06;
|
||||
static constexpr uint8_t SENSOR_GYROSCOPE_UNCALIBRATED = 0x07;
|
||||
static constexpr uint8_t SENSOR_GAME_ROTATION_VECTOR = 0x08;
|
||||
static constexpr uint8_t SENSOR_GEOMAGNETIC_ROTATION = 0x09;
|
||||
static constexpr uint8_t SENSOR_PRESSURE = 0x0a;
|
||||
static constexpr uint8_t SENSOR_AMBIENT_LIGHT = 0x0b;
|
||||
static constexpr uint8_t SENSOR_HUMIDITY = 0x0c;
|
||||
static constexpr uint8_t SENSOR_PROXIMITY = 0x0d;
|
||||
static constexpr uint8_t SENSOR_TEMPERATURE = 0x0e;
|
||||
static constexpr uint8_t SENSOR_MAGNETOMETER_UNCALIBRATED = 0x0f;
|
||||
static constexpr uint8_t SENSOR_TAP_DETECTOR = 0x10;
|
||||
static constexpr uint8_t SENSOR_STEP_COUNTER = 0x11;
|
||||
static constexpr uint8_t SENSOR_SIGNIFICANT_MOTION = 0x12;
|
||||
static constexpr uint8_t SENSOR_STABILITY_CLASSIFIER = 0x13;
|
||||
static constexpr uint8_t SENSOR_ACCELEROMETER_RAW = 0x14;
|
||||
static constexpr uint8_t SENSOR_GYROSCOPE_RAW = 0x15;
|
||||
static constexpr uint8_t SENSOR_MAGNETOMETER_RAW = 0x16;
|
||||
static constexpr uint8_t SENSOR_STEP_DETECTOR = 0x18;
|
||||
static constexpr uint8_t SENSOR_SHAKE_DETECTOR = 0x19;
|
||||
static constexpr uint8_t SENSOR_FLIP_DETECTOR = 0x1a;
|
||||
static constexpr uint8_t SENSOR_PICKUP_DETECTOR = 0x1b;
|
||||
static constexpr uint8_t SENSOR_STABILITY_DETECTOR = 0x1c;
|
||||
static constexpr uint8_t SENSOR_PERSONAL_ACTIVITY_CLASSIFIER = 0x1e;
|
||||
static constexpr uint8_t SENSOR_SLEEP_DETECTOR = 0x1f;
|
||||
static constexpr uint8_t SENSOR_TILT_DETECTOR = 0x20;
|
||||
static constexpr uint8_t SENSOR_POCKET_DETECTOR = 0x21;
|
||||
static constexpr uint8_t SENSOR_CIRCLE_DETECTOR = 0x22;
|
||||
static constexpr uint8_t SENSOR_HEART_RATE_MONITOR = 0x23;
|
||||
static constexpr uint8_t SENSOR_ARVR_STABILIZED_ROTATION_VECTOR = 0x28;
|
||||
static constexpr uint8_t SENSOR_ARVR_STABILIZED_GAME_ROTATION_VECTOR= 0x29;
|
||||
static constexpr uint8_t SENSOR_GYRO_INTEGRATED_ROTATION_VECTOR = 0x2a;
|
||||
static constexpr uint8_t SENSOR_MOTION_REQUEST = 0x2b;
|
||||
|
||||
// FRS Configuration Response: Status/Error field
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_WORDS_RECEIVED = 0x00;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_UNRECOGNIZED_FRS_TYPE = 0x01;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_BUSY = 0x02;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_WRITE_COMPLETED = 0x03;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_WRITE_MODE_READY = 0x04;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_WRITE_FAILED = 0x05;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_UNEXPECTED_DATA = 0x06;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_INVALID_LENGTH = 0x07;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_RECORD_VALID = 0x08;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_RECORD_INVALID = 0x09;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_DEVICE_ERROR__DEPRECATED = 0x0A;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_READ_ONLY_RECORD = 0x0B;
|
||||
static constexpr uint8_t FRS_WRITE_RESPONSE_STATUS_CANNOT_WRITE_MEMORY_FULL = 0x0C;
|
||||
};
|
||||
|
||||
inline uint64_t sh2GetU64(const unsigned char* d) {
|
||||
return d[0] | (d[1] << 8) | (d[2] << 16) | (d[3] << 24)
|
||||
| (static_cast<uint64_t>(d[4]) << 32) | (static_cast<uint64_t>(d[5]) << 40)
|
||||
| (static_cast<uint64_t>(d[6]) << 48) | (static_cast<uint64_t>(d[7]) << 56);
|
||||
}
|
||||
inline uint32_t sh2GetU32(const unsigned char* d) {
|
||||
return d[0] | (d[1] << 8) | (d[2] << 16) | (d[3] << 24);
|
||||
}
|
||||
inline uint16_t sh2GetU16(const unsigned char* d) {
|
||||
return d[0] | (d[1] << 8);
|
||||
}
|
||||
inline uint8_t sh2GetU8(const unsigned char* d) {
|
||||
return d[0];
|
||||
}
|
||||
inline double sh2ConvertFixedQ16(uint16_t v, unsigned char q) {
|
||||
return (double) ((int16_t) v) / (double)(1 << q);
|
||||
}
|
||||
|
||||
inline int sh2GetSensorReportLength(unsigned int sensorReportID) {
|
||||
switch(sensorReportID) {
|
||||
case SH2Constants::SENSOR_ACCELEROMETER: return 10; //ID 0x01
|
||||
case SH2Constants::SENSOR_GYROSCOPE: return 10; //ID 0x02
|
||||
case SH2Constants::SENSOR_MAGNETOMETER: return 10; //ID 0x03
|
||||
case SH2Constants::SENSOR_LINEAR_ACCELERATION: return 10; //ID 0x04
|
||||
case SH2Constants::SENSOR_ROTATION_VECTOR: return 14; //ID 0x05
|
||||
case SH2Constants::SENSOR_GRAVITY: return 10; //ID 0x06
|
||||
case SH2Constants::SENSOR_GYROSCOPE_UNCALIBRATED: return 16; //ID 0x07
|
||||
case SH2Constants::SENSOR_GAME_ROTATION_VECTOR: return 12; //ID 0x08
|
||||
case SH2Constants::SENSOR_GEOMAGNETIC_ROTATION: return 14; //ID 0x09
|
||||
case SH2Constants::SENSOR_PRESSURE: return 8; //ID 0x0a
|
||||
case SH2Constants::SENSOR_AMBIENT_LIGHT: return 8; //ID 0x0b
|
||||
case SH2Constants::SENSOR_HUMIDITY: return 6; //ID 0x0c
|
||||
case SH2Constants::SENSOR_PROXIMITY: return 6; //ID 0x0d
|
||||
case SH2Constants::SENSOR_TEMPERATURE: return 6; //ID 0x0e
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_UNCALIBRATED: return 16; //ID 0x0f
|
||||
case SH2Constants::SENSOR_TAP_DETECTOR: return 5; //ID 0x10
|
||||
case SH2Constants::SENSOR_STEP_COUNTER: return 12; //ID 0x11
|
||||
case SH2Constants::SENSOR_SIGNIFICANT_MOTION: return 6; //ID 0x12
|
||||
case SH2Constants::SENSOR_STABILITY_CLASSIFIER: return 6; //ID 0x13
|
||||
case SH2Constants::SENSOR_ACCELEROMETER_RAW: return 16; //ID 0x14
|
||||
case SH2Constants::SENSOR_GYROSCOPE_RAW: return 16; //ID 0x15
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_RAW: return 16; //ID 0x16
|
||||
case SH2Constants::SENSOR_STEP_DETECTOR: return 8; //ID 0x18
|
||||
case SH2Constants::SENSOR_SHAKE_DETECTOR: return 6; //ID 0x19
|
||||
case SH2Constants::SENSOR_FLIP_DETECTOR: return 6; //ID 0x1a
|
||||
case SH2Constants::SENSOR_PICKUP_DETECTOR: return 6; //ID 0x1b
|
||||
case SH2Constants::SENSOR_STABILITY_DETECTOR: return 6; //ID 0x1c
|
||||
case SH2Constants::SENSOR_PERSONAL_ACTIVITY_CLASSIFIER: return 16; //ID 0x1e
|
||||
case SH2Constants::SENSOR_SLEEP_DETECTOR: return 6; //ID 0x1f
|
||||
case SH2Constants::SENSOR_TILT_DETECTOR: return 6; //ID 0x20
|
||||
case SH2Constants::SENSOR_POCKET_DETECTOR: return 6; //ID 0x21
|
||||
case SH2Constants::SENSOR_CIRCLE_DETECTOR: return 6; //ID 0x22
|
||||
case SH2Constants::SENSOR_HEART_RATE_MONITOR: return 6; //ID 0x23
|
||||
case SH2Constants::SENSOR_ARVR_STABILIZED_ROTATION_VECTOR: return 14; //ID 0x28
|
||||
case SH2Constants::SENSOR_ARVR_STABILIZED_GAME_ROTATION_VECTOR: return 12; //ID 0x29
|
||||
case SH2Constants::SENSOR_GYRO_INTEGRATED_ROTATION_VECTOR: return 14; //ID 0x2a
|
||||
case SH2Constants::SENSOR_MOTION_REQUEST: return 6; //ID 0x2b
|
||||
default: return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// The Q point for fixed-point values, i.e. the base 2 exponent for division
|
||||
// for consistency reasons also 0 for N/A / undefined
|
||||
inline int sh2GetSensorQPoint(unsigned int sensorReportID) {
|
||||
switch(sensorReportID) {
|
||||
case SH2Constants::SENSOR_ACCELEROMETER: return 8; //ID 0x01
|
||||
case SH2Constants::SENSOR_GYROSCOPE: return 9; //ID 0x02
|
||||
case SH2Constants::SENSOR_MAGNETOMETER: return 4; //ID 0x03
|
||||
case SH2Constants::SENSOR_LINEAR_ACCELERATION: return 8; //ID 0x04
|
||||
case SH2Constants::SENSOR_ROTATION_VECTOR: return 14; //ID 0x05 // but 12 for accuracy fields
|
||||
case SH2Constants::SENSOR_GRAVITY: return 8; //ID 0x06
|
||||
case SH2Constants::SENSOR_GYROSCOPE_UNCALIBRATED: return 9; //ID 0x07
|
||||
case SH2Constants::SENSOR_GAME_ROTATION_VECTOR: return 14; //ID 0x08
|
||||
case SH2Constants::SENSOR_GEOMAGNETIC_ROTATION: return 14; //ID 0x09
|
||||
case SH2Constants::SENSOR_PRESSURE: return 20; //ID 0x0a
|
||||
case SH2Constants::SENSOR_AMBIENT_LIGHT: return 8; //ID 0x0b
|
||||
case SH2Constants::SENSOR_HUMIDITY: return 8; //ID 0x0c
|
||||
case SH2Constants::SENSOR_PROXIMITY: return 4; //ID 0x0d
|
||||
case SH2Constants::SENSOR_TEMPERATURE: return 7; //ID 0x0e
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_UNCALIBRATED: return 4; //ID 0x0f
|
||||
case SH2Constants::SENSOR_TAP_DETECTOR: return 0; //ID 0x10
|
||||
case SH2Constants::SENSOR_STEP_COUNTER: return 0; //ID 0x11
|
||||
case SH2Constants::SENSOR_SIGNIFICANT_MOTION: return 0; //ID 0x12
|
||||
case SH2Constants::SENSOR_STABILITY_CLASSIFIER: return 0; //ID 0x13
|
||||
case SH2Constants::SENSOR_ACCELEROMETER_RAW: return 0; //ID 0x14
|
||||
case SH2Constants::SENSOR_GYROSCOPE_RAW: return 0; //ID 0x15
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_RAW: return 0; //ID 0x16
|
||||
case SH2Constants::SENSOR_STEP_DETECTOR: return 0; //ID 0x18
|
||||
case SH2Constants::SENSOR_SHAKE_DETECTOR: return 0; //ID 0x19
|
||||
case SH2Constants::SENSOR_FLIP_DETECTOR: return 0; //ID 0x1a
|
||||
case SH2Constants::SENSOR_PICKUP_DETECTOR: return 0; //ID 0x1b
|
||||
case SH2Constants::SENSOR_STABILITY_DETECTOR: return 0; //ID 0x1c
|
||||
case SH2Constants::SENSOR_PERSONAL_ACTIVITY_CLASSIFIER: return 0; //ID 0x1e
|
||||
case SH2Constants::SENSOR_SLEEP_DETECTOR: return 0; //ID 0x1f
|
||||
case SH2Constants::SENSOR_TILT_DETECTOR: return 0; //ID 0x20
|
||||
case SH2Constants::SENSOR_POCKET_DETECTOR: return 0; //ID 0x21
|
||||
case SH2Constants::SENSOR_CIRCLE_DETECTOR: return 0; //ID 0x22
|
||||
case SH2Constants::SENSOR_HEART_RATE_MONITOR: return 0; //ID 0x23
|
||||
case SH2Constants::SENSOR_ARVR_STABILIZED_ROTATION_VECTOR: return 14; //ID 0x28
|
||||
case SH2Constants::SENSOR_ARVR_STABILIZED_GAME_ROTATION_VECTOR: return 14; //ID 0x29
|
||||
case SH2Constants::SENSOR_GYRO_INTEGRATED_ROTATION_VECTOR: return 14; //ID 0x2a // but 10 for angular velocity
|
||||
case SH2Constants::SENSOR_MOTION_REQUEST: return 0; //ID 0x2b
|
||||
default: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
inline const char* sh2GetSensorName(unsigned int sensorReportID) {
|
||||
switch(sensorReportID) {
|
||||
case SH2Constants::SENSOR_ACCELEROMETER: return "Accelerometer";
|
||||
case SH2Constants::SENSOR_GYROSCOPE: return "Gyroscope";
|
||||
case SH2Constants::SENSOR_MAGNETOMETER: return "Magnetometer";
|
||||
case SH2Constants::SENSOR_LINEAR_ACCELERATION: return "Linear Acceleration";
|
||||
case SH2Constants::SENSOR_ROTATION_VECTOR: return "Rotation Vector";
|
||||
case SH2Constants::SENSOR_GRAVITY: return "Gravity";
|
||||
case SH2Constants::SENSOR_GYROSCOPE_UNCALIBRATED: return "Gyroscope Uncalibrated";
|
||||
case SH2Constants::SENSOR_GAME_ROTATION_VECTOR: return "Game Rotation Vector";
|
||||
case SH2Constants::SENSOR_GEOMAGNETIC_ROTATION: return "Geomagnetic Rotation";
|
||||
case SH2Constants::SENSOR_PRESSURE: return "Pressure";
|
||||
case SH2Constants::SENSOR_AMBIENT_LIGHT: return "Ambient Light";
|
||||
case SH2Constants::SENSOR_HUMIDITY: return "Humidity";
|
||||
case SH2Constants::SENSOR_PROXIMITY: return "Proximity";
|
||||
case SH2Constants::SENSOR_TEMPERATURE: return "Temperature";
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_UNCALIBRATED: return "Magnetometer Uncalibrated";
|
||||
case SH2Constants::SENSOR_TAP_DETECTOR: return "Tap Detector";
|
||||
case SH2Constants::SENSOR_STEP_COUNTER: return "Step Counter";
|
||||
case SH2Constants::SENSOR_SIGNIFICANT_MOTION: return "Significant Motion";
|
||||
case SH2Constants::SENSOR_STABILITY_CLASSIFIER: return "Stability Classifier";
|
||||
case SH2Constants::SENSOR_ACCELEROMETER_RAW: return "Accelerometer Raw";
|
||||
case SH2Constants::SENSOR_GYROSCOPE_RAW: return "Gyroscope Raw";
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_RAW: return "Magnetometer Raw";
|
||||
case SH2Constants::SENSOR_STEP_DETECTOR: return "Step Detector";
|
||||
case SH2Constants::SENSOR_SHAKE_DETECTOR: return "Shake Detector";
|
||||
case SH2Constants::SENSOR_FLIP_DETECTOR: return "Flip Detector";
|
||||
case SH2Constants::SENSOR_PICKUP_DETECTOR: return "Pickup Detector";
|
||||
case SH2Constants::SENSOR_STABILITY_DETECTOR: return "Stability Detector";
|
||||
case SH2Constants::SENSOR_PERSONAL_ACTIVITY_CLASSIFIER: return "Personal Activity Classifier";
|
||||
case SH2Constants::SENSOR_SLEEP_DETECTOR: return "Sleep Detector";
|
||||
case SH2Constants::SENSOR_TILT_DETECTOR: return "Tilt Detector";
|
||||
case SH2Constants::SENSOR_POCKET_DETECTOR: return "Pocket Detector";
|
||||
case SH2Constants::SENSOR_CIRCLE_DETECTOR: return "Circle Detector";
|
||||
case SH2Constants::SENSOR_HEART_RATE_MONITOR: return "Heart Rate Monitor";
|
||||
case SH2Constants::SENSOR_ARVR_STABILIZED_ROTATION_VECTOR: return "ARVR-Stabilized Rotation Vector";
|
||||
case SH2Constants::SENSOR_ARVR_STABILIZED_GAME_ROTATION_VECTOR: return "ARVR-Stabilized Game Rotation Vector";
|
||||
case SH2Constants::SENSOR_GYRO_INTEGRATED_ROTATION_VECTOR: return "Gyro-Integrated Rotation Vector";
|
||||
case SH2Constants::SENSOR_MOTION_REQUEST: return "Motion Request";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
// Convenience function to return the appropriate unit string, if applicable
|
||||
inline const char* sh2GetSensorUnit(unsigned int sensorReportID) {
|
||||
switch(sensorReportID) {
|
||||
case SH2Constants::SENSOR_ACCELEROMETER: //ID 0x01
|
||||
case SH2Constants::SENSOR_LINEAR_ACCELERATION: //ID 0x04
|
||||
case SH2Constants::SENSOR_GRAVITY: return "m/s²"; //ID 0x06
|
||||
case SH2Constants::SENSOR_GYROSCOPE: //ID 0x02
|
||||
case SH2Constants::SENSOR_GYROSCOPE_UNCALIBRATED: return "rad/s"; //ID 0x07
|
||||
case SH2Constants::SENSOR_MAGNETOMETER: //ID 0x03
|
||||
case SH2Constants::SENSOR_MAGNETOMETER_UNCALIBRATED: return "μT"; //ID 0x0f
|
||||
case SH2Constants::SENSOR_PRESSURE: return "hPa"; //ID 0x0a
|
||||
case SH2Constants::SENSOR_AMBIENT_LIGHT: return "lx"; //ID 0x0b "cd/m²"
|
||||
case SH2Constants::SENSOR_HUMIDITY: return "%"; //ID 0x0c
|
||||
case SH2Constants::SENSOR_PROXIMITY: return "cm"; //ID 0x0d
|
||||
case SH2Constants::SENSOR_TEMPERATURE: return "°C"; //ID 0x0e
|
||||
default: return "";
|
||||
}
|
||||
}
|
||||
|
||||
inline const char* sh2GetCommandName(unsigned int cmdID) {
|
||||
static const char* cmdNames[] = {"Reserved", "Errors", "Counter", "Tare", "Initialize",
|
||||
"Reserved", "Save DCD", "ME Calibration", "Reserved", "Periodic DCD Save", "Get Oscillator Type",
|
||||
"Clear DCD and Reset", "Calibration", "Bootloader", "Interactive Calibration"};
|
||||
if (cmdID < sizeof(cmdNames)) return cmdNames[cmdID];
|
||||
else return "Unknown";
|
||||
}
|
||||
|
||||
#pragma pack(push,1) // Packed struct definitions from SH-2, co-opted for transfer
|
||||
|
||||
// Common prefix for all SH-2 cargos. SHTP headers irrelevant and not represented.
|
||||
class SH2CargoBase {
|
||||
private:
|
||||
uint8_t cargoLength[2];
|
||||
uint8_t channel;
|
||||
uint8_t sequenceNumber;
|
||||
uint8_t reportType;
|
||||
public:
|
||||
inline uint16_t getCargoLength() const { return sh2GetU16(cargoLength) & 0x7fff; } // mask out subtransfer bit
|
||||
inline uint8_t getChannel() const { return channel; }
|
||||
inline uint8_t getSequenceNumber() const { return sequenceNumber; }
|
||||
inline uint8_t getReportType() const { return reportType; }
|
||||
};
|
||||
|
||||
// Our own custom extension for sending the raw interrupt timestamp (report 0xFF, never reported by SH2)
|
||||
class SH2CargoBodyScenescanTimestamp {
|
||||
private:
|
||||
SH2CargoBase base;
|
||||
uint8_t usecSinceEpoch[8]; // 64-bit microsecond count
|
||||
public:
|
||||
inline uint64_t getUSecSinceEpoch() const { return (uint64_t) sh2GetU64(usecSinceEpoch); }
|
||||
};
|
||||
|
||||
// A Timestamp Rebase (0xFA), reporting additional sensor delay offset since last Timebase
|
||||
class SH2CargoBodyTimestampRebase {
|
||||
private:
|
||||
SH2CargoBase base;
|
||||
uint8_t rebaseTime[4];
|
||||
public:
|
||||
inline long getRebaseTime() const { return (int32_t) sh2GetU32(rebaseTime); }
|
||||
};
|
||||
|
||||
// A Time Base report with a batch transfer (0xFB)
|
||||
// It may be followed by any amount of sensor reports, below.
|
||||
// Refer to the base.getCargoLength() value and the known
|
||||
// record sizes for parsing them.
|
||||
class SH2CargoBodyTimeBase {
|
||||
private:
|
||||
SH2CargoBase base;
|
||||
uint8_t timeBase_100uSec[4];
|
||||
public:
|
||||
inline long getTimeBase() const { return 100l * sh2GetU32(timeBase_100uSec); }
|
||||
};
|
||||
|
||||
// Common base prefix for all sensor reports
|
||||
class SH2SensorReportBase {
|
||||
private:
|
||||
uint8_t sensorID;
|
||||
uint8_t sequenceNumber;
|
||||
uint8_t statusAndDelayMSB;
|
||||
uint8_t delayLSB;
|
||||
public:
|
||||
inline unsigned int getStatus() const { return statusAndDelayMSB & 0x03; }
|
||||
inline unsigned int getDelay() const { return ((statusAndDelayMSB & 0xfc) << 6) | delayLSB; }
|
||||
};
|
||||
|
||||
// 10-byte reports with individual Q scaling for non-raw 3D sensors
|
||||
class SH2SensorReportAccelerometer {
|
||||
private:
|
||||
SH2SensorReportBase base;
|
||||
uint8_t xAxis[2];
|
||||
uint8_t yAxis[2];
|
||||
uint8_t zAxis[2];
|
||||
public:
|
||||
inline double getX() const { return sh2ConvertFixedQ16(sh2GetU16(xAxis), 8); } // Accel Q: shift 8 bits
|
||||
inline double getY() const { return sh2ConvertFixedQ16(sh2GetU16(yAxis), 8); }
|
||||
inline double getZ() const { return sh2ConvertFixedQ16(sh2GetU16(zAxis), 8); }
|
||||
};
|
||||
class SH2SensorReportMagnetometer {
|
||||
private:
|
||||
SH2SensorReportBase base;
|
||||
uint8_t xAxis[2];
|
||||
uint8_t yAxis[2];
|
||||
uint8_t zAxis[2];
|
||||
public:
|
||||
inline double getX() const { return sh2ConvertFixedQ16(sh2GetU16(xAxis), 4); } // Magn Q: shift 4 bits
|
||||
inline double getY() const { return sh2ConvertFixedQ16(sh2GetU16(yAxis), 4); }
|
||||
inline double getZ() const { return sh2ConvertFixedQ16(sh2GetU16(zAxis), 4); }
|
||||
};
|
||||
class SH2SensorReportGyroscope {
|
||||
private:
|
||||
SH2SensorReportBase base;
|
||||
uint8_t xAxis[2];
|
||||
uint8_t yAxis[2];
|
||||
uint8_t zAxis[2];
|
||||
public:
|
||||
inline double getX() const { return sh2ConvertFixedQ16(sh2GetU16(xAxis), 9); } // Gyro Q: shift 9 bits
|
||||
inline double getY() const { return sh2ConvertFixedQ16(sh2GetU16(yAxis), 9); }
|
||||
inline double getZ() const { return sh2ConvertFixedQ16(sh2GetU16(zAxis), 9); }
|
||||
};
|
||||
|
||||
// 14-byte orientation (quaternion) data. i,j,k,real are also known as x,y,z,w
|
||||
class SH2SensorReportOrientation {
|
||||
private:
|
||||
SH2SensorReportBase base;
|
||||
uint8_t quatI[2];
|
||||
uint8_t quatJ[2];
|
||||
uint8_t quatK[2];
|
||||
uint8_t quatReal[2];
|
||||
uint8_t accuracy[2];
|
||||
public:
|
||||
inline double getI() const { return sh2ConvertFixedQ16(sh2GetU16(quatI), 14); } // Quaternion data: shift 14 bits
|
||||
inline double getJ() const { return sh2ConvertFixedQ16(sh2GetU16(quatJ), 14); }
|
||||
inline double getK() const { return sh2ConvertFixedQ16(sh2GetU16(quatK), 14); }
|
||||
inline double getReal() const { return sh2ConvertFixedQ16(sh2GetU16(quatReal), 14); }
|
||||
inline double getAccuracy() const { return sh2ConvertFixedQ16(sh2GetU16(accuracy), 12); } // Accuracy: shift 12
|
||||
};
|
||||
|
||||
// 6-byte 1D sensor (pressure, ambient light, humidity, proximity, temperature,
|
||||
// 16-byte data for *raw* accelerometer, gyro, magnetometer
|
||||
class SH2SensorReportRawAGM {
|
||||
private:
|
||||
SH2SensorReportBase base;
|
||||
uint8_t xAxisRaw[2];
|
||||
uint8_t yAxisRaw[2];
|
||||
uint8_t zAxisRaw[2];
|
||||
uint8_t temperature_forGyro[2];
|
||||
uint8_t timestamp[4];
|
||||
};
|
||||
|
||||
#pragma pack(pop) // End of common sensor data / transport packed struct definitions
|
||||
|
||||
}} // namespaces
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue