Merge branch 'master' of https://github.com/lab11/socitrack

software/ersp/requirements.txt

@@ -12,7 +12,7 @@ pycryptodomex==3.19.1
 pylint==2.6.0
 pyparsing==2.4.7
 python-dateutil==2.8.1
-scikit-learn==1.0.1
+scikit-learn==1.5.0
 scipy==1.10.0
 six==1.15.0
 sklearn==0.0

software/firmware/src/app/app_config.h

@@ -35,8 +35,10 @@
 #define MAX_NUM_RANGING_DEVICES                     10
 #define COMPRESSED_RANGE_DATUM_LENGTH               (1 + sizeof(int16_t))       // EUI + Range
 #define MAX_COMPRESSED_RANGE_DATA_LENGTH            (1 + (COMPRESSED_RANGE_DATUM_LENGTH * MAX_NUM_RANGING_DEVICES))
+#define MAX_IMU_DATA_LENGTH                         40
 
 #define STORAGE_QUEUE_MAX_NUM_ITEMS                 24
+#define STORAGE_IMU_BUFFER_NUM_ITEM                 50
 
 #define BATTERY_CHECK_INTERVAL_S                    300
 
@@ -96,6 +98,7 @@ typedef enum { BATTERY_EMPTY = 3500, BATTERY_CRITICAL = 3680, BATTERY_NOMINAL =
 #define BLE_MAINTENANCE_EXPERIMENT_CHAR             0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x61,0x31,0x8c,0xd6
 #define BLE_MAINTENANCE_COMMAND_CHAR                0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x62,0x31,0x8c,0xd6
 #define BLE_MAINTENANCE_DATA_CHAR                   0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x63,0x31,0x8c,0xd6
+#define BLE_MODE_SWITCH_CHAR                        0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x64,0x31,0x8c,0xd6
 
 
 // Ranging Protocol Configuration --------------------------------------------------------------------------------------

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/include/storage.h

@@ -13,7 +13,8 @@ typedef enum {
    STORAGE_TYPE_VOLTAGE,
    STORAGE_TYPE_CHARGING_EVENT,
    STORAGE_TYPE_MOTION,
-   STORAGE_TYPE_RANGES
+   STORAGE_TYPE_RANGES,
+   STORAGE_TYPE_IMU,
 } storage_data_type_t;
 
 

software/firmware/src/peripherals/src/bluetooth.c

@@ -58,7 +58,9 @@ enum
 {
    TOTTAG_GATT_SERVICE_CHANGED_CCC_IDX,
    TOTTAG_RANGING_CCC_IDX,
+#ifdef _LIVE_IMU_DATA
    TOTTAG_IMU_DATA_CCC_IDX,
+#endif
    TOTTAG_MAINTENANCE_RESULT_CCC_IDX,
    TOTTAG_NUM_CCC_CHARACTERISTICS
 };
@@ -67,7 +69,9 @@ static const attsCccSet_t characteristicSet[TOTTAG_NUM_CCC_CHARACTERISTICS] =
 {
    { GATT_SERVICE_CHANGED_CCC_HANDLE,  ATT_CLIENT_CFG_INDICATE,  DM_SEC_LEVEL_NONE },
    { RANGES_CCC_HANDLE,                  ATT_CLIENT_CFG_NOTIFY,  DM_SEC_LEVEL_NONE },
+#ifdef _LIVE_IMU_DATA
    { IMU_DATA_CCC_HANDLE,                ATT_CLIENT_CFG_NOTIFY,  DM_SEC_LEVEL_NONE },
+#endif
    { MAINTENANCE_RESULT_CCC_HANDLE,    ATT_CLIENT_CFG_INDICATE,  DM_SEC_LEVEL_NONE }
 };
 
@@ -227,8 +231,10 @@ static void cccCallback(attsCccEvt_t *pEvt)
    print("TotTag BLE: cccCallback: index = %d, handle = %d, value = %d\n", pEvt->idx, pEvt->handle, pEvt->value);
    if (pEvt->idx == TOTTAG_RANGING_CCC_IDX)
       ranges_requested = (pEvt->value == ATT_CLIENT_CFG_NOTIFY);
+#ifdef _LIVE_IMU_DATA
    else if (pEvt->idx == TOTTAG_IMU_DATA_CCC_IDX)
       imu_data_requested = (pEvt->value == ATT_CLIENT_CFG_NOTIFY);
+#endif
    else if (pEvt->idx == TOTTAG_MAINTENANCE_RESULT_CCC_IDX)
       data_requested = (pEvt->value == ATT_CLIENT_CFG_INDICATE);
 }
@@ -464,7 +470,7 @@ void bluetooth_clear_whitelist(void)
 
 void bluetooth_add_device_to_whitelist(uint8_t* uid)
 {
-#ifndef _TEST_BLE_RANGING_TASK
+#ifndef _TEST_NO_EXP_DETAILS
    // Add the specified device to the whitelist
    DmDevWhiteListAdd(DM_ADDR_PUBLIC, uid);
    //DmDevSetFilterPolicy(DM_FILT_POLICY_MODE_ADV, HCI_ADV_FILT_CONN);

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
@@ -313,6 +313,7 @@ void imu_init(void)
 
    // Set up incoming interrupts from the IMU
    disable_motion_interrupts();
+   disable_data_ready_interrupts();
    uint32_t imu_interrupt_pin = PIN_IMU_INTERRUPT;
    configASSERT0(am_hal_gpio_pinconfig(PIN_IMU_INTERRUPT, am_hal_gpio_pincfg_input));
    configASSERT0(am_hal_gpio_interrupt_control(AM_HAL_GPIO_INT_CHANNEL_0, AM_HAL_GPIO_INT_CTRL_INDV_ENABLE, &imu_interrupt_pin));
@@ -330,6 +331,7 @@ void imu_deinit(void)
 {
    // Disable interrupts and put the device into suspend mode
    disable_motion_interrupts();
+   disable_data_ready_interrupts();
    enter_suspend_mode();
 
    // Disable all IMU-based interrupts
@@ -526,6 +528,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/peripherals/src/storage.c

@@ -8,7 +8,7 @@
 
 // Chip-Specific Definitions -------------------------------------------------------------------------------------------
 
-#if REVISION_ID != REVISION_APOLLO4_EVB && !defined(_TEST_BLE_RANGING_TASK)
+#if REVISION_ID != REVISION_APOLLO4_EVB && !defined(_TEST_NO_STORAGE)
 
 #define STORAGE_DEVICE_ID                           { 0xEF, 0xBA, 0x21 }
 
@@ -595,6 +595,10 @@ void storage_retrieve_experiment_details(experiment_details_t *details)
 
 void storage_begin_reading(uint32_t starting_timestamp)
 {
+#ifdef _DOWNLOAD_EVERYTHING
+   reading_page = (starting_page + 1) % BBM_LUT_BASE_ADDRESS;
+   is_reading = in_maintenance_mode;
+#else
    // Update the data reading details
    experiment_details_t details;
    storage_retrieve_experiment_details(&details);
@@ -629,6 +633,7 @@ void storage_begin_reading(uint32_t starting_timestamp)
          if (!found_valid_timestamp)
             reading_page = (reading_page + 1) % BBM_LUT_BASE_ADDRESS;
       }
+#endif
 }
 
 void storage_end_reading(void)
@@ -657,7 +662,9 @@ uint32_t storage_retrieve_num_data_chunks(uint32_t ending_timestamp)
    // Ensure that we are in reading mode
    if (!is_reading)
       return 0;
-
+#ifdef _DOWNLOAD_EVERYTHING
+   return (starting_page < current_page) ? (current_page - starting_page) : (BBM_LUT_BASE_ADDRESS - starting_page + current_page);
+#else
    if (ending_timestamp)
    {
       // Convert the ending timestamp to the appropriate format
@@ -697,6 +704,7 @@ uint32_t storage_retrieve_num_data_chunks(uint32_t ending_timestamp)
    else
       last_reading_page = current_page;
    return (reading_page <= last_reading_page) ? (1 + last_reading_page - reading_page) : (BBM_LUT_BASE_ADDRESS - reading_page + last_reading_page + 1);
+#endif
 }
 
 uint32_t storage_retrieve_next_data_chunk(uint8_t *buffer)
@@ -707,6 +715,15 @@ uint32_t storage_retrieve_next_data_chunk(uint8_t *buffer)
 
    // Determine if a full page of memory is available to read
    uint32_t num_bytes_retrieved = 0;
+#ifdef _DOWNLOAD_EVERYTHING
+   if (reading_page == current_page)
+   {
+      // Return the valid available bytes
+      memcpy(buffer, cache, cache_index);
+      num_bytes_retrieved = cache_index;
+      is_reading = false;
+   }
+#else
    if (reading_page == last_reading_page)
    {
       if (reading_page == current_page)
@@ -722,6 +739,7 @@ uint32_t storage_retrieve_next_data_chunk(uint8_t *buffer)
       }
       is_reading = false;
    }
+#endif
    else
    {
       // Read the next page of memory and update the reading metadata
@@ -752,4 +770,4 @@ uint32_t storage_retrieve_data_length(void) { return 0; }
 uint32_t storage_retrieve_next_data_chunk(uint8_t *buffer) { return 0; }
 uint32_t storage_retrieve_num_data_chunks(uint32_t) {return 0; }
 
-#endif  // #if REVISION_ID != REVISION_APOLLO4_EVB && !defined(_TEST_BLE_RANGING_TASK)
+#endif  // #if REVISION_ID != REVISION_APOLLO4_EVB && !defined(_TEST_NO_STORAGE)

software/firmware/src/tasks/app_task_ranging.c

@@ -223,10 +223,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
-   //bluetooth_write_imu_data(localBuffer, 38);
+#ifdef _LIVE_IMU_DATA
+   bluetooth_write_imu_data(burst_data_buffer, 38);
+#endif
+
+   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)
@@ -297,6 +314,19 @@ void app_activate_find_my_tottag(uint32_t seconds_to_activate)
       app_maintenance_activate_find_my_tottag(seconds_to_activate);
 }
 
+void app_switch_mode(uint8_t command)
+{
+   if (command==1)
+   {  //enable data downloading from ranging mode
+
+      //disable storage writing
+      storage_disable(true);
+      storage_enter_maintenance_mode();
+      //stop imu
+      imu_deinit();
+   }
+}
+
 void AppTaskRanging(void *uid)
 {
    // Store the UID and application task handle
@@ -315,18 +345,21 @@ void AppTaskRanging(void *uid)
 
    // Register handlers for motion detection, battery status changes, and BLE events
    bluetooth_register_discovery_callback(ble_discovery_handler);
-#ifndef _TEST_BLE_RANGING_TASK
+#ifndef _TEST_NO_BATTERY_CALLBACK
    if (battery_monitor_is_plugged_in())
       storage_flush_and_shutdown();
    else
    {
-      storage_write_motion_status(imu_read_in_motion());
       battery_register_event_callback(battery_event_handler);
-      imu_set_fusion_mode(OPERATION_MODE_ACCONLY);
-      imu_register_motion_change_callback(motion_change_handler);
    }
 #endif
 
+#if !defined(_TEST_NO_STORAGE) && !defined(_TEST_IMU_DATA)
+   storage_write_motion_status(imu_read_in_motion());
+   imu_set_fusion_mode(OPERATION_MODE_ACCONLY);
+   imu_register_motion_change_callback(motion_change_handler);
+#endif
+
 #ifdef _TEST_IMU_DATA
    //imu_register_motion_change_callback(motion_change_handler);
    imu_register_data_ready_callback(imu_burst_data_handler);

software/firmware/src/tasks/app_tasks.c

@@ -48,6 +48,29 @@ void run_tasks(void)
    ranging_radio_init(uid);
    ranging_radio_sleep(true);
 
+#ifdef _USE_DEFAULT_EXP_DETAILS
+      //only set immediately after flashing, not on reboot
+      if (!rtc_is_valid())
+      {
+         rtc_set_time_to_compile_time();
+         //default exp details
+         uint32_t current_timestamp = rtc_get_timestamp();
+         experiment_details_t details = {
+         .experiment_start_time = current_timestamp, .experiment_end_time = current_timestamp + 604800,
+         .daily_start_time = 1, .daily_end_time = 23,
+         .num_devices = 2, .uids = {}, .uid_name_mappings = {}
+         };
+         //new exp details can only be set in maintenance mode
+         storage_enter_maintenance_mode();
+         storage_store_experiment_details(&details);
+         if (!battery_monitor_is_plugged_in())
+         {
+         storage_exit_maintenance_mode();
+         }
+      }
+#endif
+
+
    // Determine whether there is an active experiment taking place
    static experiment_details_t scheduled_experiment;
    storage_retrieve_experiment_details(&scheduled_experiment);
@@ -102,7 +125,7 @@ void run_tasks(void)
    // Create tasks with the following priority order:
    //    IdleTask < TimeAlignedTask < AppTask < BLETask < RangingTask < StorageTask
    xTaskCreateStatic(StorageTask, "StorageTask", configMINIMAL_STACK_SIZE, allow_ranging ? uid : NULL, 5, storage_task_stack, &storage_task_tcb);
-#if !defined(_TEST_BLE_RANGING_TASK)
+#if !defined(_TEST_NO_EXP_DETAILS)
    xTaskCreateStatic(RangingTask, "RangingTask", configMINIMAL_STACK_SIZE, allow_ranging ? &scheduled_experiment : NULL, 4, ranging_task_stack, &ranging_task_tcb);
 #else 
    xTaskCreateStatic(RangingTask, "RangingTask", configMINIMAL_STACK_SIZE, allow_ranging ? uid : NULL, 4, ranging_task_stack, &ranging_task_tcb);

software/firmware/src/tasks/app_tasks.h

@@ -35,6 +35,7 @@ typedef struct __attribute__ ((__packed__))
 // Application Task Public Functions
 void app_notify(app_notification_t notification, bool from_isr);
 void app_activate_find_my_tottag(uint32_t seconds_to_activate);
+void app_switch_mode(uint8_t command);
 uint32_t app_get_experiment_time(int32_t offset);
 uint32_t app_experiment_time_to_rtc_time(uint32_t experiment_time);
 
@@ -47,6 +48,7 @@ void storage_flush_and_shutdown(void);
 void storage_write_battery_level(uint32_t battery_voltage_mV);
 void storage_write_motion_status(bool in_motion);
 void storage_write_ranging_data(uint32_t timestamp, const uint8_t *ranging_data, uint32_t ranging_data_len, int32_t timestamp_offset);
+void storage_write_imu_data(uint32_t timestamp, const uint8_t *imu_data, uint32_t imu_data_len);
 
 // Maintenance Task Public Functions
 void maintenance_get_requested_log_start_end_times(uint32_t *start_time, uint32_t *end_time);