STM32 I2C Library for BNO08x 9 axis IMU
Please read Liability Disclaimer and License Agreement CAREFULLY
Before going further please read BNO086 datasheet
In BNO_08x_I2C.h add the following code
#ifndef __BNO_08x_I2C_H
#define __BNO_08x_I2C_H
#ifdef __cplusplus
extern "C" {
#endif
#include "i2c.h"
// Used to eneble or disable functions needed for rough telescope position
// Uncomment lne below for fully featured driver
#define USE_FOR_TELESCOPE
// Enable disable extra features
//#define USE_ERROR_REPORT
//#define USE_COUNTER_REPORT
//#define USE_REORIENT //Tare
// For telescope use we need to be able to calibrate and change the orientation
#ifdef USE_FOR_TELESCOPE
#define ACCELEROMETER 0X01
#define GYROSCOPE_CALIBRATED 0X02
#define MAGNETIC_FIELD_CALIBRATED 0X03
#define ROTATION_VECTOR 0X05
#define GAME_ROTATION_VECTOR 0X08
#define USE_REORIENT // Used to set X to North, Y to East and Z down
#else
//Features that can be used. Play with the conditions above to activate them
#define RAW_ACCELEROMETER 0X14
#define ACCELEROMETER 0X01
#define LINEAR_ACCELERATION 0X04
#define GRAVITY 0X06
#define RAW_GYROSCOPE 0X15
#define GYROSCOPE_CALIBRATED 0X02
#define GYROSCOPE_UNCALIBRATED 0X07
#define RAW_MAGNETOMETER 0X16
#define MAGNETIC_FIELD_CALIBRATED 0X03
#define MAGNETIC_FIELD_UNCALIBRATED 0X0F
#define ROTATION_VECTOR 0X05
#define GAME_ROTATION_VECTOR 0X08
#define GEOMAGNETIC_ROTATION_VECTOR 0X09
#define PRESSURE 0X0A
#define AMBIENT_LIGHT 0X0B
#define HUMIDITY 0X0C
#define PROXIMITY 0X0D
#define TEMPERATURE 0X0E
#define RESERVED 0X17
#define TAP_DETECTOR 0X10
#define STEP_DETECTOR 0X18
#define STEP_COUNTER 0X11
#define SIGNIFICANT_MOTION 0X12
#define STABILITY_CLASSIFIER 0X13
#define SHAKE_DETECTOR 0X19
#define FLIP_DETECTOR 0X1A
#define PICKUP_DETECTOR 0X1B
#define STABILITY_DETECTOR 0X1C
#define PERSONAL_ACTIVITY_CLASSIFIER 0X1E
#define SLEEP_DETECTOR 0X1F
#define TILT_DETECTOR 0X20
#define POCKET_DETECTOR 0X21
#define CIRCLE_DETECTOR 0X22
#define HEART_RATE_MONITOR 0X23
#define ARVR_STABILIZED_RV 0X28
#define ARVR_STABILIZED_GRV 0X29
#define GYRO_INTEGRATED_RV 0X2A
#define IZRO_MOTION_REQUEST 0X2B
#define RAW_OPTICAL_FLOW 0X2C
#define DEAD_RECKONING_POSE 0X2D
#define WHEEL_ENCODER 0X2E
#endif
// Address
#define BNO_W_ADDR 0x96
#define BNO_R_ADDR 0x97
#define ADDR_BNO_DFU_W_ADDR 0x52
#define ADDR_BNO_DFU_R_ADDR 0x53
// Delays
#define RESET_DELAY 200
#define PORT_TIMEOUT 500
#define Q_FLIP 0.70710678118654752440084436210485
#define _180_DIV_PI 57.295779515f // = 180 / PI
// Array sizes
#define SEQUENCE_SIZE 6
#define HEADER_SIZE 4
#define RX_PACKET_SIZE 284 // For BNO086 276 for BNO080
#define TX_PACKET_SIZE 21
// Channels
#define CHANNEL_COMMAND 0
#define CHANNEL_EXECUTABLE 1
#define CHANNEL_CONTROL 2
#define CHANNEL_REPORTS 3
#define CHANNEL_WAKE_REPORTS 4
#define CHANNEL_GYRO 5
// Reports
#define REPORT_UNSOLICITED_RESPONSE 0x00
#define REPORT_UNSOLICITED_RESPONSE1 0x01
#define REPORT_SENSOR_FLUSH_REQUEST 0xF0
#define REPORT_SENSOR_FLUSH_RESPONSE 0xEF
#define REPORT_COMMAND_REQUEST 0xF2
#define REPORT_COMMAND_RESPONSE 0xF1
#define REPORT_FRS_READ_REQUEST 0xF4
#define REPORT_FRS_READ_RESPONSE 0xF3
#define REPORT_FRS_WRITE_REQUEST 0xF7
#define REPORT_FRS_WRITE_DATA_REQUEST 0xF6
#define REPORT_FRS_WRITE_RESPONSE 0xF5
#define REPORT_PRODUCT_ID_REQUEST 0xF9
#define REPORT_PRODUCT_ID_RESPONSE 0xF8
#define REPORT_TIMESTAMP_REBASE 0xFA
#define REPORT_BASE_TIMESTAMP_REF 0xFB
#define REPORT_GET_FEATURE_REQUEST 0xFE
#define REPORT_GET_FEATURE_RESPONSE 0xFC
#define REPORT_SET_FEATURE_COMMAND 0xFD
// Command and Subcommand values
#define COMMAND_ERRORS 1
#define COMMAND_COUNTER 2
#define COMMAND_TARE 3
#define COMMAND_TARE_NOW 0
#define COMMAND_TARE_PERSIST 1
#define COMMAND_TARE_REORIENT 2
#define COMMAND_INITIALIZE 4
#define COMMAND_INITIALIZE_RESET 1
#define COMMAND_INITIALIZE_ON 2
#define COMMAND_INITIALIZE_SLEEP 3
#define COMMAND_SAVE_DCD 6
#define COMMAND_ME_CALIBRATE 7
#define COMMAND_ME_CALIBRATE_CONFIG 0
#define COMMAND_ME_CALIBRATE_GET 1
#define COMMAND_DCD_PERIOD_SAVE 9
#define COMMAND_OSCILLATOR 0x0A
#define COMMAND_CLEAR_DCD_RESET 0x0B
#define COMMAND_TURNTABLE_CAL 0x0C
#define COMMAND_BOOTLOADER 0x0D
#define COMMAND_BOOTLOADER_MODE_REQ 0x00
#define COMMAND_BOOTLOADER_STATUS_REQ 0x01
#define COMMAND_INTERACTIVE_CAL_REQ 0x0E
#define COMMAND_WHEEL_REQ 0x0F
#define COMMAND_UNSOLICITED_INITIALIZE 0x84
#define CALIBRATE_ACCEL 0
#define CALIBRATE_GYRO 1
#define CALIBRATE_MAG 2
#define CALIBRATE_PLANAR_ACCEL 3
#define CALIBRATE_ON_TABLE 4
#define CALIBRATE_ACCEL_GYRO_MAG 5
#define CALIBRATE_ALL 6
#define CALIBRATE_STOP 5
// FRS Record Ids
#define STATIC_CALIBRATION_AGM 0x7979
#define NOMINAL_CALIBRATION 0x4D4D
#define STATIC_CALIBRATION_SRA 0x8A8A
#define NOMINAL_CALIBRATION_SRA 0x4E4E
#define DYNAMIC_CALIBRATION 0x1F1F
#define ME_POWER_MGMT 0xD3E2
#define SYSTEM_ORIENTATION 0x2D3E
#define ACCEL_ORIENTATION 0x2D41
#define SCREEN_ACCEL_ORIENTATION 0x2D43
#define GYROSCOPE_ORIENTATION 0x2D46
#define MAGNETOMETER_ORIENTATION 0x2D4C
#define ARVR_STABILIZATION_RV 0x3E2D
#define ARVR_STABILIZATION_GRV 0x3E2E
#define TAP_DETECT_CONFIG 0xC269
#define SIG_MOTION_DETECT_CONFIG 0xC274
#define SHAKE_DETECT_CONFIG 0x7D7D
#define MAX_FUSION_PERIOD 0xD7D7
#define SERIAL_NUMBER 0x4B4B
#define ES_PRESSURE_CAL 0x39AF
#define ES_TEMPERATURE_CAL 0x4D20
#define ES_HUMIDITY_CAL 0x1AC9
#define ES_AMBIENT_LIGHT_CAL 0x39B1
#define ES_PROXIMITY_CAL 0x4DA2
#define ALS_CAL 0xD401
#define PROXIMITY_SENSOR_CAL 0xD402
#define PICKUP_DETECTOR_CONFIG 0x1B2A
#define FLIP_DETECTOR_CONFIG 0xFC94
#define STABILITY_DETECTOR_CONFIG 0xED85
#define ACTIVITY_TRACKER_CONFIG 0xED88
#define SLEEP_DETECTOR_CONFIG 0xED87
#define TILT_DETECTOR_CONFIG 0xED89
#define POCKET_DETECTOR_CONFIG 0xEF27
#define CIRCLE_DETECTOR_CONFIG 0xEE51
#define USER_RECORD 0x74B4
#define ME_TIME_SOURCE_SELECT 0xD403
#define UART_FORMAT 0xA1A1
#define GYRO_INTEGRATED_RV_CONFIG 0xA1A2
#define DR_IMU_CONFIG 0xDED2
#define DR_VEL_EST_CONFIG 0xDED3
#define DR_SYNC_CONFIG 0xDED4
#define DR_QUAL_CONFIG 0xDED5
#define DR_CAL_CONFIG 0xDED6
#define DR_LIGHT_REC_CONFIG 0xDED8
#define DR_FUSION_CONFIG 0xDED9
#define DR_OF_CONFIG 0xDEDA
#define DR_WHEEL_CONFIG 0xDEDB
#define DR_CAL 0xDEDC
#define DR_WHEEL_SELECT 0xDEDF
#define FRS_ID_META_RAW_ACCELEROMETER 0xE301
#define FRS_ID_META_ACCELEROMETER 0xE302
#define FRS_ID_META_LINEAR_ACCELERATION 0xE303
#define FRS_ID_META_GRAVITY 0xE304
#define FRS_ID_META_RAW_GYROSCOPE 0xE305
#define FRS_ID_META_GYROSCOPE_CALIBRATED 0xE306
#define FRS_ID_META_GYROSCOPE_UNCALIBRATED 0xE307
#define FRS_ID_META_RAW_MAGNETOMETER 0xE308
#define FRS_ID_META_MAGNETIC_FIELD_CALIBRATED 0xE309
#define FRS_ID_META_MAGNETIC_FIELD_UNCALIBRATED 0xE30A
#define FRS_ID_META_ROTATION_VECTOR 0xE30B
#define FRS_ID_META_GAME_ROTATION_VECTOR 0xE30C
#define FRS_ID_META_GEOMAGNETIC_ROTATION_VECTOR 0xE30D
#define FRS_ID_META_PRESSURE 0xE30E
#define FRS_ID_META_AMBIENT_LIGHT 0xE30F
#define FRS_ID_META_HUMIDITY 0xE310
#define FRS_ID_META_PROXIMITY 0xE311
#define FRS_ID_META_TEMPERATURE 0xE312
#define FRS_ID_META_TAP_DETECTOR 0xE313
#define FRS_ID_META_STEP_DETECTOR 0xE314
#define FRS_ID_META_STEP_COUNTER 0xE315
#define FRS_ID_META_SIGNIFICANT_MOTION 0xE316
#define FRS_ID_META_STABILITY_CLASSIFIER 0xE317
#define FRS_ID_META_SHAKE_DETECTOR 0xE318
#define FRS_ID_META_FLIP_DETECTOR 0xE319
#define FRS_ID_META_PICKUP_DETECTOR 0xE31A
#define FRS_ID_META_STABILITY_DETECTOR 0xE31B
#define FRS_ID_META_PERSONAL_ACTIVITY_CLASSIFIER 0xE31C
#define FRS_ID_META_SLEEP_DETECTOR 0xE31D
#define FRS_ID_META_TILT_DETECTOR 0xE31E
#define FRS_ID_META_POCKET_DETECTOR 0xE31F
#define FRS_ID_META_CIRCLE_DETECTOR 0xE320
#define FRS_ID_META_HEART_RATE_MONITOR 0xE321
#define FRS_ID_META_ARVR_STABILIZED_RV 0xE322
#define FRS_ID_META_ARVR_STABILIZED_GRV 0xE323
#define FRS_ID_META_GYRO_INTEGRATED_RV 0xE324
#define FRS_ID_META_RAW_OPTICAL_FLOW 0xE326
//Record IDs from SH-2 figure 28
//These are used to read and set various configuration options
#define FRS_RECORDID_SERIAL_NUMBER 0x4B4B
#define FRS_RECORDID_SYSTEM_ORIENTATION 0x2D3E
// FRS write status values
#define FRS_WRITE_STATUS_RECEIVED 0
#define FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE 1
#define FRS_WRITE_STATUS_BUSY 2
#define FRS_WRITE_STATUS_WRITE_COMPLETED 3
#define FRS_WRITE_STATUS_READY 4
#define FRS_WRITE_STATUS_FAILED 5
#define FRS_WRITE_STATUS_NOT_READY 6 // data received when not in write mode
#define FRS_WRITE_STATUS_INVALID_LENGTH 7
#define FRS_WRITE_STATUS_RECORD_VALID 8
#define FRS_WRITE_STATUS_INVALID_RECORD 9
#define FRS_WRITE_STATUS_DEVICE_ERROR 10
#define FRS_WRITE_STATUS_READ_ONLY 11
// Status values
#define FRS_READ_STATUS_NO_ERROR 0
#define FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE 1
#define FRS_READ_STATUS_BUSY 2
#define FRS_READ_STATUS_READ_RECORD_COMPLETED 3
#define FRS_READ_STATUS_OFFSET_OUT_OF_RANGE 4
#define FRS_READ_STATUS_RECORD_EMPTY 5
#define FRS_READ_STATUS_READ_BLOCK_COMPLETED 6
#define FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED 7
#define FRS_READ_STATUS_DEVICE_ERROR 8
// The below values are pre-calculated using SCALE_Q(n) = (1.0f / (1 << n))
#define SCALE_Q4 0.06250000000000000000
#define SCALE_Q7 0.00781250000000000000
#define SCALE_Q8 0.00390625000000000000
#define SCALE_Q9 0.00195312500000000000
#define SCALE_Q10 0.00097656250000000000
#define SCALE_Q12 0.00024414062500000000
#define SCALE_Q14 0.00006103515625000000
#define SCALE_Q17 0.00000762939453125000
#define SCALE_Q20 0.00000095367431640625
#define SCALE_Q25 0.00000002980232238770
#define SCALE_Q30 0.00000000093132257462
#define SCALE_TO_Q14 16384.0
// General Enums
typedef enum {
NA = 0,
PowerOnReset,
InternalSystemReset,
WatchdogTimeout,
ExternalReset,
Other,
} BNO_ResetCause_t;
typedef enum {
InternalOscillator = 0,
ExternalCrystal,
ExternalClock,
OscillatorError
} BNO_Oscillator_t;
typedef enum {
ResetToBootloader = 0, // Reset to bootloader Mode
UpgradeApp, //1 – Upgrade Application Mode; upgrade the application image in flash.
ValidateImage, //2 – Validate Image Mode; validate an application image without updating the flash
LauchApp, //3 – Launch Application; launch the application image in flash
BootInvalid
} BNO_BootMode_t;
typedef enum {
MOTION_INTENT_UNKNOWN = 0, // FME_MOBILE_MOTION_INTENT_UNKNOWN – this is the initial state assumed by the sensor hub
INTENT_STATIONARY_WITHOUT_VIBRATION, //
MOTION_INTENT_STATIONARY_WITH_VIBRATION, //
MOTION_INTENT_IN_MOTION, //
MOTION_INTENT_IN_MOTION_ACCELERATING
} BNO_MotionIntent_t;
// General structures
typedef struct __attribute__((packed)) {
BNO_ResetCause_t rst_Cause; // report 0xF8 byte 1 Reset Cause
uint8_t sw_Maj; // report 0xF8 byte 2 SW Version Major
uint8_t sw_Min; // report 0xF8 byte 3 SW Version Minor
uint32_t sw_PN; // report 0xF8 byte 4-7 SW Part Number
uint32_t sw_BN; // report 0xF8 byte 8-11 SW Build Number
uint16_t sw_VP; // report 0xF8 byte 12-13 SW Version Patch
} BNO_productID_t;
typedef struct __attribute__((packed)){
uint8_t status;
uint16_t wordOffset;
} BNO_FrsWriteResp_t;
typedef struct __attribute__((packed)){
uint8_t len_status;
uint16_t wordOffset;
uint32_t data0;
uint32_t data1;
uint16_t frsType;
} BNO_FrsReadResp_t;
#define RESPONSE_VALUES 11
typedef struct __attribute__((packed)) {
uint8_t seq;
uint8_t command;
uint8_t commandSeq;
uint8_t respSeq;
uint8_t r[RESPONSE_VALUES];
} BNO_CommandResp_t;
#define COMMAND_PARAMS 9
typedef struct __attribute__((packed)) {
uint8_t reportId;
uint8_t seq;
uint8_t command;
uint8_t p[COMMAND_PARAMS];
} BNO_CommandReq_t;
#ifdef USE_ERROR_REPORT
typedef struct __attribute__((packed)) {
uint8_t Severity; // Severity – the severity of the error currently being reported
uint8_t SeqNo; // Error sequence number. A monotonically incrementing uin8_t that counts all the errors generated for the reported severity. It may rollover.
uint8_t Source; // Error source. 0 – reserved, 1 – MotionEngine, 2 – MotionHub, 3 – SensorHub, 4 – Chip level executable, 5-254 reserved. 255 – no error to report.
uint8_t Error; // Error. See library API
uint8_t Module; // Error module. See library API
uint8_t Code; // Error code. See library API
} BNO_Error_t;
#endif
#ifdef USE_COUNTER_REPORT
typedef struct __attribute__((packed)) {
uint32_t offered; // [events]
uint32_t accepted; // [events]
uint32_t on; // [events]
uint32_t attempted; // [events]
} BNO_Counts_t;
#endif
#ifdef USE_REORIENT
typedef enum {
TARE_Z = 4,
TARE_ALL = 7,
} BNO_TareAxis_t;
typedef enum {
RotationVector = 0,
GamingRotationVector = 1,
GeomagneticRotationVector = 2,
GyroIntegratedRotationVector = 3,
ArVrStabilizedRotationVector = 4,
ArVrStabilizedGameRotationVector =5,
} BNO_TareRV_t;
#endif
typedef struct __attribute__((packed)) {
uint8_t Status; // Status (0 – success. Non Zero – Operation failed)
uint8_t AccCalEnable; // Accel Cal Enable (1 – enabled, 0 – disabled)
uint8_t GyroCalEnable; // Gyro Cal Enable (1 – enabled, 0 – disabled)
uint8_t MagCalEnable; // Mag Cal Enable (1 – enabled, 0 – disabled)
uint8_t PlanCalEnable; // Planar Accel Cal Enable (1 – enabled, 0 – disabled)
uint8_t OnTableEnable; // On Table Cal Enable (1 – enabled, 0 – disabled)
} BNO_calibrationStat_t;
typedef struct __attribute__((packed)) {
uint8_t OperationMode;
uint8_t reserved;
uint32_t Status;
uint32_t Errors;
} BNO_Boot_t;
typedef struct __attribute__((packed)) {
uint8_t sensorID; // sensor id
uint8_t flags; // FEAT_... values
uint16_t changeSensitivity;
uint32_t reportInterval_uS;
uint32_t batchInterval_uS;
uint32_t sensorSpecific;
} BNO_Feature_t;
// Sensor values
typedef struct BNO_RollPitchYaw {
float Roll;
float Pitch;
float Yaw;
} BNO_RollPitchYaw_t;
#ifdef RAW_ACCELEROMETER
// Raw Accelerometer
typedef struct __attribute__((packed)) {
// Units are ADC counts
int16_t X; // [ADC counts]
int16_t Y; // [ADC counts]
int16_t Z; // [ADC counts]
uint32_t TimeStamp; // [uS]
} BNO_RawAccelerometer_t;
#endif
#if defined(ACCELEROMETER) || defined(LINEAR_ACCELERATION) || defined(GRAVITY)
// Accelerometer
typedef struct __attribute__((packed)) {
float X;
float Y;
float Z;
} BNO_Accelerometer_t;
#endif
#ifdef RAW_GYROSCOPE
// Raw Gyroscope
typedef struct __attribute__((packed)) {
// Units are ADC counts
int16_t X; // [ADC Counts]
int16_t Y; // [ADC Counts]
int16_t Z; // [ADC Counts]
int16_t Temperature; // [ADC Counts]
uint32_t TimeStamp; // [uS]
} BNO_RawGyroscope_t;
#endif
#ifdef GYROSCOPE_CALIBRATED
// Gyroscope
typedef struct __attribute__((packed)) {
// Units are rad/s
float X;
float Y;
float Z;
} BNO_Gyroscope_t;
#endif
#ifdef GYROSCOPE_UNCALIBRATED
// Uncalibrated Gyroscope
typedef struct __attribute__((packed)) {
// Units are rad/s
float X; // [rad/s]
float Y; // [rad/s]
float Z; // [rad/s]
float BiasX; // [rad/s]
float BiasY; // [rad/s]
float BiasZ; // [rad/s]
} BNO_GyroscopeUncalibrated_t;
#endif
#ifdef RAW_MAGNETOMETER
// Raw Magnetometer
typedef struct __attribute__((packed)) {
// Units are ADC counts
int16_t X; // [ADC Counts]
int16_t Y; // [ADC Counts]
int16_t Z; // [ADC Counts]
uint32_t TimeStamp; // [uS]
} BNO_RawMagnetometer_t;
#endif
#ifdef MAGNETIC_FIELD_CALIBRATED
// Magnetic Field
typedef struct __attribute__((packed)) {
// Units are uTesla
float X; // [uTesla]
float Y; // [uTesla]
float Z; // [uTesla]
} BNO_MagneticField_t;
#endif
#ifdef MAGNETIC_FIELD_UNCALIBRATED
// Uncalibrated Magnetic Field
typedef struct __attribute__((packed)) {
// Units are uTesla
float X; // [uTesla]
float Y; // [uTesla]
float Z; // [uTesla]
float BiasX; // [uTesla]
float BiasY; // [uTesla]
float BiasZ; // [uTesla]
} BNO_MagneticFieldUncalibrated_t;
#endif
#if defined(ROTATION_VECTOR) || defined(GEOMAGNETIC_ROTATION_VECTOR) || defined(ARVR_STABILIZED_RV)
// Rotation Vector with Accuracy
typedef struct __attribute__((packed)) {
float I; // Quaternion component i
float J; // Quaternion component j
float K; // Quaternion component k
float Real; // Quaternion component, real
float Accuracy; // Accuracy estimate [radians]
} BNO_RotationVectorWAcc_t;
#endif
#if defined(GAME_ROTATION_VECTOR) || defined(ARVR_STABILIZED_GRV)
// Rotation Vector
typedef struct __attribute__((packed)) {
float I; // Quaternion component i
float J; // Quaternion component j
float K; // Quaternion component k
float Real; // Quaternion component real
} BNO_RotationVector_t;
#endif
#ifdef TAP_DETECTOR
typedef enum {
TAPDET_X = 1,
TAPDET_X_POS = 2,
TAPDET_Y = 4,
TAPDET_Y_POS = 8,
TAPDET_Z = 16,
TAPDET_Z_POS = 32,
TAPDET_DOUBLE = 64,
} BNO_Tap_t;
#endif
#ifdef STEP_COUNTER
typedef struct __attribute__((packed)) {
uint32_t Latency; // Step counter latency [uS].
uint16_t Steps; // Steps counted.
} BNO_StepCounter_t;
#endif
#ifdef STABILITY_CLASSIFIER
typedef enum {
STABILITY_CLASSIFIER_UNKNOWN = 0,
STABILITY_CLASSIFIER_ON_TABLE,
STABILITY_CLASSIFIER_STATIONARY,
STABILITY_CLASSIFIER_STABLE,
STABILITY_CLASSIFIER_MOTION,
} BNO_Stability_t;
#endif
#ifdef SHAKE_DETECTOR
typedef enum {
SHAKE_X = 1,
SHAKE_Y = 2,
SHAKE_Z = 4,
} BNO_Shake_t;
#endif
#ifdef PICKUP_DETECTOR
typedef enum {
PICKUP_LEVEL_TO_NOT_LEVEL = 1,
PICKUP_STOP_WITHIN_REGION = 2,
} BNO_Pickup_t;
#endif
#ifdef STABILITY_DETECTOR
typedef enum {
STABILITY_ENTERED = 1,
STABILITY_EXITED = 2,
} BNO_StabilityDetector_t;
#endif
#ifdef PERSONAL_ACTIVITY_CLASSIFIER
typedef enum {
PAC_UNKNOWN = 0,
PAC_IN_VEHICLE,
PAC_ON_BICYCLE,
PAC_ON_FOOT,
PAC_STILL,
PAC_TILTING,
PAC_WALKING,
PAC_RUNNING,
} BNO_PAC_t;
// Personal Activity Classifier
typedef struct BNO_PersonalActivityClassifier {
uint8_t Page;
uint8_t LastPage;
BNO_PAC_t MostLikelyState;
uint8_t Confidence[10];
} BNO_PersonalActivityClassifier_t;
#endif
#ifdef GYRO_INTEGRATED_RV
// Gyro Integrated Rotation Vector
typedef struct BNO_GyroIntegratedRV {
float I; // Quaternion component i
float J; // Quaternion component j
float K; // Quaternion component k
float Real;// Quaternion component real
float AngleVelX; // Angular velocity about x [rad/s]
float AngleVelY; // Angular velocity about y [rad/s]
float AngleVelZ; // Angular velocity about z [rad/s]
} BNO_GyroIntegratedRV_t;
#endif
#ifdef IZRO_MOTION_REQUEST
// Interactive ZRO Motion Intent. See the SH-2 Reference Manual, 6.4.13
typedef enum {
IZRO_MI_UNKNOWN = 0,
IZRO_MI_STATIONARY_NO_VIBRATION,
IZRO_MI_STATIONARY_WITH_VIBRATION,
IZRO_MI_IN_MOTION,
IZRO_MI_ACCELERATING,
} BNO_IZroMotionIntent_t;
typedef enum {
IZRO_MR_NO_REQUEST = 0,
IZRO_MR_STAY_STATIONARY,
IZRO_MR_STATIONARY_NON_URGENT,
IZRO_MR_STATIONARY_URGENT,
} BNO_IZroMotionRequest_t;
typedef struct BNO_IZroRequest {
BNO_IZroMotionIntent_t Intent;
BNO_IZroMotionRequest_t Request;
} BNO_IZroRequest_t;
#endif
#ifdef RAW_OPTICAL_FLOW
typedef struct BNO_RawOptFlow {
uint32_t TimeStamp;
int16_t Dt;
int16_t Dx;
int16_t Dy;
int16_t Iq;
uint8_t ResX;
uint8_t ResY;
uint8_t Shutter;
uint8_t FrameMax;
uint8_t FrameAvg;
uint8_t FrameMin;
uint8_t LaserOn;
} BNO_RawOptFlow_t;
#endif
#ifdef DEAD_RECKONING_POSE
typedef struct BNO_DeadReckoningPose {
uint32_t TimeStamp;
float LinPosX;
float LinPosY;
float LinPosZ;
float I;
float J;
float K;
float Real;
float LinVelX;
float LinVelY;
float LinVelZ;
float AngleVelX;
float AngleVelY;
float AngleVelZ;
} BNO_DeadReckoningPose_t;
#endif
#ifdef WHEEL_ENCODER
typedef struct BNO_WheelEncoder {
uint32_t TimeStamp;
uint8_t WheelIndex;
uint8_t DataType;
uint16_t Data;
} BNO_WheelEncoder_t;
#endif
typedef struct BNO_SensorValue {
uint8_t sensorId; // Which sensor produced this event.
uint8_t sequence; // The sequence number increments once for each report sent. Gaps in the sequence numbers indicate missing or dropped reports.
uint8_t status; // bits 7-5: reserved, 4-2: exponent delay, 1-0: Accuracy 0 - Unreliable 1 - Accuracy low 2 - Accuracy medium 3 - Accuracy high
uint64_t timestamp; // [uS]
uint32_t delay; /// [uS] value is delay * 2^exponent (see status)
union {
#ifdef RAW_ACCELEROMETER
BNO_RawAccelerometer_t RawAccelerometer;
#endif
#ifdef ACCELEROMETER
BNO_Accelerometer_t Accelerometer;
#endif
#ifdef LINEAR_ACCELERATION
BNO_Accelerometer_t LinearAcceleration;
#endif
#ifdef GRAVITY
BNO_Accelerometer_t Gravity;
#endif
#ifdef RAW_GYROSCOPE
BNO_RawGyroscope_t RawGyroscope;
#endif
#ifdef GYROSCOPE_CALIBRATED
BNO_Gyroscope_t Gyroscope;
#endif
#ifdef GYROSCOPE_UNCALIBRATED
BNO_GyroscopeUncalibrated_t GyroscopeUncal;
#endif
#ifdef RAW_MAGNETOMETER
BNO_RawMagnetometer_t RawMagnetometer;
#endif
#ifdef MAGNETIC_FIELD_CALIBRATED
BNO_MagneticField_t MagneticField;
#endif
#ifdef MAGNETIC_FIELD_UNCALIBRATED
BNO_MagneticFieldUncalibrated_t MagneticFieldUncal;
#endif
#ifdef ROTATION_VECTOR
BNO_RotationVectorWAcc_t RotationVector;
#endif
#ifdef GAME_ROTATION_VECTOR
BNO_RotationVector_t GameRotationVector;
#endif
#ifdef GEOMAGNETIC_ROTATION_VECTOR
BNO_RotationVectorWAcc_t GeoMagRotationVector;
#endif
#ifdef PRESSURE
float Pressure; // Atmospheric Pressure. [hectopascals]
#endif
#ifdef AMBIENT_LIGHT
float AmbientLight; // Ambient Light. [lux]
#endif
#ifdef HUMIDITY
float Humidity; // Relative Humidity. [percent]
#endif
#ifdef PROXIMITY
float Proximity; // Proximity. [cm]
#endif
#ifdef TEMPERATURE
float Temperature; // Temperature. [C]
#endif
#ifdef RESERVED
float Reserved; // Reserved
#endif
#ifdef TAP_DETECTOR
BNO_Tap_t TapDetectorFlag;
#endif
#ifdef STEP_DETECTOR
uint32_t StepDetectorLatency; // Step detect latency [uS]
#endif
#ifdef STEP_COUNTER
BNO_StepCounter_t StepCounter;
#endif
#ifdef SIGNIFICANT_MOTION
uint16_t SignificantMotion;
#endif
#ifdef STABILITY_CLASSIFIER
BNO_Stability_t StabilityClassifier;
#endif
#ifdef SHAKE_DETECTOR
BNO_Shake_t ShakeDetector;
#endif
#ifdef FLIP_DETECTOR
uint16_t FlipDetector;
#endif
#ifdef PICKUP_DETECTOR
BNO_Pickup_t PickupDetector;
#endif
#ifdef STABILITY_DETECTOR
BNO_StabilityDetector_t StabilityDetector;
#endif
#ifdef PERSONAL_ACTIVITY_CLASSIFIER
BNO_PersonalActivityClassifier_t PersonalActivityClassifier;
#endif
#ifdef SLEEP_DETECTOR
uint8_t SleepDetector;
#endif
#ifdef TILT_DETECTOR
uint16_t TiltDetector;
#endif
#ifdef POCKET_DETECTOR
uint16_t PocketDetector;
#endif
#ifdef CIRCLE_DETECTOR
uint16_t CircleDetector;
#endif
#ifdef HEART_RATE_MONITOR
uint16_t HeartRateMonitor; // Heart rate in beats per minute.
#endif
#ifdef ARVR_STABILIZED_RV
BNO_RotationVectorWAcc_t ArVrStabilizedRV;
#endif
#ifdef ARVR_STABILIZED_GRV
BNO_RotationVector_t ArVrStabilizedGRV;
#endif
#ifdef GYRO_INTEGRATED_RV
BNO_GyroIntegratedRV_t GyroIntegratedRV;
#endif
#ifdef IZRO_MOTION_REQUEST
BNO_IZroRequest_t IzroRequest;
#endif
#ifdef RAW_OPTICAL_FLOW
BNO_RawOptFlow_t RawOptFlow;
#endif
#ifdef DEAD_RECKONING_POSE
BNO_DeadReckoningPose_t DeadReckoningPose;
#endif
#ifdef WHEEL_ENCODER
BNO_WheelEncoder_t WheelEncoder;
#endif
} SenVal;
} BNO_SensorValue_t;
extern BNO_SensorValue_t sensorData;
extern BNO_RollPitchYaw_t rpy;
// Initialize the sesnor
// During reset or power-on sequence, the bootloader first checks the status of the BOOTN pin.
// If the pin is pulled low during reset or poweron, the BNO08X will enter the bootloader mode.
// If the BOOTN pin is pulled high, then the bootloader starts the application
HAL_StatusTypeDef BNO_Init(void);
// Check if we have unexpected reset
uint8_t isResetOccurred(void);
// Get the sensor that has new data
uint8_t BNO_getSensorEventID(void);
// Soft reset the sensor
uint8_t BNO_Reset(void);
// Turn sensor ON
HAL_StatusTypeDef BNO_On(void);
// When sleep command is issued all sensors that are configured as always on or wake (see 1.3.5.1) will continue to operate all, other sensors will be disabled
HAL_StatusTypeDef BNO_Sleep(void);
// Returns the product id data structure
BNO_productID_t BNO_getProductID(void);
HAL_StatusTypeDef BNO_writeFRS(const uint16_t length, const uint16_t frsType);
HAL_StatusTypeDef BNO_readFRS(const uint16_t frsType);
#ifdef USE_ERROR_REPORT
// See 6.4.1 Report Errors (0x01)
HAL_StatusTypeDef BNO_getErrors(void);
#endif
#ifdef USE_COUNTER_REPORT
// See 6.4.2 Counter Commands (0x02)
HAL_StatusTypeDef BNO_getCounter(const uint8_t sensorID);
HAL_StatusTypeDef BNO_clearCounter(const uint8_t sensorID);
#endif
#ifdef USE_REORIENT
// See 6.4.3.1 Tare Now (0x00)
HAL_StatusTypeDef BNO_TareNow(const BNO_TareAxis_t axis, const BNO_TareRV_t vector);
//This command instructs SH-2 to persist the results of the last tare operation to flash for use at the next system restart.
HAL_StatusTypeDef BNO_TarePerist(void);
// This command instructs SH-2 to set the current run-time sensor reorientation.
// The rotation vector is a signed, 16-bit 2’s-complement fixed point number with a Q-point of 14.
HAL_StatusTypeDef BNO_TareSetReorientation(const double X, const double Y, const double Z, const double W);
// Clears the previously applied tare operation.
HAL_StatusTypeDef BNO_ClearTare(void);
#endif
// !!! ToDo check why is not responding
// The sensor hub responds to the Initialize command with an Initialize Response. In the case
// where the sensor hub reinitializes itself, this response is unsolicited. An unsolicited response is
// also generated after startup
HAL_StatusTypeDef BNO_Initialize(void);
// Save Dynamic Calibration Data (DCD) to flash
HAL_StatusTypeDef BNO_saveCalibration(void);
// This command is sent by the host to configure the ME calibration of the accelerometer, gyro and
// magnetometer giving the host the ability to control when calibration is performed.
HAL_StatusTypeDef BNO_condigureCalibration(const uint8_t sensors);
// This command is sent by the host to request the enable/disable state of the accelerometer, gyro and magnetometer calibration routines
HAL_StatusTypeDef BNO_enableCalibration(void);
// The Configure Periodic DCD Save command configures the automatic saving of DCD. There is
// no response to this command. This command does not inhibit the Save DCD command.
// 0 Enable, 1 Disable
HAL_StatusTypeDef BNO_configurePeriodicDcdSave(const uint8_t enableStatus);
// The Get Oscillator Type command is used to get information about the oscillator type used in the clock system of the SH-2
BNO_Oscillator_t BNO_getOscillatorType(void);
// This command performs an atomic clearDCD (from RAM) and system reset
HAL_StatusTypeDef BNO_clearDcdReset(void);
// See 6.4.10 Simple Calibration Commands (0x0C)
HAL_StatusTypeDef BNO_simpleCalibration(const uint32_t usInterval, const uint16_t calibrationTimeMs);
// !!! ToDo check why is not responding
// The bootloader operating mode request is used to request various operating modes of the FSP200 bootloader
HAL_StatusTypeDef BNO_setBootMode(const BNO_BootMode_t mode);
// !!! ToDo check why is not responding
// Request product ID information about the FSP200 bootloader
BNO_BootMode_t BNO_getBootMode(void);
// The interactive calibration feature requires that the sensor hub be told of the device’s intended motion.
HAL_StatusTypeDef BNO_interactiveCalibration(const BNO_MotionIntent_t intent);
#ifdef WHEEL_ENCODER
// Provide a single sample of wheel encoder data. No response is sent for this command.
HAL_StatusTypeDef BNO_WheelRequest(const uint8_t wheelIndex, const uint32_t timeStampUs, const int16_t wheelData, const uint8_t dataType);
#endif
// This is sent from the host to the hub to trigger a flush of outstanding data from a given sensor
HAL_StatusTypeDef BNO_forceFlush(const uint8_t sensorID);
#ifdef USE_FOR_TELESCOPE
// Sets -X North, Y East Z down
HAL_StatusTypeDef setTelescopeOrientation(void);
// This command is sent by the host to request the enable/disable state of the accelerometer, gyro
// and magnetometer calibration routines.
HAL_StatusTypeDef BNO_getCalibrationStatus(void);
HAL_StatusTypeDef BNO_isCalibrationComplete(void);
HAL_StatusTypeDef BNO_calibrateHighAccuracyAndReset(void);
#endif
HAL_StatusTypeDef BNO_dataAvailable(void);
// Enable features an set report time in mili seconds
BNO_Feature_t BNO_getFeature(const uint8_t sensorID);
HAL_StatusTypeDef BNO_setFeature(const uint8_t reportID, const uint32_t microsBetweenReports, const uint32_t specificConfig);
BNO_RollPitchYaw_t BNO_getRollPitchYaw(void);
#ifdef RAW_ACCELEROMETER
// Raw uncalibrated accelerometer data (ADC units), used for testing
BNO_RawAccelerometer_t getRawAccelerometer(void);
#endif
#ifdef ACCELEROMETER
// Calibrated Acceleration (m/s2). Acceleration of the device including gravity
BNO_Accelerometer_t getaccelerometer(void);
#endif
#ifdef LINEAR_ACCELERATION
// Linear acceleration (m/s2). Acceleration of the device with gravity removed
BNO_Accelerometer_t getLinearAcceleration(void);
#endif
#ifdef GRAVITY
// Gravity (m/s2)
BNO_Accelerometer_t getGravity(void);
#endif
#ifdef RAW_GYROSCOPE
// Raw uncalibrated gyroscope (ADC units). Data direct from the gyroscope, used for testing
BNO_RawGyroscope_t getRawGyroscope(void);
#endif
#ifdef GYROSCOPE_CALIBRATED
// Calibrated gyroscope (rad/s). The angular velocity of the device
BNO_Gyroscope_t getGyroscope(void);
#endif
#ifdef GYROSCOPE_UNCALIBRATED
// Uncalibrated gyroscope (rad/s). Angular velocity without bias compensation.
BNO_GyroscopeUncalibrated_t getGyroscopeUncal(void);
#endif
#ifdef RAW_MAGNETOMETER
// Raw magnetic field measurement (in ADC units). Direct data from the magnetometer, used for testing
BNO_RawMagnetometer_t getRawMagnetometer(void);
#endif
#ifdef MAGNETIC_FIELD_CALIBRATED
// Magnetic field calibrated (in µTesla). The fully calibrated magnetic field measurement.
BNO_MagneticField_t getMagneticField(void);
#endif
#ifdef MAGNETIC_FIELD_UNCALIBRATED
// Magnetic field uncalibrated (in µTesla). The magnetic field measurement without hard-iron offset applied
BNO_MagneticFieldUncalibrated_t getMagneticFieldUncal(void);
#endif
#ifdef ROTATION_VECTOR
// THE ROTATION VECTOR PROVIDES an orientation output that is expressed as a quaternion referenced to magnetic north
// and gravity. It is produced by fusing the outputs of the accelerometer, gyroscope and magnetometer. The rotation
// vector is the most accurate orientation estimate available. The magnetometer provides correction in yaw to
// reduce drift and the gyroscope enables the most responsive performance.
BNO_RotationVectorWAcc_t getRotationVector(void);
#endif
#ifdef GAME_ROTATION_VECTOR
// THE GAME ROTATION VECTOR is an orientation output that is expressed as a quaternion with no specific reference for
// heading, while roll and pitch are referenced against gravity. It is produced by fusing the outputs of the
// accelerometer and the gyroscope (i.e. no magnetometer). The game rotation vector does not use the
// magnetometer to correct the gyroscopes drift in yaw. This is a deliberate omission (as specified by Google) to
// allow gaming applications to use a smoother representation of the orientation without the jumps that an
// instantaneous correction provided by a magnetic field update could provide.
BNO_RotationVector_t getGameRotationVector(void);
#endif
#ifdef GEOMAGNETIC_ROTATION_VECTOR
// THE GEOMAGNETIC ROTATION VECTOR is an orientation output that is expressed as a quaternion referenced to magnetic
// north and gravity. It is produced by fusing the outputs of the accelerometer and magnetometer. The gyroscope is
// specifically excluded in order to produce a rotation vector output using less power than is required to produce the
// rotation vector of section 2.2.4. The consequences of removing the gyroscope are:
// • Less responsive output since the highly dynamic outputs of the gyroscope are not used
// • More errors in the presence of varying magnetic fields
BNO_RotationVectorWAcc_t getGeoMagRotationVector(void);
#endif
#ifdef PRESSURE
// The pressures sensor reports atmospheric pressure. The units are hectopascals.
float getPressure(void);
#endif
#ifdef AMBIENT_LIGHT
// The ambient light sensor reports the measures the amount of light entering the sensor. The units are lux.
float getAmbientLight(void);
#endif
#ifdef HUMIDITY
// The humidity sensor reports relative humidity in the ambient air. The units are percent.
float getHumidity(void);
#endif
#ifdef PROXIMITY
// The proximity sensor reports distance from the device. The units are centimeters.
float getProximity(void);
#endif
#ifdef TEMPERATURE
// The temperature sensor reports ambient temperature. The units are °C.
float getTemperature(void);
#endif
#ifdef RESERVED
//
float getReserved(void);
#endif
#ifdef TAP_DETECTOR
// The tap detector reports single and double taps.
BNO_Tap_t getTapDetectorFlag(void);
#endif
#ifdef STEP_DETECTOR
// The step detector reports steps detected. Each report indicates a single step.
uint32_t getStepDetectorLatency(void);
#endif
#ifdef STEP_COUNTER
// The step counter reports steps counted. The units are steps.
BNO_StepCounter_t getStepCounter(void);
#endif
#ifdef SIGNIFICANT_MOTION
// The significant motion detector sends a single report when it detects significant motion. It
// automatically turns itself off after sending its single report.
uint16_t getSignificantMotion(void);
#endif
#ifdef STABILITY_CLASSIFIER
// The stability classifier sensor reports the type of stability detected.
BNO_Stability_t getStabilityClassifier(void);
#endif
#ifdef SHAKE_DETECTOR
// The shake detector sends a report each time it detects a shake.
BNO_Shake_t getShakeDetector(void);
#endif
#ifdef FLIP_DETECTOR
// The flip detector sends a report each time it detects a flip.
uint16_t getFlipDetector(void);
#endif
#ifdef PICKUP_DETECTOR
// The pickup detector sends a report each time it detects a pickup.
BNO_Pickup_t getPickupDetector(void);
#endif
#ifdef STABILITY_DETECTOR
// The stability detector sends a report each time it detects entry in or exit from a stable state.
BNO_StabilityDetector_t getStabilityDetector(void);
#endif
#ifdef PERSONAL_ACTIVITY_CLASSIFIER
// See 6.5.36 Personal Activity Classifier (0x1E)
BNO_PersonalActivityClassifier_t getPersonalActivityClassifier(void);
#endif
#ifdef SLEEP_DETECTOR
// The sleep detector sends a report each time it detects a change in sleep state.
uint8_t getSleepDetector(void);
#endif
#ifdef TILT_DETECTOR
// The tilt detector sends a report each time it detects a tilt.
uint16_t getTiltDetector(void);
#endif
#ifdef POCKET_DETECTOR
// The pocket detector sends a report each time it detects entry in or exit from a pocket state.
uint16_t getPocketDetector(void);
#endif
#ifdef CIRCLE_DETECTOR
// The circle detector sends a report each time it detects a double circle gesture.
uint16_t getCircleDetector(void);
#endif
#ifdef HEART_RATE_MONITOR
// The heart rate monitor measures the user’s heart rate reported in beats per minute (bpm)
uint16_t getHeartRateMonitor(void);
#endif
#ifdef ARVR_STABILIZED_RV
// AR/VR STABILIZED ROTATION VECTOR
// Estimates of the magnetic field and the roll/pitch of the device can create a potential correction in the rotation
// vector produced. For applications (typically augmented or virtual reality applications) where a sudden jump can be
// disturbing, the output is adjusted to prevent these jumps in a manner that takes account of the velocity of the
// sensor system. This process is called AR/VR stabilization. An FRS (Flash Record System – see Figure 1-31)
// record is provided to allow configuration of this feature
BNO_RotationVectorWAcc_t BNO_getArvrStabilizedRV(void);
#endif
#ifdef ARVR_STABILIZED_GRV
// AR/VR STABILIZED GAME ROTATION VECTOR
// While the magnetometer is removed from the calculation of the game rotation vector, the accelerometer itself can
// create a potential correction in the rotation vector produced (i.e. the estimate of gravity changes). For applications
// (typically augmented or virtual reality applications) where a sudden jump can be disturbing, the output is adjusted
// to prevent these jumps in a manner that takes account of the velocity of the sensor system. This process is called
// AR/VR stabilization. An FRS (Flash Record System – see Figure 1-31) record is provided to allow configuration of this feature.
BNO_RotationVector_t BNO_getArVrStabilizedGRV(void);
#endif
#ifdef GYRO_INTEGRATED_RV
// GYRO ROTATION VECTOR
// Head tracking systems within a virtual reality headset require low latency processing of motion. To facilitate this,
// the BNO08X can provide a rotation vector at rates up to 1kHz. The gyro rotation Vector provides an alternate
// orientation to the standard rotation vector. Compared to the standard rotation vector the gyro rotation vector has
// an optimized processing path and correction style (correction is the adjustments made to the output based on
// more accurate estimates of gravity, mag field, angular velocity) that is suitable for head tracking applications.
BNO_GyroIntegratedRV_t BNO_getGyroIntegratedRV(void);
#endif
#ifdef IZRO_MOTION_REQUEST
BNO_IZroRequest_t getIzroRequest(void);
#endif
#ifdef RAW_OPTICAL_FLOW
// The Optical Flow report provides raw optical flow sensor data.
BNO_RawOptFlow_t getRawOptFlow(void);
#endif
#ifdef DEAD_RECKONING_POSE
// The Dead Reckoning Pose report provides the linear position, angular position, linear velocity,
// and angular velocity as determined by the Dead Reckoning system.
BNO_DeadReckoningPose_t getDeadReckoningPose(void);
#endif
#ifdef WHEEL_ENCODER
BNO_WheelEncoder_t getWheelEncoder(void);
#endif
#ifdef __cplusplus
}
#endif
#endif /* __BNO_08x_I2C_H */
In BNO_08x_I2C.c add the following code
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include "BNO_08x_I2C.h"
#include "i2c.h"
#include "gpio.h"
#include "tim.h"
//Global Variables
// The interrupt variable
volatile uint8_t BNO_Ready = 0;
//A monotonically incrementing uint8_t that rolls over. It is used to detect missing commands and to synchronize responses
static uint8_t commandSequenceNumber = 0;
// Similar with the above var
static uint8_t cmdSeqNo = 0;
// The data array for read and write operations
static uint8_t bufferIO[RX_PACKET_SIZE];
// 6 SHTP channels. Each channel has its own sequence number
static uint8_t sequenceNumber[SEQUENCE_SIZE];
// Variable that hold the reset status of the sensor
static uint8_t resetOccurred = 0;
static uint8_t saveDcdStatus = 1;
// Structure to hold the product ID
static BNO_productID_t myID = {0};
#ifdef USE_ERROR_REPORT
static BNO_Error_t errors = {0};
#endif
#ifdef USE_COUNTER_REPORT
static BNO_Counts_t counts = {0};
#endif
#ifdef USE_ERROR_REPORT
static BNO_Error_t errors = {0};
#endif
// Structure to hold the product calibration status data
static BNO_calibrationStat_t calibrationStatus = {0};
// Structure to hold the product calibration status data
static BNO_FrsWriteResp_t frsWriteResponse = {0};
static BNO_FrsReadResp_t frsReadResponse = {0};
static BNO_Oscillator_t oscillatorType = {0};
static BNO_Boot_t bootLoader = {0};
#ifdef USE_FOR_TELESCOPE
static BNO_CommandReq_t cmdRequest = {0};
static BNO_CommandResp_t cmdResponse = {0};
static BNO_Feature_t sensorFeartures = {0};
BNO_RollPitchYaw_t rpy = {0};
#endif
BNO_SensorValue_t sensorData = {0};
// Wait for an interrupt to occur or timeout in 200ms
static uint8_t waitInt(void) {
uint32_t timeOut = HAL_GetTick() + RESET_DELAY;
while(timeOut > HAL_GetTick()) {
if(BNO_Ready) {
BNO_Ready = 0;
return 1;
}
}
return 0;
}
// Timer 2 set to get microseconds
static void start_timer(void) {
__HAL_RCC_TIM2_CLK_ENABLE();
// Prescale to get 1 count per uS
uint16_t prescaler = 274;
htim2.Instance = TIM2;
htim2.Init.Period = 0xFFFFFFFF;
htim2.Init.Prescaler = prescaler;
htim2.Init.ClockDivision = 0;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&htim2);
HAL_TIM_Base_Start(&htim2);
}
// Get microseconds
static uint32_t timeNowUs(void) {
return __HAL_TIM_GET_COUNTER(&htim2);
}
// Delay microseconds
static void delay_us(uint32_t t) {
uint32_t now = timeNowUs();
uint32_t start = now;
while ((now - start) < t) {
now = timeNowUs();
}
}
#ifdef USE_FOR_TELESCOPE
// get 1/sqrt(x)
static float invSqrt(const float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
// Update structures quatIJKR and angles
static void quaternionUpdate(const BNO_RotationVectorWAcc_t *inRotVector) {
double norm = invSqrt(inRotVector->I * inRotVector->I + inRotVector->J * inRotVector->J + inRotVector->K * inRotVector->K + inRotVector->Real * inRotVector->Real); // normalize quaternion
double q1 = inRotVector->I * norm; //x
double q2 = inRotVector->J * norm; //y
double q3 = inRotVector->K * norm; //z
double q4 = inRotVector->Real * norm; //w
rpy.Pitch = atan2f(2.0f * (q2*q3 + q1*q4), q1*q1 + q2*q2 - q3*q3 - q4*q4);
rpy.Roll = -asinf(2.0f * (q2*q4 - q1*q3));
rpy.Yaw = atan2f(2.0f * (q1*q2 + q3*q4), q1*q1 - q2*q2 - q3*q3 + q4*q4);
rpy.Pitch *= _180_DIV_PI;
rpy.Roll *= _180_DIV_PI;
rpy.Yaw *= _180_DIV_PI;
if(rpy.Yaw >= 0.0)
rpy.Yaw = 360.0 - rpy.Yaw;
else
rpy.Yaw = -rpy.Yaw;
if(rpy.Pitch >= 0.0)
rpy.Pitch = 180.0 - rpy.Pitch;
else
rpy.Pitch = -(rpy.Pitch + 180.f);
}
#endif
// Sets to buffrIO first 21 bytes to 0
static void resetHeader(const uint8_t id) {
memset(bufferIO, 0, TX_PACKET_SIZE);
bufferIO[4] = id;
}
// Send a packet data to BNO
static HAL_StatusTypeDef sendPacket(const uint8_t channelNumber) {
// dataLength includes the SHTP_HEADER_SIZE
uint8_t dataLength = 0;
bufferIO[2] = channelNumber;
bufferIO[3] = sequenceNumber[channelNumber]++;
if(bufferIO[2] == CHANNEL_EXECUTABLE) {
dataLength = 5;
} else {
switch(bufferIO[4]) {
case REPORT_SENSOR_FLUSH_REQUEST:
case REPORT_GET_FEATURE_REQUEST:
case REPORT_PRODUCT_ID_REQUEST:
dataLength = 6;
break;
case REPORT_FRS_READ_REQUEST:
dataLength = 12;
break;
case COMMAND_ME_CALIBRATE:
case COMMAND_TARE:
case COMMAND_SAVE_DCD:
case REPORT_COMMAND_REQUEST:
case REPORT_FRS_WRITE_REQUEST:
dataLength = 16;
break;
case REPORT_SET_FEATURE_COMMAND:
dataLength = 21;
break;
}
}
bufferIO[0] = dataLength & 0xFF;
bufferIO[1] = (dataLength >> 8) & 0x7F;
// Send packet to IMU
HAL_StatusTypeDef ret = HAL_I2C_Master_Transmit(&hi2c1, BNO_W_ADDR, bufferIO, dataLength, PORT_TIMEOUT);
delay_us(RESET_DELAY); // Delay 100 microsecs before next I2C
return ret;
}
// Get a data packet from BNO
static HAL_StatusTypeDef receivePacket(void) {
// Reset Interrupt status
BNO_Ready = 0;
memset(bufferIO, 0, TX_PACKET_SIZE);
//Ask for 4 bytes to find out how much data we need to read
if(HAL_I2C_Master_Receive(&hi2c1, BNO_R_ADDR, bufferIO, HEADER_SIZE, PORT_TIMEOUT) != HAL_OK) {
return HAL_ERROR;
}
// Calculate the number of data bytes in packet
uint16_t rxPacketLength = *(uint16_t *)&bufferIO;//getPacketLenghth();
// No data received ?
if(!rxPacketLength) {
return HAL_ERROR;
}
if(rxPacketLength > RX_PACKET_SIZE) {
return HAL_ERROR;
}
// Wait 100us
delay_us(RESET_DELAY); // Delay 100 microsecs before next I2C
if(HAL_I2C_Master_Receive(&hi2c1, BNO_R_ADDR, bufferIO, rxPacketLength, PORT_TIMEOUT) != HAL_OK) {
return HAL_ERROR;
}
delay_us(RESET_DELAY); // Delay 100 microsecs before next I2C
return HAL_OK;
}
// Send a command on exe channel
static HAL_StatusTypeDef sendExecutable(const uint8_t cmd) {
resetHeader(cmd);
return sendPacket(CHANNEL_EXECUTABLE);
}
// Compute a sensor value from bufferIO
//bufferIO - 0..3 Header
//bufferIO - 4..8 Time stamp
//bufferIO - 9 Which sensor produced this event
//bufferIO - 10 Sequence number increments once for each report sent. Gaps in the sequence numbers indicate missing or dropped reports.
//bufferIO - 11 Status bits 7-5: reserved, 4-2: exponent delay, 1-0: Accuracy
static void getSensorValue(void) {
//Calculate the number of data bytes in this packet
//int16_t dataLength = *(uint16_t *)&bufferIO - 4;
sensorData.sensorId = bufferIO[9];
sensorData.timestamp = *(uint32_t *)&bufferIO[5];
#ifdef GYRO_INTEGRATED_RV
if(sensorData.sensorId != GYRO_INTEGRATED_RV) {
sensorData.sequence = bufferIO[10];
sensorData.status = bufferIO[11] & 0x03; //Get status bits
} else {
sensorData.sequence = 0;
sensorData.status = 0; //Get status bits
}
#else
sensorData.sequence = bufferIO[10];
sensorData.status = bufferIO[11] & 0x03; //Get status bits
#endif
switch(sensorData.sensorId) {
#ifdef RAW_ACCELEROMETER
case RAW_ACCELEROMETER:
sensorData.SenVal.RawAccelerometer.X = *(int16_t *)&bufferIO[13];
sensorData.SenVal.RawAccelerometer.Y = *(int16_t *)&bufferIO[15];
sensorData.SenVal.RawAccelerometer.Z = *(int16_t *)&bufferIO[17];
sensorData.SenVal.RawAccelerometer.TimeStamp = *(uint32_t *)&bufferIO[21];
break;
#endif
#ifdef ACCELEROMETER
case ACCELEROMETER:
sensorData.SenVal.Accelerometer.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
sensorData.SenVal.Accelerometer.Y = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
sensorData.SenVal.Accelerometer.Z = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
break;
#endif
#ifdef LINEAR_ACCELERATION
case LINEAR_ACCELERATION:
sensorData.SenVal.LinearAcceleration.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
sensorData.SenVal.LinearAcceleration.Y = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
sensorData.SenVal.LinearAcceleration.Z = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
break;
#endif
#ifdef GRAVITY
case GRAVITY:
sensorData.SenVal.Gravity.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
sensorData.SenVal.Gravity.Y = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
sensorData.SenVal.Gravity.Z = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
break;
#endif
#ifdef RAW_GYROSCOPE
case RAW_GYROSCOPE:
sensorData.SenVal.RawGyroscope.X = *(int16_t *)&bufferIO[13];
sensorData.SenVal.RawGyroscope.Y = *(int16_t *)&bufferIO[15];
sensorData.SenVal.RawGyroscope.Z = *(int16_t *)&bufferIO[17];
sensorData.SenVal.RawGyroscope.Temperature = *(int16_t *)&bufferIO[19];
sensorData.SenVal.RawGyroscope.TimeStamp = *(uint32_t *)&bufferIO[21];
break;
#endif
#ifdef GYROSCOPE_CALIBRATED
case GYROSCOPE_CALIBRATED:
sensorData.SenVal.Gyroscope.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q9;
sensorData.SenVal.Gyroscope.Y = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q9;
sensorData.SenVal.Gyroscope.Z = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q9;
break;
#endif
#ifdef GYROSCOPE_UNCALIBRATED
case GYROSCOPE_UNCALIBRATED:
sensorData.SenVal.GyroscopeUncal.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q9;
sensorData.SenVal.GyroscopeUncal.Y = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q9;
sensorData.SenVal.GyroscopeUncal.Z = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q9;
sensorData.SenVal.GyroscopeUncal.BiasX = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q9;
sensorData.SenVal.GyroscopeUncal.BiasY = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q9;
sensorData.SenVal.GyroscopeUncal.BiasZ = (float)(*(int16_t *)&bufferIO[23]) * SCALE_Q9;
break;
#endif
#ifdef RAW_MAGNETOMETER
case RAW_MAGNETOMETER:
sensorData.SenVal.RawMagnetometer.X = *(int16_t *)&bufferIO[13];
sensorData.SenVal.RawMagnetometer.Y = *(int16_t *)&bufferIO[15];
sensorData.SenVal.RawMagnetometer.Z = *(int16_t *)&bufferIO[17];
sensorData.SenVal.RawMagnetometer.TimeStamp = *(uint32_t *)&bufferIO[21];
break;
#endif
#ifdef MAGNETIC_FIELD_CALIBRATED
case MAGNETIC_FIELD_CALIBRATED:
sensorData.SenVal.MagneticField.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q4;
sensorData.SenVal.MagneticField.Y = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q4;
sensorData.SenVal.MagneticField.Z = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q4;
break;
#endif
#ifdef MAGNETIC_FIELD_UNCALIBRATED
case MAGNETIC_FIELD_UNCALIBRATED:
sensorData.SenVal.MagneticFieldUncal.X = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q4;
sensorData.SenVal.MagneticFieldUncal.Y = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q4;
sensorData.SenVal.MagneticFieldUncal.Z = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q4;
sensorData.SenVal.MagneticFieldUncal.BiasX = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q4;
sensorData.SenVal.MagneticFieldUncal.BiasY = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q4;
sensorData.SenVal.MagneticFieldUncal.BiasZ = (float)(*(int16_t *)&bufferIO[23]) * SCALE_Q4;
break;
#endif
#ifdef ROTATION_VECTOR
case ROTATION_VECTOR:
#ifdef USE_FOR_TELESCOPE
sensorData.SenVal.RotationVector.I = -(float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.RotationVector.J = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.RotationVector.K = -(float)(*(int16_t *)&bufferIO[17]) * SCALE_Q14;
sensorData.SenVal.RotationVector.Real = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q14;
sensorData.SenVal.RotationVector.Accuracy = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q14;
// Update Euler
quaternionUpdate(&sensorData.SenVal.RotationVector);
#else
sensorData.SenVal.RotationVector.I = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.RotationVector.J = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.RotationVector.K = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q14;
sensorData.SenVal.RotationVector.Real = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q14;
sensorData.SenVal.RotationVector.Accuracy = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q14;
#endif
break;
#endif
#ifdef GAME_ROTATION_VECTOR
case GAME_ROTATION_VECTOR:
sensorData.SenVal.GameRotationVector.I = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.GameRotationVector.J = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.GameRotationVector.K = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q14;
sensorData.SenVal.GameRotationVector.Real = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q14;
break;
#endif
#ifdef GEOMAGNETIC_ROTATION_VECTOR
case GEOMAGNETIC_ROTATION_VECTOR:
sensorData.SenVal.GeoMagRotationVector.I = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.GeoMagRotationVector.J = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.GeoMagRotationVector.K = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q14;
sensorData.SenVal.GeoMagRotationVector.Real = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q14;
sensorData.SenVal.GeoMagRotationVector.Accuracy = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q14;
break;
#endif
#ifdef PRESSURE
case PRESSURE:
sensorData.SenVal.Pressure = (float)(*(int32_t *)&bufferIO[13]) * SCALE_Q20;
break;
#endif
#ifdef AMBIENT_LIGHT
case AMBIENT_LIGHT:
sensorData.SenVal.AmbientLight = (float)(*(int32_t *)&bufferIO[13]) * SCALE_Q8;
break;
#endif
#ifdef HUMIDITY
case HUMIDITY:
sensorData.SenVal.Humidity = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q8;
break;
#endif
#ifdef PROXIMITY
case PROXIMITY:
sensorData.SenVal.Proximity = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q4;
break;
#endif
#ifdef TEMPERATURE
case TEMPERATURE:
sensorData.SenVal.Temperature = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q7;
break;
#endif
#ifdef RESERVED
case RESERVED:
sensorData.SenVal.Reserved = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q7;
break;
#endif
#ifdef TAP_DETECTOR
case TAP_DETECTOR:
sensorData.SenVal.TapDetectorFlag = bufferIO[13];
break;
#endif
#ifdef STEP_DETECTOR
case STEP_DETECTOR:
sensorData.SenVal.StepDetectorLatency = *(uint32_t *)&bufferIO[13];
break;
#endif
#ifdef STEP_COUNTER
case STEP_COUNTER:
sensorData.SenVal.StepCounter.Latency = *(uint32_t *)&bufferIO[13];
sensorData.SenVal.StepCounter.Steps = *(uint32_t *)&bufferIO[17];
break;
#endif
#ifdef SIGNIFICANT_MOTION
case SIGNIFICANT_MOTION:
sensorData.SenVal.SignificantMotion = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef STABILITY_CLASSIFIER
case STABILITY_CLASSIFIER:
sensorData.SenVal.StabilityClassifier = bufferIO[13];
break;
#endif
#ifdef SHAKE_DETECTOR
case SHAKE_DETECTOR:
sensorData.SenVal.ShakeDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef FLIP_DETECTOR
case FLIP_DETECTOR:
sensorData.SenVal.FlipDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef PICKUP_DETECTOR
case PICKUP_DETECTOR:
sensorData.SenVal.PickupDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef STABILITY_DETECTOR
case STABILITY_DETECTOR:
sensorData.SenVal.StabilityDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef PERSONAL_ACTIVITY_CLASSIFIER
case PERSONAL_ACTIVITY_CLASSIFIER:
sensorData.SenVal.PersonalActivityClassifier.Page = bufferIO[13] & 0x7F;
sensorData.SenVal.PersonalActivityClassifier.LastPage = ((bufferIO[13] & 0x80) != 0);
sensorData.SenVal.PersonalActivityClassifier.MostLikelyState = bufferIO[14];
// ToDo remove for loop, use pointer
for (int n = 0; n < 10; n++) {
sensorData.SenVal.PersonalActivityClassifier.Confidence[n] = bufferIO[15+n];
}
break;
#endif
#ifdef SLEEP_DETECTOR
case SLEEP_DETECTOR:
sensorData.SenVal.SleepDetector = bufferIO[13];
break;
#endif
#ifdef TILT_DETECTOR
case TILT_DETECTOR:
sensorData.SenVal.TiltDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef POCKET_DETECTOR
case POCKET_DETECTOR:
sensorData.SenVal.PocketDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef CIRCLE_DETECTOR
case CIRCLE_DETECTOR:
sensorData.SenVal.CircleDetector = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef HEART_RATE_MONITOR
case HEART_RATE_MONITOR:
sensorData.SenVal.HeartRateMonitor = *(uint16_t *)&bufferIO[13];
break;
#endif
#ifdef ARVR_STABILIZED_RV
case ARVR_STABILIZED_RV:
sensorData.SenVal.ArVrStabilizedRV.I = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedRV.J = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedRV.K = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedRV.Real = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedRV.Accuracy = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q12;
break;
#endif
#ifdef ARVR_STABILIZED_GRV
case ARVR_STABILIZED_GRV:
sensorData.SenVal.ArVrStabilizedGRV.I = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedGRV.J = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedGRV.K = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q14;
sensorData.SenVal.ArVrStabilizedGRV.Real = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q14;
break;
#endif
#ifdef GYRO_INTEGRATED_RV
case GYRO_INTEGRATED_RV:
sensorData.SenVal.GyroIntegratedRV.I = (float)(*(int16_t *)&bufferIO[9]) * SCALE_Q14;
sensorData.SenVal.GyroIntegratedRV.J = (float)(*(int16_t *)&bufferIO[11]) * SCALE_Q14;
sensorData.SenVal.GyroIntegratedRV.J = (float)(*(int16_t *)&bufferIO[13]) * SCALE_Q14;
sensorData.SenVal.GyroIntegratedRV.Real = (float)(*(int16_t *)&bufferIO[15]) * SCALE_Q14;
sensorData.SenVal.GyroIntegratedRV.AngleVelX = (float)(*(int16_t *)&bufferIO[17]) * SCALE_Q10;
sensorData.SenVal.GyroIntegratedRV.AngleVelY = (float)(*(int16_t *)&bufferIO[19]) * SCALE_Q10;
sensorData.SenVal.GyroIntegratedRV.AngleVelZ = (float)(*(int16_t *)&bufferIO[21]) * SCALE_Q10;
break;
#endif
#ifdef IZRO_MOTION_REQUEST
case IZRO_MOTION_REQUEST:
sensorData.SenVal.IzroRequest.Intent = (BNO_IZroMotionIntent_t)bufferIO[13];
sensorData.SenVal.IzroRequest.Request = (BNO_IZroMotionRequest_t)bufferIO[14];
break;
#endif
#ifdef RAW_OPTICAL_FLOW
case RAW_OPTICAL_FLOW:
sensorData.SenVal.RawOptFlow.Dx = *(int16_t *)&bufferIO[13];
sensorData.SenVal.RawOptFlow.Dy = *(int16_t *)&bufferIO[15];
sensorData.SenVal.RawOptFlow.Iq = *(int16_t *)&bufferIO[17];
sensorData.SenVal.RawOptFlow.ResX = bufferIO[19];
sensorData.SenVal.RawOptFlow.ResY = bufferIO[20];
sensorData.SenVal.RawOptFlow.Shutter = bufferIO[21];
sensorData.SenVal.RawOptFlow.FrameMax = bufferIO[22];
sensorData.SenVal.RawOptFlow.FrameAvg = bufferIO[23];
sensorData.SenVal.RawOptFlow.FrameMin = bufferIO[24];
sensorData.SenVal.RawOptFlow.LaserOn = bufferIO[25];
sensorData.SenVal.RawOptFlow.Dt = *(int16_t *)&bufferIO[27];
sensorData.SenVal.RawOptFlow.TimeStamp = *(int32_t *)&bufferIO[29];
break;
#endif
#ifdef DEAD_RECKONING_POSE
case DEAD_RECKONING_POSE:
sensorData.SenVal.DeadReckoningPose.TimeStamp = *(int32_t *)&bufferIO[13];
sensorData.SenVal.DeadReckoningPose.LinPosX = (float)(*(int32_t *)&bufferIO[17]) * SCALE_Q17;
sensorData.SenVal.DeadReckoningPose.LinPosY = (float)(*(int32_t *)&bufferIO[21]) * SCALE_Q17;
sensorData.SenVal.DeadReckoningPose.LinPosZ = (float)(*(int32_t *)&bufferIO[25]) * SCALE_Q17;
sensorData.SenVal.DeadReckoningPose.I = (float)(*(int32_t *)&bufferIO[29]) * SCALE_Q30;
sensorData.SenVal.DeadReckoningPose.J = (float)(*(int32_t *)&bufferIO[33]) * SCALE_Q30;
sensorData.SenVal.DeadReckoningPose.K = (float)(*(int32_t *)&bufferIO[37]) * SCALE_Q30;
sensorData.SenVal.DeadReckoningPose.Real = (float)(*(int32_t *)&bufferIO[41]) * SCALE_Q30;
sensorData.SenVal.DeadReckoningPose.LinVelX = (float)(*(int32_t *)&bufferIO[45]) * SCALE_Q25;
sensorData.SenVal.DeadReckoningPose.LinVelY = (float)(*(int32_t *)&bufferIO[49]) * SCALE_Q25;
sensorData.SenVal.DeadReckoningPose.LinVelZ = (float)(*(int32_t *)&bufferIO[53]) * SCALE_Q25;
sensorData.SenVal.DeadReckoningPose.AngleVelX = (float)(*(int32_t *)&bufferIO[57]) * SCALE_Q25;
sensorData.SenVal.DeadReckoningPose.AngleVelY = (float)(*(int32_t *)&bufferIO[61]) * SCALE_Q25;
sensorData.SenVal.DeadReckoningPose.AngleVelZ = (float)(*(int32_t *)&bufferIO[65]) * SCALE_Q25;
break;
#endif
#ifdef WHEEL_ENCODER
case WHEEL_ENCODER:
sensorData.SenVal.WheelEncoder.TimeStamp = *(int32_t *)&bufferIO[13];
sensorData.SenVal.WheelEncoder.WheelIndex = bufferIO[17];
sensorData.SenVal.WheelEncoder.DataType = bufferIO[18];
sensorData.SenVal.WheelEncoder.Data = *(int16_t *)&bufferIO[19];
break;
#endif
}
}
// Process a command response
static HAL_StatusTypeDef processCommandResponse(void) {
// Reset complete
switch(cmdResponse.command) {
#ifdef USE_ERROR_REPORT
case COMMAND_ERRORS: // 0x01 – report all errors in the error queue
errors = *(BNO_Error_t *)&bufferIO[9];
return HAL_OK;
break;
#endif
#ifdef USE_COUNTER_REPORT
case COMMAND_COUNTER: // 0x02 – Counter command
counts = *(BNO_Counts_t *)&bufferIO[9];
return HAL_OK;
break;
#endif
case COMMAND_UNSOLICITED_INITIALIZE: // 0x84 – Initialize (unsolicited)
case COMMAND_INITIALIZE: // 0x04 – Initialize
if (!bufferIO[9]) {
resetOccurred = 1;
return HAL_OK;
}
break;
case COMMAND_SAVE_DCD: // 0x06 – Save DCD
saveDcdStatus = bufferIO[9];
return HAL_OK;
break;
case COMMAND_ME_CALIBRATE: // 0x07 – Configure ME Calibration
calibrationStatus = *(BNO_calibrationStat_t *)&bufferIO[9];
return HAL_OK;
break;
case COMMAND_OSCILLATOR: // 0x0A – Get Oscillator Type Command
oscillatorType = bufferIO[9];
return HAL_OK;
break;
case COMMAND_TURNTABLE_CAL: // 0x0C – Turntable Calibration
calibrationStatus.Status = bufferIO[9];
return HAL_OK;
break;
case COMMAND_BOOTLOADER:// 0x0D – Bootloader command
bootLoader = *(BNO_Boot_t *)&bufferIO[10];
return HAL_OK;
break;
}
return HAL_ERROR;
}
// Process a response
static HAL_StatusTypeDef processResponse(void) {
switch(bufferIO[4]) {
case REPORT_UNSOLICITED_RESPONSE: // 0x00
if(bufferIO[2] == CHANNEL_COMMAND) return HAL_OK;
break;
case REPORT_UNSOLICITED_RESPONSE1: // 0x01
if(bufferIO[2] == CHANNEL_EXECUTABLE) return HAL_OK;
break;
case REPORT_COMMAND_RESPONSE: // 0xF1
cmdResponse = *(BNO_CommandResp_t *)&bufferIO[5];
return processCommandResponse();
break;
case REPORT_FRS_READ_RESPONSE: // 0xF3
frsReadResponse = *(BNO_FrsReadResp_t *)&bufferIO[5];
return HAL_OK;
break;
case REPORT_FRS_WRITE_RESPONSE: // 0xF5
frsWriteResponse = *(BNO_FrsWriteResp_t *)&bufferIO[5];
return HAL_OK;
break;
case REPORT_PRODUCT_ID_RESPONSE: // 0xF8
myID = *(BNO_productID_t *)&bufferIO[5];
return HAL_OK;
break;
case REPORT_BASE_TIMESTAMP_REF: // 0xFB
if(bufferIO[2] == CHANNEL_REPORTS) {
getSensorValue();
return HAL_OK;
}
return HAL_ERROR;
break;
case REPORT_GET_FEATURE_RESPONSE: // 0xFC
sensorFeartures = *(BNO_Feature_t *)&bufferIO[5];
return HAL_OK;
break;
case REPORT_SENSOR_FLUSH_RESPONSE: // 0xEF
if(bufferIO[2] == CHANNEL_REPORTS) {
// Not using them, so....
return HAL_OK;
}
return HAL_ERROR;
break;
}
return HAL_ERROR;
}
// Wait to receive a packet from BNO
static HAL_StatusTypeDef waitForPacket(void) {
if(waitInt()) {
return receivePacket();
}
return HAL_ERROR;
}
// Wait for a response from sensor
static HAL_StatusTypeDef waitForCommandResponse(void) {
uint8_t sendChannel = CHANNEL_CONTROL;
uint8_t receiveChannel = CHANNEL_CONTROL;
uint8_t expectedResponse = REPORT_COMMAND_RESPONSE;
switch(bufferIO[4]) {
case REPORT_PRODUCT_ID_REQUEST:
expectedResponse = REPORT_PRODUCT_ID_RESPONSE;
break;
case REPORT_SENSOR_FLUSH_REQUEST:
receiveChannel = CHANNEL_REPORTS;
expectedResponse = REPORT_SENSOR_FLUSH_RESPONSE;
break;
case REPORT_GET_FEATURE_REQUEST:
expectedResponse = REPORT_GET_FEATURE_RESPONSE;
break;
case REPORT_FRS_WRITE_REQUEST:
expectedResponse = REPORT_FRS_WRITE_RESPONSE;
break;
case REPORT_FRS_READ_REQUEST:
expectedResponse = REPORT_FRS_READ_RESPONSE;
break;
}
if(sendPacket(sendChannel) == HAL_OK) {
uint8_t retry = 5;
while(retry) {
if(waitForPacket() == HAL_OK) {
if((bufferIO[2] == receiveChannel) && (bufferIO[4] == expectedResponse)) {
return processResponse(); // Found correct packet!
}
}
retry--;
}
}
return HAL_ERROR;
}
// Gets the sensor SW information
static HAL_StatusTypeDef getID(void) {
resetHeader(REPORT_PRODUCT_ID_REQUEST);
return waitForCommandResponse();
}
//--------------------------------------------------------------------------------------------------------------------------
// Initialize the sesnor
// During reset or power-on sequence, the bootloader first checks the status of the BOOTN pin.
// If the pin is pulled low during reset or poweron, the BNO08X will enter the bootloader mode.
// If the BOOTN pin is pulled high, then the bootloader starts the application
HAL_StatusTypeDef BNO_Init(void) {
BNO_BOOT_On; // Off only for DFU mode 0x52 I2C Address
// Set reset pin low
BNO_RST_Off;
HAL_Delay(RESET_DELAY);
// Start us timer
start_timer();
// Enable interrupt BNO_Ready
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
// Delay for RESET_DELAY_US to ensure reset takes effect
BNO_RST_On;
HAL_Delay(RESET_DELAY);
// If we got the initiat packet we make a soft reset
//if(waitForPacket()) {
if(processResponse() == HAL_OK) {
// Wait for intterupt
if(waitInt()) {
if(BNO_Reset() == HAL_OK) {
// Finally, we want to interrogate the device about its model and version.
BNO_On();
return getID();
}
}
}
//}
return HAL_ERROR;
}
// Check if we have unexpected reset
uint8_t isResetOccurred(void) {
if(resetOccurred) {
resetOccurred = 0;
return 1;
}
return resetOccurred;
}
// Get the sensor that has new data
uint8_t BNO_getSensorEventID(void) {
return sensorData.sensorId;
}
// Soft reset the sensor
HAL_StatusTypeDef BNO_Reset(void) {
if(sendExecutable(COMMAND_INITIALIZE_RESET) != HAL_OK) { // Write 1 byte to chan EXE
return HAL_ERROR;
}
HAL_Delay(700); // 700 millisecs for reboot
// 2 packet to be ignored after reset
if(waitForCommandResponse() == HAL_OK) {
if(resetOccurred) {
resetOccurred = 0;
} else {
return HAL_ERROR;
}
}
return HAL_OK;
}
// Turn sensor ON
HAL_StatusTypeDef BNO_On(void) {
return sendExecutable(COMMAND_INITIALIZE_ON);
}
// When sleep command is issued all sensors that are configured as always on or wake (see 1.3.5.1) will continue to operate all, other sensors will be disabled
HAL_StatusTypeDef BNO_Sleep(void) {
return sendExecutable(COMMAND_INITIALIZE_SLEEP);
}
// Returns the product id data structure see 6.3.1 and 6.3.2
BNO_productID_t BNO_getProductID(void) {
return myID;
}
// Length in 32-bit words of the record to be written. If the length is set to 0 then the record is erased
HAL_StatusTypeDef BNO_writeFRS(const uint16_t length, const uint16_t frsType) {
// FRS Write Request (0xF7)
resetHeader(REPORT_FRS_WRITE_REQUEST);
//bufferIO[5] = 0; // Reserved
*(uint16_t *)&bufferIO[6] = length;
*(uint16_t *)&bufferIO[8] = frsType;
// No response for this
if(waitForCommandResponse() == HAL_OK) {
//!!! ToDo analize the response and do something with it
uint8_t sendMoreData = 0;
uint8_t completed = 0;
// Status is bufferIO[5] see 6.3.5 FRS Write Response (0xF5)
switch(bufferIO[5]) {
case FRS_WRITE_STATUS_RECEIVED:
case FRS_WRITE_STATUS_READY:
sendMoreData = 1;
break;
case FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE:
case FRS_WRITE_STATUS_BUSY:
case FRS_WRITE_STATUS_FAILED:
case FRS_WRITE_STATUS_NOT_READY:
case FRS_WRITE_STATUS_INVALID_LENGTH:
case FRS_WRITE_STATUS_INVALID_RECORD:
case FRS_WRITE_STATUS_DEVICE_ERROR:
case FRS_WRITE_STATUS_READ_ONLY:
completed = 1;
break;
case FRS_WRITE_STATUS_WRITE_COMPLETED:
// Successful completion
completed = 1;
break;
case FRS_WRITE_STATUS_RECORD_VALID:
break;
}
return HAL_OK;
}
return HAL_ERROR;
}
// See 6.3.6 FRS Read Request (0xF4) 6.3.7 FRS Read Response (0xF3)
HAL_StatusTypeDef BNO_readFRS(const uint16_t frsType) {
resetHeader(REPORT_FRS_READ_REQUEST);
//bufferIO[5] = 0; // Reserved
//bufferIO[6] = 0; // Read from start
*(uint16_t *)&bufferIO[8] = frsType;
//bufferIO[10] = 0; // Read all avail data
if(waitForCommandResponse() == HAL_OK) {
//!!! ToDo analize the response and do something with it
// Get status portion of len_status field
uint8_t status = bufferIO[5] & 0x0F;
// Get Datalen portion of len_status field
uint8_t Datalen = ((bufferIO[5] >> 4) & 0x0F);
switch(status) {
// Check for errors: Unrecognized FRS type, Busy, Out of range, Device error
case FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE:
case FRS_READ_STATUS_BUSY:
case FRS_READ_STATUS_OFFSET_OUT_OF_RANGE:
case FRS_READ_STATUS_DEVICE_ERROR:
// Operation failed
return HAL_ERROR;
break;
case FRS_READ_STATUS_RECORD_EMPTY: // Empty record
return HAL_OK;
break;
// If read is done...complete the operation
case FRS_READ_STATUS_READ_RECORD_COMPLETED:
case FRS_READ_STATUS_READ_BLOCK_COMPLETED:
case FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED:
return HAL_OK;
break;
}
}
return HAL_ERROR;
}
#ifdef USE_ERROR_REPORT
HAL_StatusTypeDef BNO_getErrors(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_ERRORS; // 0x01 – report all errors in the error queue
//bufferIO[7] = 0; // he severity of errors to report. Errors of this severity and higher will be reported. 0 – highest priority
if(waitForCommandResponse() == HAL_OK) {
// Error source. 0 – reserved, 1 – MotionEngine, 2 – MotionHub, 3 – SensorHub, 4 – Chip level executable, 5-254 reserved. 255 – no error to report.
if(errors.Source == 0xFF) {
return HAL_OK;
}
}
return HAL_ERROR;
}
#endif
#ifdef USE_COUNTER_REPORT
HAL_StatusTypeDef BNO_getCounter(const uint8_t sensorID) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_COUNTER; // 0x02 – Counter command
//bufferIO[7] = 0; // 0x00 – Sub-command: get counts
bufferIO[8] = sensorID;
if(waitForCommandResponse() == HAL_OK) {
// Error source. 0 – reserved, 1 – MotionEngine, 2 – MotionHub, 3 – SensorHub, 4 – Chip level executable, 5-254 reserved. 255 – no error to report.
if(errors.Source == 0xFF) {
return HAL_OK;
}
}
return HAL_ERROR;
}
HAL_StatusTypeDef BNO_clearCounter(const uint8_t sensorID) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_COUNTER; // 0x02 – Counter command
bufferIO[7] = 1; // 0x01 – Sub-command: clear counts
bufferIO[8] = sensorID;
return sendPacket(CHANNEL_CONTROL);
}
#endif
#ifdef USE_REORIENT
// See 6.4.3.1 Tare Now (0x00)
HAL_StatusTypeDef BNO_TareNow(const BNO_TareAxis_t axis, const BNO_TareRV_t vector) {
// from SH-2 6.4.3 Tare (0x03)
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_TARE; // 0x03 – Tare command
//bufferIO[7] = 0; // 0x00 – Subcommand: Perform Tare now
// Bitmap of axes to tare: Bit 0= X, Bit 1= Y, Bit 2= Z 7 all
bufferIO[8] = axis;
//Which rotation vector to use as the basis for Tare adjustment.
//Rotation Vector to use as basis for tare.
//0: Rotation Vector
//1: Gaming Rotation Vector
//2: Geomagnetic Rotation Vector
//3: Gyro-Integrated Rotation Vector
//4: ARVR-Stabilized Rotation Vector
//5: ARVR-Stabilized Game Rotation Vector
bufferIO[9] = vector; //
return sendPacket(CHANNEL_CONTROL);
}
//This command instructs SH-2 to persist the results of the last tare operation to flash for use at the next system restart.
HAL_StatusTypeDef BNO_TarePerist(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_TARE; // 0x03 – Tare command
bufferIO[7] = COMMAND_TARE_PERSIST; // 0x01 – Persist Tare
return sendPacket(CHANNEL_CONTROL);
}
// This command instructs SH-2 to set the current run-time sensor reorientation.
// The rotation vector is a signed, 16-bit 2’s-complement fixed point number with a Q-point of 14.
HAL_StatusTypeDef BNO_TareSetReorientation(const double X, const double Y, const double Z, const double W) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_TARE; // 0x03 – Tare command
bufferIO[7] = COMMAND_TARE_REORIENT; // 0x02 – Set Reorientation
*(int16_t *)&bufferIO[8] = (int16_t)(X * SCALE_TO_Q14);
*(int16_t *)&bufferIO[10] = (int16_t)(Y * SCALE_TO_Q14);
*(int16_t *)&bufferIO[12] = (int16_t)(Z * SCALE_TO_Q14);
*(int16_t *)&bufferIO[14] = (int16_t)(W * SCALE_TO_Q14);
return sendPacket(CHANNEL_CONTROL);
}
// Clears the previously applied tare operation.
HAL_StatusTypeDef BNO_ClearTare(void){
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_TARE; // 0x03 – Tare command
bufferIO[7] = COMMAND_TARE_REORIENT; // 0x02 – Set Reorientation
return sendPacket(CHANNEL_CONTROL);
}
#endif
// The sensor hub responds to the Initialize command with an Initialize Response. In the case
// where the sensor hub reinitializes itself, this response is unsolicited. An unsolicited response is
// also generated after startup
HAL_StatusTypeDef BNO_Initialize(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_INITIALIZE; // 0x04 – Initialize command
bufferIO[7] = COMMAND_INITIALIZE_RESET; //1 Reinitialize the entire sensor hub.
return waitForCommandResponse();
}
// Save Dynamic Calibration Data (DCD) to flash
HAL_StatusTypeDef BNO_saveCalibration(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_SAVE_DCD; // 0x06 – Save DCD Command
saveDcdStatus = 1; // Set it as non 0
if(waitForCommandResponse() == HAL_OK) {
if(!saveDcdStatus) return HAL_OK;
}
return HAL_ERROR;
}
// This command is sent by the host to configure the ME calibration of the accelerometer, gyro and
// magnetometer giving the host the ability to control when calibration is performed.
HAL_StatusTypeDef BNO_condigureCalibration(const uint8_t sensors) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_ME_CALIBRATE; // 0x07 – ME Calibration Command
//bufferIO[10] = 0; // 0x00 – Subcommand: Configure ME Calibration
// Make the internal calStatus variable non-zero (fail)
calibrationStatus.Status = 1;
switch(sensors) {
case CALIBRATE_ACCEL:
bufferIO[7] = 1; // Accel Cal Enable (1 – enabled, 0 – disabled)
break;
case CALIBRATE_GYRO:
bufferIO[8] = 1; // Gyro Cal Enable (1 – enabled, 0 – disabled)
break;
case CALIBRATE_MAG:
bufferIO[9] = 1; // Mag Cal Enable (1 – enabled, 0 – disabled)
break;
case CALIBRATE_PLANAR_ACCEL:
bufferIO[11] = 1; // Planar Accel Cal Enable (1 – enabled, 0 – disabled)
break;
case CALIBRATE_ON_TABLE:
bufferIO[12] = 1; // On Table Cal Enable (1 – enabled, 0 – disabled)
break;
case CALIBRATE_ACCEL_GYRO_MAG:
bufferIO[7] = bufferIO[8] = bufferIO[9] = 1;
break;
case CALIBRATE_ALL:
bufferIO[7] = bufferIO[8] = bufferIO[9] = bufferIO[11] = bufferIO[12] = 1;
break;
}
bufferIO[13] = ((sensors & 0x60) >> 5);
if(waitForCommandResponse() == HAL_OK) {
if(!calibrationStatus.Status) {
return HAL_OK;
}
}
return HAL_ERROR;
}
// This command is sent by the host to request the enable/disable state of the accelerometer, gyro and magnetometer calibration routines
HAL_StatusTypeDef BNO_enableCalibration(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_ME_CALIBRATE;
bufferIO[10] = COMMAND_ME_CALIBRATE_GET;
return waitForCommandResponse();
}
// The Configure Periodic DCD Save command configures the automatic saving of DCD. There is
// no response to this command. This command does not inhibit the Save DCD command.
// 0 Enable, 1 Disable
HAL_StatusTypeDef BNO_configurePeriodicDcdSave(const uint8_t enableStatus) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_DCD_PERIOD_SAVE; // 0x09 Configure Periodic DCD Save
bufferIO[7] = enableStatus;
return sendPacket(CHANNEL_CONTROL);
}
// The Get Oscillator Type command is used to get information about the oscillator type used in the clock system of the SH-2
BNO_Oscillator_t BNO_getOscillatorType(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_OSCILLATOR;
if(waitForCommandResponse() == HAL_OK) {
if(oscillatorType > ExternalClock) return OscillatorError;
}
return oscillatorType;
}
// This command performs an atomic clearDCD (from RAM) and system reset
HAL_StatusTypeDef BNO_clearDcdReset(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_CLEAR_DCD_RESET;
if(waitForCommandResponse() == HAL_OK) {
if(resetOccurred) {
resetOccurred = 0;
return HAL_OK;
}
}
return HAL_ERROR;
}
// ToDo Check functionality Not getting a reponce to start
// See 6.4.10 Simple Calibration Commands (0x0C)
HAL_StatusTypeDef BNO_simpleCalibration(const uint32_t usInterval, const uint16_t calibrationTimeMs) {
if(BNO_condigureCalibration(CALIBRATE_ON_TABLE) == HAL_OK) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_TURNTABLE_CAL; // 0x0C – Turntable Calibration
//bufferIO[7] = 0x00; // 0x00 – Start Calibration
*(uint32_t *)&bufferIO[8] = usInterval;
if(waitForCommandResponse() == HAL_OK) {//CHANNEL_CONTROL, REPORT_COMMAND_RESPONSE
if(!calibrationStatus.Status) {
uint32_t endCalibrationTime = HAL_GetTick() + calibrationTimeMs;
while(HAL_GetTick() < endCalibrationTime) {} // Just wait
// Stop the calibration and get response
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_TURNTABLE_CAL; // 0x0C – Turntable Calibration
bufferIO[7] = 0x01; // 0x01 – Finish Calibration
if(waitForCommandResponse() == HAL_OK) {
if(!calibrationStatus.Status) {
return HAL_OK;
}
}
}
}
}
return HAL_ERROR;
}
// ToDo Check functionality Not getting a reponce to start
// The bootloader operating mode request is used to request various operating modes of the FSP200 bootloader
HAL_StatusTypeDef BNO_setBootMode(const BNO_BootMode_t mode) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_BOOTLOADER; // 0x0D – Bootloader command
bufferIO[7] = COMMAND_BOOTLOADER_MODE_REQ; // 0x00 – Sub-command: Bootloader Operating Mode Request
bufferIO[8] = mode; // Bootloader Operating Mode ID
return sendPacket(CHANNEL_CONTROL);
}
// ToDo Check functionality Not getting a reponce to start
// Request product ID information about the FSP200 bootloader
BNO_BootMode_t BNO_getBootMode(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_BOOTLOADER; // 0x0D – Bootloader command
bufferIO[7] = COMMAND_BOOTLOADER_STATUS_REQ; // 0x01 – Sub-command: Bootloader Status Request
if(waitForCommandResponse() == HAL_OK) {
return bootLoader.OperationMode;
}
return BootInvalid;
}
// The interactive calibration feature requires that the sensor hub be told of the device’s intended motion.
HAL_StatusTypeDef BNO_interactiveCalibration(const BNO_MotionIntent_t intent) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_INTERACTIVE_CAL_REQ; // 0x0E – Interactive Calibration command
bufferIO[7] = intent; // Motion intent
return sendPacket(CHANNEL_CONTROL);
}
#ifdef WHEEL_ENCODER
// Provide a single sample of wheel encoder data. No response is sent for this command.
HAL_StatusTypeDef BNO_WheelRequest(const uint8_t wheelIndex, const uint32_t timeStampUs, const int16_t wheelData, const uint8_t dataType) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_WHEEL_REQ; // 0x0F – Wheel Request
bufferIO[7] = wheelIndex; // Wheel Index (0 = Left Wheel, 1 = Right Wheel)
*(uint32_t *)&bufferIO[8] = timeStampUs; // The timestamp is a 32-bit unsigned timestamp in microseconds.
*(int16_t *)&bufferIO[12] = wheelData; // The timestamp is a 32-bit unsigned timestamp in microseconds.
bufferIO[14] = dataType; // Data Type (0 = Position, 1 = Velocity)
return sendPacket(CHANNEL_CONTROL);
}
#endif
// This is sent from the host to the hub to trigger a flush of outstanding data from a given sensor
HAL_StatusTypeDef BNO_forceFlush(const uint8_t sensorID) {
resetHeader(REPORT_SENSOR_FLUSH_REQUEST);
bufferIO[5] = sensorID;
return waitForCommandResponse();
}
// This command is sent by the host to request the enable/disable state of the accelerometer, gyro
// and magnetometer calibration routines.
HAL_StatusTypeDef BNO_getCalibrationStatus(void) {
resetHeader(REPORT_COMMAND_REQUEST);
bufferIO[5] = cmdSeqNo++;
bufferIO[6] = COMMAND_ME_CALIBRATE;
bufferIO[10] = 0x01; // 0x01 – Subcommand: Get ME Calibration
return waitForCommandResponse();
}
// Check if calibration is complete
HAL_StatusTypeDef BNO_isCalibrationComplete(void) {
if(!calibrationStatus.Status) {
return HAL_OK;
}
return HAL_ERROR;
}
#ifdef USE_FOR_TELESCOPE
HAL_StatusTypeDef setTelescopeOrientation(void) {
if(BNO_TareNow(TARE_ALL, RotationVector) == HAL_OK){
// -X North, Y East Z down
if(BNO_TareSetReorientation(0.0, 0.0, 0.0, Q_FLIP) == HAL_OK){
// Save new orientation and reset
if(BNO_TarePerist() == HAL_OK){
return BNO_Reset();
}
}
}
return HAL_ERROR;
}
// Start the calibration for 20s or until accuracy is 3
HAL_StatusTypeDef BNO_calibrateHighAccuracyAndReset(void) {
const uint16_t calibrationTime = 20000;
if(BNO_condigureCalibration(CALIBRATE_ACCEL_GYRO_MAG) == HAL_OK) {
if(BNO_setFeature(MAGNETIC_FIELD_CALIBRATED, 100000, 0) == HAL_OK) {
if(BNO_setFeature(GAME_ROTATION_VECTOR, 100000, 0) == HAL_OK) {
uint32_t startTime = HAL_GetTick() + calibrationTime;
uint8_t magA,grvA = 0;
while(HAL_GetTick() < startTime) {
if((BNO_dataAvailable() == HAL_OK) && sensorData.sensorId){
if(sensorData.sensorId == MAGNETIC_FIELD_CALIBRATED){
magA = sensorData.status;
printf("MagA=%d\r\n", magA);
}
if(sensorData.sensorId == GAME_ROTATION_VECTOR){
grvA = sensorData.status;
printf("GrvA=%d\r\n", grvA);
}
sensorData.sensorId = 0;
}
// If we have maximum accuracy we stop
if((magA == 3) && (grvA == 3))
break;
}
if(BNO_saveCalibration() == HAL_OK){
return BNO_Reset();
}
}
}
}
return HAL_ERROR;
}
#endif
// Check if we have new data
HAL_StatusTypeDef BNO_dataAvailable(void) {
if(waitForPacket() == HAL_OK) {
return processResponse();
}
return HAL_ERROR;
}
// Enable features an set report time in mili seconds
BNO_Feature_t BNO_getFeature(const uint8_t sensorID) {
resetHeader(REPORT_GET_FEATURE_REQUEST);
BNO_Feature_t ret = {0};
bufferIO[5] = sensorID; // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
if(waitForCommandResponse() == HAL_OK) {
ret = sensorFeartures;
}
return ret;
}
// Enable features an set report time in mili seconds
HAL_StatusTypeDef BNO_setFeature(const uint8_t sensorID, const uint32_t microsBetweenReports, const uint32_t specificConfig) {
resetHeader(REPORT_SET_FEATURE_COMMAND); // Set feature command. Reference page 55
// sensorFeartures.sensorID = sensorID;
// sensorFeartures.flags = 0;
// sensorFeartures.changeSensitivity = 0;
// sensorFeartures.reportInterval_uS = microsBetweenReports;
// sensorFeartures.batchInterval_uS = 0;
// sensorFeartures.sensorSpecific = specificConfig;
// *(BNO_Feature_t *)&bufferIO[5] = sensorFeartures;
bufferIO[5] = sensorID; // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
//bufferIO[6] = 0; // Feature flags
//*(uint16_t *)&bufferIO[7] = 0; // Change sensitivity [absolute | relative]
*(uint32_t *)&bufferIO[9] = microsBetweenReports; // Report interval (LSB) in microseconds
//*(uint32_t *)&bufferIO[13] = 0; // Batch Interval
*(uint32_t *)&bufferIO[17] = specificConfig; // Sensor-specific config
//Transmit packet on channel 2, 17 bytes
return sendPacket(CHANNEL_CONTROL);
}
//
BNO_RollPitchYaw_t BNO_getRollPitchYaw(void) {
return rpy;
}
#ifdef RAW_ACCELEROMETER
BNO_RawAccelerometer_t getRawAccelerometer(void) {
return sensorData.SenVal.RawAccelerometer;
}
#endif
#ifdef ACCELEROMETER
BNO_Accelerometer_t getaccelerometer(void) {
return sensorData.SenVal.Accelerometer;
}
#endif
#ifdef LINEAR_ACCELERATION
BNO_Accelerometer_t getLinearAcceleration(void) {
return sensorData.SenVal.LinearAcceleration;
}
#endif
#ifdef GRAVITY
BNO_Accelerometer_t getGravity(void) {
return sensorData.SenVal.Gravity;
}
#endif
#ifdef RAW_GYROSCOPE
BNO_RawGyroscope_t getRawGyroscope(void) {
return sensorData.SenVal.RawGyroscope;
}
#endif
#ifdef GYROSCOPE_CALIBRATED
BNO_Gyroscope_t getGyroscope(void) {
return sensorData.SenVal.Gyroscope;
}
#endif
#ifdef GYROSCOPE_UNCALIBRATED
BNO_GyroscopeUncalibrated_t getGyroscopeUncal(void) {
return sensorData.SenVal.GyroscopeUncal;
}
#endif
#ifdef RAW_MAGNETOMETER
BNO_RawMagnetometer_t getRawMagnetometer(void) {
return sensorData.SenVal.RawMagnetometer;
}
#endif
#ifdef MAGNETIC_FIELD_CALIBRATED
BNO_MagneticField_t getMagneticField(void) {
return sensorData.SenVal.MagneticField;
}
#endif
#ifdef MAGNETIC_FIELD_UNCALIBRATED
BNO_MagneticFieldUncalibrated_t getMagneticFieldUncal(void) {
return sensorData.SenVal.MagneticFieldUncal;
}
#endif
#ifdef ROTATION_VECTOR
BNO_RotationVectorWAcc_t getRotationVector(void) {
return sensorData.SenVal.RotationVector;
}
#endif
#ifdef GAME_ROTATION_VECTOR
BNO_RotationVector_t getGameRotationVector(void) {
return sensorData.SenVal.GameRotationVector;
}
#endif
#ifdef GEOMAGNETIC_ROTATION_VECTOR
BNO_RotationVectorWAcc_t getGeoMagRotationVector(void) {
return sensorData.SenVal.GeoMagRotationVector;
}
#endif
#ifdef PRESSURE
float getPressure(void) {
return sensorData.SenVal.Pressure; // Atmospheric Pressure. [hectopascals]
}
#endif
#ifdef AMBIENT_LIGHT
float getAmbientLight(void) {
return sensorData.SenVal.AmbientLight; // Ambient Light. [lux]
}
#endif
#ifdef HUMIDITY
float getHumidity(void) {
return sensorData.SenVal.Humidity; // Relative Humidity. [percent]
}
#endif
#ifdef PROXIMITY
float getProximity(void) {
return sensorData.SenVal.Proximity; // Proximity. [cm]
}
#endif
#ifdef TEMPERATURE
float getTemperature(void) {
return sensorData.SenVal.Temperature; // Temperature. [C]
}
#endif
#ifdef RESERVED
float getReserved(void) {
return sensorData.SenVal.Reserved; // Reserved
}
#endif
#ifdef TAP_DETECTOR
BNO_Tap_t getTapDetectorFlag(void) {
return sensorData.SenVal.TapDetectorFlag;
}
#endif
#ifdef STEP_DETECTOR
uint32_t getStepDetectorLatency(void) {
return sensorData.SenVal.StepDetectorLatency; // Step detect latency [uS]
}
#endif
#ifdef STEP_COUNTER
BNO_StepCounter_t getStepCounter(void) {
return sensorData.SenVal.StepCounter;
}
#endif
#ifdef SIGNIFICANT_MOTION
uint16_t getSignificantMotion(void) {
return sensorData.SenVal.SignificantMotion;
}
#endif
#ifdef STABILITY_CLASSIFIER
BNO_Stability_t getStabilityClassifier(void) {
return sensorData.SenVal.StabilityClassifier;
}
#endif
#ifdef SHAKE_DETECTOR
BNO_Shake_t getShakeDetector(void) {
return sensorData.SenVal.ShakeDetector;
}
#endif
#ifdef FLIP_DETECTOR
uint16_t getFlipDetector(void) {
return sensorData.SenVal.FlipDetector;
}
#endif
#ifdef PICKUP_DETECTOR
BNO_Pickup_t getPickupDetector(void) {
return sensorData.SenVal.PickupDetector;
}
#endif
#ifdef STABILITY_DETECTOR
BNO_StabilityDetector_t getStabilityDetector(void) {
return sensorData.SenVal.StabilityDetector;
}
#endif
#ifdef PERSONAL_ACTIVITY_CLASSIFIER
BNO_PersonalActivityClassifier_t getPersonalActivityClassifier(void) {
return sensorData.SenVal.PersonalActivityClassifier;
}
#endif
#ifdef SLEEP_DETECTOR
uint8_t getSleepDetector(void) {
return sensorData.SenVal.SleepDetector;
}
#endif
#ifdef TILT_DETECTOR
uint16_t getTiltDetector(void) {
return sensorData.SenVal.TiltDetector;
}
#endif
#ifdef POCKET_DETECTOR
uint16_t getPocketDetector(void) {
return sensorData.SenVal.PocketDetector;
}
#endif
#ifdef CIRCLE_DETECTOR
uint16_t getCircleDetector(void) {
return sensorData.SenVal.CircleDetector;
}
#endif
#ifdef HEART_RATE_MONITOR
uint16_t getHeartRateMonitor(void) {
return sensorData.SenVal.HeartRateMonitor; // Heart rate in beats per minute.
}
#endif
#ifdef ARVR_STABILIZED_RV
BNO_RotationVectorWAcc_t BNO_getArvrStabilizedRV(void) {
return sensorData.SenVal.ArVrStabilizedRV;
}
#endif
#ifdef ARVR_STABILIZED_GRV
BNO_RotationVector_t BNO_getArVrStabilizedGRV(void) {
return sensorData.SenVal.ArVrStabilizedGRV;
}
#endif
#ifdef GYRO_INTEGRATED_RV
BNO_GyroIntegratedRV_t BNO_getGyroIntegratedRV(void) {
return sensorData.SenVal.GyroIntegratedRV;
}
#endif
#ifdef IZRO_MOTION_REQUEST
BNO_IZroRequest_t getIzroRequest(void) {
return sensorData.SenVal.IzroRequest;
}
#endif
#ifdef RAW_OPTICAL_FLOW
BNO_RawOptFlow_t getRawOptFlow(void) {
return sensorData.SenVal.RawOptFlow;
}
#endif
#ifdef DEAD_RECKONING_POSE
BNO_DeadReckoningPose_t getDeadReckoningPose(void) {
return sensorData.SenVal.DeadReckoningPose;
}
#endif
#ifdef WHEEL_ENCODER
BNO_WheelEncoder_t getWheelEncoder(void) {
return sensorData.SenVal.WheelEncoder;
}
#endif
Comments powered by CComment