BLE live imu data sketch

lab11 · Jun 19, 2024 · d6b3548 · d6b3548
1 parent 1758765
commit d6b3548
Show file tree

Hide file tree

Showing 8 changed files with 71 additions and 3 deletions.
diff --git a/software/firmware/src/app/app_config.h b/software/firmware/src/app/app_config.h
@@ -89,6 +89,7 @@ typedef enum { BATTERY_EMPTY = 3500, BATTERY_CRITICAL = 3680, BATTERY_NOMINAL =
 #define BLE_LIVE_STATS_FINDMYTOTTAG_CHAR            0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x55,0x31,0x8c,0xd6
 #define BLE_LIVE_STATS_RANGING_CHAR                 0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x56,0x31,0x8c,0xd6
 #define BLE_LIVE_STATS_ADDRESS_CHAR                 0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x57,0x31,0x8c,0xd6
+#define BLE_LIVE_STATS_IMU_DATA_CHAR                0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x58,0x31,0x8c,0xd6
 #define BLE_SCHEDULING_SERVICE_ID                   0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x5A,0x31,0x8c,0xd6
 #define BLE_SCHEDULING_REQUEST_CHAR                 0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x5B,0x31,0x8c,0xd6
 #define BLE_MAINTENANCE_SERVICE_ID                  0x2e,0x5d,0x5e,0x39,0x31,0x52,0x45,0x0c,0x90,0xee,0x3f,0xa2,0x60,0x31,0x8c,0xd6

diff --git a/software/firmware/src/peripherals/include/bluetooth.h b/software/firmware/src/peripherals/include/bluetooth.h
@@ -33,5 +33,6 @@ bool bluetooth_is_scanning(void);
 bool bluetooth_is_connected(void);
 void bluetooth_clear_whitelist(void);
 void bluetooth_add_device_to_whitelist(uint8_t* uid);
+void bluetooth_write_imu_data(const uint8_t *results, uint16_t results_length);
 
 #endif  // #ifndef __BLUETOOTH_HEADER_H__
diff --git a/software/firmware/src/peripherals/src/bluetooth.c b/software/firmware/src/peripherals/src/bluetooth.c
@@ -17,7 +17,7 @@
 // Static Global Variables ---------------------------------------------------------------------------------------------
 
 static volatile uint16_t connection_mtu;
-static volatile bool is_scanning, is_advertising, is_connected, ranges_requested, data_requested;
+static volatile bool is_scanning, is_advertising, is_connected, ranges_requested, data_requested, imu_data_requested;
 static volatile bool expected_scanning, expected_advertising, is_initialized, first_initialization;
 static volatile uint8_t adv_data_conn[HCI_ADV_DATA_LEN], scan_data_conn[HCI_ADV_DATA_LEN], current_ranging_role[3];
 static const uint8_t adv_data_flags[] = { DM_FLAG_LE_GENERAL_DISC | DM_FLAG_LE_BREDR_NOT_SUP };
@@ -58,6 +58,7 @@ enum
 {
    TOTTAG_GATT_SERVICE_CHANGED_CCC_IDX,
    TOTTAG_RANGING_CCC_IDX,
+   TOTTAG_IMU_DATA_CCC_IDX,
    TOTTAG_MAINTENANCE_RESULT_CCC_IDX,
    TOTTAG_NUM_CCC_CHARACTERISTICS
 };
@@ -66,6 +67,7 @@ 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 },
+   { IMU_DATA_CCC_HANDLE,                ATT_CLIENT_CFG_NOTIFY,  DM_SEC_LEVEL_NONE },
    { MAINTENANCE_RESULT_CCC_HANDLE,    ATT_CLIENT_CFG_INDICATE,  DM_SEC_LEVEL_NONE }
 };
 
@@ -140,7 +142,7 @@ static void deviceManagerCallback(dmEvt_t *pDmEvt)
          break;
       case DM_CONN_CLOSE_IND:
          print("TotTag BLE: deviceManagerCallback: Received DM_CONN_CLOSE_IND\n");
-         is_connected = ranges_requested = data_requested = false;
+         is_connected = ranges_requested = data_requested = imu_data_requested = false;
          AttsCccClearTable(pDmEvt->hdr.param);
          bluetooth_start_advertising();
          break;
@@ -225,6 +227,8 @@ 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);
+   else if (pEvt->idx == TOTTAG_IMU_DATA_CCC_IDX)
+      imu_data_requested = (pEvt->value == ATT_CLIENT_CFG_NOTIFY);
    else if (pEvt->idx == TOTTAG_MAINTENANCE_RESULT_CCC_IDX)
       data_requested = (pEvt->value == ATT_CLIENT_CFG_INDICATE);
 }
@@ -238,7 +242,7 @@ void bluetooth_init(uint8_t* uid)
    const uint8_t ranging_role[] = { BLUETOOTH_COMPANY_ID, 0x00 };
    memcpy((uint8_t*)current_ranging_role, ranging_role, sizeof(ranging_role));
    data_requested = expected_scanning = expected_advertising = is_initialized = false;
-   is_scanning = is_advertising = is_connected = ranges_requested = false;
+   is_scanning = is_advertising = is_connected = ranges_requested = imu_data_requested = false;
    first_initialization = true;
    discovery_callback = NULL;
 
@@ -371,6 +375,12 @@ void bluetooth_write_range_results(const uint8_t *results, uint16_t results_leng
       updateRangeResults(AppConnIsOpen(), results, results_length);
 }
 
+void bluetooth_write_imu_data(const uint8_t *results, uint16_t results_length){
+   //TODO
+   if (imu_data_requested)
+      updateIMUData(AppConnIsOpen(), results, results_length);
+}
+
 void bluetooth_start_advertising(void)
 {
    // Attempt to begin advertising

diff --git a/software/firmware/src/tasks/app_task_ranging.c b/software/firmware/src/tasks/app_task_ranging.c
@@ -195,6 +195,7 @@ static void motion_change_handler(bool in_motion)
 static void imu_burst_data_handler(uint8_t *localBuffer)
 {
    //TODO
+   //bluetooth_write_imu_data(localBuffer, 38);
 }
 
 static void ble_discovery_handler(const uint8_t ble_address[EUI_LEN], uint8_t ranging_role)

diff --git a/software/firmware/src/tasks/bluetooth/live_stats_functionality.c b/software/firmware/src/tasks/bluetooth/live_stats_functionality.c
@@ -38,3 +38,10 @@ void updateRangeResults(dmConnId_t connId, const uint8_t *results, uint16_t resu
    if (connId != DM_CONN_ID_NONE)
       AttsHandleValueNtf(connId, RANGES_HANDLE, results_length, (uint8_t*)results);
 }
+
+void updateIMUData(dmConnId_t connId, const uint8_t *results, uint16_t results_length)
+{
+   // Update the imu data characteristic
+   if (connId != DM_CONN_ID_NONE)
+      AttsHandleValueNtf(connId, IMU_DATA_HANDLE, results_length, (uint8_t*)results);
+}
diff --git a/software/firmware/src/tasks/bluetooth/live_stats_functionality.h b/software/firmware/src/tasks/bluetooth/live_stats_functionality.h
@@ -6,5 +6,6 @@
 uint8_t handleLiveStatsRead(dmConnId_t connId, uint16_t handle, uint8_t operation, uint16_t offset, attsAttr_t *pAttr);
 uint8_t handleLiveStatsWrite(dmConnId_t connId, uint16_t handle, uint8_t operation, uint16_t offset, uint16_t len, uint8_t *pValue, attsAttr_t *pAttr);
 void updateRangeResults(dmConnId_t connId, const uint8_t *results, uint16_t results_length);
+void updateIMUData(dmConnId_t connId, const uint8_t *results, uint16_t results_length);
 
 #endif  // #ifndef __LIVE_STATS_FUNCTIONALITY_HEADER_H__
diff --git a/software/firmware/src/tasks/bluetooth/live_stats_service.c b/software/firmware/src/tasks/bluetooth/live_stats_service.c
@@ -32,6 +32,7 @@ static uint32_t findMyTottagDuration = 0;
 static const uint16_t findMyTottagDurationLen = sizeof(findMyTottagDuration);
 static const uint8_t findMyTottagDesc[] = "FindMyTottagRequest";
 static const uint16_t findMyTottagDescLen = sizeof(findMyTottagDesc);
+
 static const uint8_t rangingChUuid[] = { BLE_LIVE_STATS_RANGING_CHAR };
 static const uint8_t rangesChar[] = { ATT_PROP_NOTIFY, UINT16_TO_BYTES(RANGES_HANDLE), BLE_LIVE_STATS_RANGING_CHAR };
 static const uint16_t rangesCharLen = sizeof(rangesChar);
@@ -42,6 +43,16 @@ static const uint16_t rangesDescLen = sizeof(rangesDesc);
 static uint8_t rangesCcc[] = { UINT16_TO_BYTES(0x0000) };
 static const uint16_t rangesCccLen = sizeof(rangesCcc);
 
+static const uint8_t imuDataChUuid[] = { BLE_LIVE_STATS_IMU_DATA_CHAR };
+static const uint8_t imuDataChar[] = { ATT_PROP_NOTIFY, UINT16_TO_BYTES(IMU_DATA_HANDLE), BLE_LIVE_STATS_IMU_DATA_CHAR };
+static const uint16_t imuDataCharLen = sizeof(imuDataChar);
+static uint8_t imuData[40] = { 0 };
+static const uint16_t imuDataLen = sizeof(imuData);
+static const uint8_t imuDataDesc[] = "LiveIMUData";
+static const uint16_t imuDataDescLen = sizeof(imuDataDesc);
+static uint8_t imuDataCcc[] = { UINT16_TO_BYTES(0x0001) };
+static const uint16_t imuDataCccLen = sizeof(imuDataCcc);
+
 static const attsAttr_t liveStatsList[] =
 {
    {
@@ -155,6 +166,38 @@ static const attsAttr_t liveStatsList[] =
       sizeof(rangesCcc),
       ATTS_SET_CCC,
       (ATTS_PERMIT_READ | ATTS_PERMIT_WRITE)
+   },
+   {
+      attChUuid,
+      (uint8_t*)imuDataChar,
+      (uint16_t*)&imuDataCharLen,
+      sizeof(imuDataChar),
+      0,
+      ATTS_PERMIT_READ
+   },
+   {
+      imuDataChUuid,
+      (uint8_t*)imuData,
+      (uint16_t*)&imuDataLen,
+      sizeof(imuData),
+      (ATTS_SET_UUID_128 | ATTS_SET_VARIABLE_LEN),
+      0
+   },
+   {
+      attChUserDescUuid,
+      (uint8_t*)imuDataDesc,
+      (uint16_t*)&imuDataDescLen,
+      sizeof(imuDataDesc),
+      0,
+      ATTS_PERMIT_READ
+   },
+   {
+      attCliChCfgUuid,
+      (uint8_t*)imuDataCcc,
+      (uint16_t*)&imuDataCccLen,
+      sizeof(imuDataCcc),
+      ATTS_SET_CCC,
+      (ATTS_PERMIT_READ | ATTS_PERMIT_WRITE)
    }
 };
 

diff --git a/software/firmware/src/tasks/bluetooth/live_stats_service.h b/software/firmware/src/tasks/bluetooth/live_stats_service.h
@@ -19,6 +19,10 @@ enum
    RANGES_HANDLE,                           // Current ranges
    RANGES_DESC_HANDLE,                      // Current ranges description
    RANGES_CCC_HANDLE,                       // Current ranges CCCD
+   IMU_DATA_CHAR_HANDLE,                    // Current imu data characteristic
+   IMU_DATA_HANDLE,                         // Current imu data
+   IMU_DATA_DESC_HANDLE,                    // Current imu data description
+   IMU_DATA_CCC_HANDLE,                     // Current imu data CCCD
    LIVE_STATS_MAX_HANDLE                    // Maximum live statistics handle
 };
-Original file line number
+Diff line change
@@ Expand Up / @@ -195,6 +195,7 @@ static void motion_change_handler(bool in_motion) @@
     static void imu_burst_data_handler(uint8_t *localBuffer)
     {
        //TODO
+       //bluetooth_write_imu_data(localBuffer, 38);
     }
     static void ble_discovery_handler(const uint8_t ble_address[EUI_LEN], uint8_t ranging_role)
@@ Expand Down @@