enable picking data from the multi-byte burst read

software/firmware/src/peripherals/include/imu.h

@@ -15,6 +15,13 @@
 #define BURST_READ_BASE_ADDR BNO055_GYRO_DATA_X_LSB_ADDR
 #define BURST_READ_LEN 38
 
+#define GYRO_DATA_LEN 6
+#define ACC_DATA_LEN 6
+#define LACC_DATA_LEN 6
+#define GACC_DATA_LEN 6
+#define QUAT_DATA_LEN 8
+#define STAT_DATA_LEN 1
+
 typedef void (*motion_change_callback_t)(bool in_motion);
 #if NONBLOCKING
 typedef void (*data_ready_callback_t)(uint8_t *localBuffer);
@@ -185,6 +192,16 @@ typedef enum
 
 } bno055_reg_t;
 
+typedef enum
+{
+   GYRO_DATA,
+   ACC_DATA,
+   LACC_DATA,
+   GACC_DATA,
+   QUAT_DATA,
+   STAT_DATA,
+} bno055_data_type_t;
+
 typedef enum
 {
    OPERATION_MODE_CONFIG = 0X00,
@@ -310,6 +327,7 @@ bool imu_set_axis_remap(bno055_axis_remap_t remap);
 void imu_read_euler_data(bno055_euler_t *euler);
 bool imu_read_in_motion(void);
 void imu_read_burst_buffer(uint8_t *destBuffer);
+uint8_t imu_pick_data_from_burst_buffer(uint8_t *picked, uint8_t *full, bno055_data_type_t data_type);
 
 // Math utilities
 void quaternion_to_euler(bno055_quaternion_t quaternion, bno055_euler_t *euler);

software/firmware/src/peripherals/src/imu.c

@@ -39,9 +39,9 @@ void imu_iom_isr(void)
 
 static void nonblocking_read_complete(void *pCallbackCtxt, uint32_t transactionStatus)
 {
-   uint8_t localBuffer[BURST_READ_LEN] = {0};
-   imu_read_burst_buffer(localBuffer);
-   data_ready_callback(localBuffer);
+   uint8_t saved_buffer[BURST_READ_LEN] = {0};
+   imu_read_burst_buffer(saved_buffer);
+   data_ready_callback(saved_buffer);
 }
 
 #endif
@@ -526,6 +526,37 @@ bool imu_read_in_motion(void)
    return previously_in_motion;
 }
 
+uint8_t imu_pick_data_from_burst_buffer(uint8_t *picked, uint8_t *full, bno055_data_type_t data_type)
+{
+   //pick data from the continuous read data, and copy it at the start of the picked buffer
+   if (data_type == GYRO_DATA)
+   {
+      memcpy(picked, full+BNO055_GYRO_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR, GYRO_DATA_LEN);
+      return GYRO_DATA_LEN;
+   }
+   else if (data_type == LACC_DATA)
+   {
+      memcpy(picked, full+BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR, LACC_DATA_LEN);
+      return LACC_DATA_LEN;
+   }
+   else if (data_type == GACC_DATA)
+   {
+      memcpy(picked, full+BNO055_GRAVITY_DATA_X_LSB_ADDR-BURST_READ_BASE_ADDR, LACC_DATA_LEN);
+      return GACC_DATA_LEN;
+   }
+   else if (data_type == QUAT_DATA)
+   {
+      memcpy(picked, full+BNO055_QUATERNION_DATA_W_LSB_ADDR-BURST_READ_BASE_ADDR, QUAT_DATA_LEN);
+      return QUAT_DATA_LEN;
+   }
+   else if  (data_type == STAT_DATA)
+   {
+      memcpy(picked, full+BNO055_CALIB_STAT_ADDR-BURST_READ_BASE_ADDR, STAT_DATA_LEN);
+      return STAT_DATA_LEN;
+   }
+   return 0;
+}
+
 #if NONBLOCKING
 void imu_read_burst_buffer(uint8_t *destBuffer)
 {

software/firmware/src/tasks/app_task_ranging.c

@@ -192,13 +192,27 @@ static void motion_change_handler(bool in_motion)
    app_notify(APP_NOTIFY_MOTION_EVENT, true);
 }
 
-static void imu_burst_data_handler(uint8_t *localBuffer)
+static void imu_burst_data_handler(uint8_t *burst_data_buffer)
 {
    //TODO
 #ifdef _LIVE_IMU_DATA
-   bluetooth_write_imu_data(localBuffer, 38);
+   bluetooth_write_imu_data(burst_data_buffer, 38);
 #endif
-   storage_write_imu_data(app_get_experiment_time(0), localBuffer, 38);
+
+   uint8_t useful_imu_data[38] = {0};
+
+   //types of imu data to be saved
+   const bno055_data_type_t data_types[] = {STAT_DATA,LACC_DATA,GYRO_DATA};
+   uint8_t index = 0;
+   uint8_t len = 0;
+
+   for (uint8_t i = 0; i < sizeof(data_types)/sizeof(data_types[0]); i+=1)
+   {
+      len = imu_pick_data_from_burst_buffer(useful_imu_data+index, burst_data_buffer, data_types[i]);
+      index+= len;
+   }
+   storage_write_imu_data(app_get_experiment_time(0), useful_imu_data, index);
+   //storage_write_imu_data(app_get_experiment_time(0), burst_data_buffer, 38);
 }
 
 static void ble_discovery_handler(const uint8_t ble_address[EUI_LEN], uint8_t ranging_role)
@@ -277,8 +291,7 @@ void app_switch_mode(uint8_t command)
       //disable storage writing
       storage_disable(true);
       storage_enter_maintenance_mode();
-      //stop ranging and imu
-      scheduler_stop();
+      //stop imu
       imu_deinit();
    }
 }