STM32 I2C Library for BNO08x 9 axis IMU

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

Who’s online

We have 91 guests and no members online