From 373201e40786d51bdc25e48ff7930db4ae4b0fe6 Mon Sep 17 00:00:00 2001 From: Aapo Torkkeli <aamato@utu.fi> Date: Thu, 7 Nov 2019 19:42:01 +0200 Subject: [PATCH] basic orientation calculation and representing. --- project/ble.c | 11 +++++----- project/imu.c | 59 +++++++++++++++++++++++++++++++++++++++++++++++--- project/main.c | 1 + 3 files changed, 63 insertions(+), 8 deletions(-) diff --git a/project/ble.c b/project/ble.c index 50911e7..008f441 100644 --- a/project/ble.c +++ b/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) { diff --git a/project/imu.c b/project/imu.c index caba0b9..2c62df2 100644 --- a/project/imu.c +++ b/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; diff --git a/project/main.c b/project/main.c index 70a18c0..9491752 100644 --- a/project/main.c +++ b/project/main.c @@ -5,6 +5,7 @@ #include "imu.h" #include "ble.h" +uint8_t orientation; int main(void) { -- GitLab