basic orientation calculation and representing.

project/ble.c

@@ -83,6 +83,7 @@
 #include "nrf_temp.h"
 #include "ble_hts.h"
 #include "ble_bas.h"
+#include "arm_math.h"
 
 
 #define DEVICE_NAME                     "nRF_Aapo"                       /**< Name of device. Will be included in the advertising data. */
@@ -116,6 +117,8 @@
 // Determines if temperature type is given as characteristic (1) or as a field of measurement (0)
 #define TEMP_TYPE_AS_CHARACTERISTIC 0
 
+extern uint8_t orientation;
+uint8_t previous_orientation;
 
 NRF_BLE_GATT_DEF(m_gatt);                                                       /**< GATT module instance. */
 NRF_BLE_QWR_DEF(m_qwr);                                                         /**< Context for the Queued Write module.*/
@@ -129,7 +132,6 @@ BLE_BAS_DEF(m_bas); // Macro for defining a ble_bas instance
 
 // Flag to keep track of when an indication confirmation is pending
 bool m_hts_meas_ind_conf_pending = false;
-volatile uint32_t hts_counter; // hold dummy hts data
 
 // Function declarations
 void on_hts_evt(ble_hts_t * p_hts, ble_hts_evt_t * p_evt);
@@ -146,17 +148,16 @@ void advertising_start(bool erase_bonds);
 /*
 * Function for generating a dummy temperature information packet.
 */
-void generate_temperature(ble_hts_meas_t * p_meas) {
+void fetch_orientation(ble_hts_meas_t * p_meas) {
     ble_date_time_t time_stamp = { 2018, 16, 10, 16, 15, 0 };
 
     uint32_t celciusX100;
+    celciusX100 = orientation; // one unit is 0.01 Celcius
 
     p_meas->temp_in_fahr_units = false;
     p_meas->time_stamp_present = true;
     p_meas->temp_type_present = (TEMP_TYPE_AS_CHARACTERISTIC ? false : true);
 
-    celciusX100 = 2000+hts_counter++; // one unit is 0.01 Celcius
-
     p_meas->temp_in_celcius.exponent = -2;
     p_meas->temp_in_celcius.mantissa = celciusX100;
     p_meas->temp_in_fahr.exponent = -2;
@@ -199,7 +200,7 @@ void temperature_measurement_send(void) {
     err_code;
 
     if (!m_hts_meas_ind_conf_pending) {
-        generate_temperature(&hts_meas);
+        fetch_orientation(&hts_meas);
         err_code = ble_hts_measurement_send(&m_hts, &hts_meas);
 
         switch (err_code) {

project/imu.c

@@ -64,6 +64,14 @@
 #define NUM_TAPS 58
 #define BLOCK_SIZE 28
 
+extern uint8_t orientation;
+
+float32_t x_axis;
+float32_t y_axis;
+float32_t z_axis;
+float32_t x_axis_abs;
+float32_t y_axis_abs;
+float32_t z_axis_abs;
 
 // Declare a state array
 static float32_t firStateF32[BLOCK_SIZE + NUM_TAPS - 1];
@@ -77,9 +85,6 @@ float32_t acc_x_out_buf[140];
 float32_t acc_y_out_buf[140];
 float32_t acc_z_out_buf[140];
 
-int in_array[16];
-int out_array[16];
-
 int block_cnt = 0;
 
 //SPI instance
@@ -131,6 +136,52 @@ void compute_fir() {
   block_cnt = 0; // Reset block counting
 }
 
+void calculate_absolute_axis_values(){
+  x_axis = acc_x_out_buf[0];
+  if(acc_x_out_buf[0] < 0) {
+    x_axis_abs = (-1)*acc_x_out_buf[0];
+  } else {
+    x_axis_abs = acc_x_out_buf[0];
+  }
+  y_axis = acc_y_out_buf[0];
+  if(acc_y_out_buf[0] < 0) {
+    y_axis_abs = (-1)*acc_y_out_buf[0];
+  } else {
+    y_axis_abs = acc_y_out_buf[0];
+  }
+  z_axis = acc_z_out_buf[0];
+  if(acc_z_out_buf[0] < 0) {
+    z_axis_abs = (-1)*acc_z_out_buf[0];
+  } else {
+    z_axis_abs = acc_z_out_buf[0];
+  }
+}
+
+void calculate_orientation(){
+  if ( (z_axis_abs > x_axis_abs) && (z_axis_abs > y_axis_abs)) {
+      // base orientation on Z
+      if (z_axis > 0) {
+          orientation = 0;  
+      } else {
+          orientation = 100;
+      }
+  } else if ( (y_axis_abs > x_axis_abs) && (y_axis_abs > z_axis_abs)) {
+      // base orientation on Y
+      if (y_axis > 0) {
+          orientation = 200;
+      } else {
+          orientation = 300;
+      }
+  } else {
+      // base orientation on X
+      if (x_axis < 0) {
+          orientation = 400;
+      } else {
+          orientation = 500;
+      }
+  }
+}
+
 /**
 * Function for reading FIFO data
 */
@@ -155,6 +206,8 @@ int8_t get_bmi160_fifo_data() {
     // After 5 reads the buffer is almost full and the data is ready to be processed
     if (block_cnt == 5) {
         compute_fir();
+        calculate_absolute_axis_values();
+        calculate_orientation();
     }
 
     return rslt;

project/main.c

@@ -5,6 +5,7 @@
 #include "imu.h"
 #include "ble.h"
 
+uint8_t orientation;
 
 int main(void) {