Skip to content
Snippets Groups Projects
Commit 373201e4 authored by Aapo Torkkeli's avatar Aapo Torkkeli
Browse files

basic orientation calculation and representing.

parent 74ff9808
Branches
No related tags found
No related merge requests found
...@@ -83,6 +83,7 @@ ...@@ -83,6 +83,7 @@
#include "nrf_temp.h" #include "nrf_temp.h"
#include "ble_hts.h" #include "ble_hts.h"
#include "ble_bas.h" #include "ble_bas.h"
#include "arm_math.h"
#define DEVICE_NAME "nRF_Aapo" /**< Name of device. Will be included in the advertising data. */ #define DEVICE_NAME "nRF_Aapo" /**< Name of device. Will be included in the advertising data. */
...@@ -116,6 +117,8 @@ ...@@ -116,6 +117,8 @@
// Determines if temperature type is given as characteristic (1) or as a field of measurement (0) // Determines if temperature type is given as characteristic (1) or as a field of measurement (0)
#define TEMP_TYPE_AS_CHARACTERISTIC 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_GATT_DEF(m_gatt); /**< GATT module instance. */
NRF_BLE_QWR_DEF(m_qwr); /**< Context for the Queued Write module.*/ 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 ...@@ -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 // Flag to keep track of when an indication confirmation is pending
bool m_hts_meas_ind_conf_pending = false; bool m_hts_meas_ind_conf_pending = false;
volatile uint32_t hts_counter; // hold dummy hts data
// Function declarations // Function declarations
void on_hts_evt(ble_hts_t * p_hts, ble_hts_evt_t * p_evt); 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); ...@@ -146,17 +148,16 @@ void advertising_start(bool erase_bonds);
/* /*
* Function for generating a dummy temperature information packet. * 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 }; ble_date_time_t time_stamp = { 2018, 16, 10, 16, 15, 0 };
uint32_t celciusX100; uint32_t celciusX100;
celciusX100 = orientation; // one unit is 0.01 Celcius
p_meas->temp_in_fahr_units = false; p_meas->temp_in_fahr_units = false;
p_meas->time_stamp_present = true; p_meas->time_stamp_present = true;
p_meas->temp_type_present = (TEMP_TYPE_AS_CHARACTERISTIC ? false : 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.exponent = -2;
p_meas->temp_in_celcius.mantissa = celciusX100; p_meas->temp_in_celcius.mantissa = celciusX100;
p_meas->temp_in_fahr.exponent = -2; p_meas->temp_in_fahr.exponent = -2;
...@@ -199,7 +200,7 @@ void temperature_measurement_send(void) { ...@@ -199,7 +200,7 @@ void temperature_measurement_send(void) {
err_code; err_code;
if (!m_hts_meas_ind_conf_pending) { 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); err_code = ble_hts_measurement_send(&m_hts, &hts_meas);
switch (err_code) { switch (err_code) {
......
...@@ -64,6 +64,14 @@ ...@@ -64,6 +64,14 @@
#define NUM_TAPS 58 #define NUM_TAPS 58
#define BLOCK_SIZE 28 #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 // Declare a state array
static float32_t firStateF32[BLOCK_SIZE + NUM_TAPS - 1]; static float32_t firStateF32[BLOCK_SIZE + NUM_TAPS - 1];
...@@ -77,9 +85,6 @@ float32_t acc_x_out_buf[140]; ...@@ -77,9 +85,6 @@ float32_t acc_x_out_buf[140];
float32_t acc_y_out_buf[140]; float32_t acc_y_out_buf[140];
float32_t acc_z_out_buf[140]; float32_t acc_z_out_buf[140];
int in_array[16];
int out_array[16];
int block_cnt = 0; int block_cnt = 0;
//SPI instance //SPI instance
...@@ -131,6 +136,52 @@ void compute_fir() { ...@@ -131,6 +136,52 @@ void compute_fir() {
block_cnt = 0; // Reset block counting 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 * Function for reading FIFO data
*/ */
...@@ -155,6 +206,8 @@ int8_t get_bmi160_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 // After 5 reads the buffer is almost full and the data is ready to be processed
if (block_cnt == 5) { if (block_cnt == 5) {
compute_fir(); compute_fir();
calculate_absolute_axis_values();
calculate_orientation();
} }
return rslt; return rslt;
......
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include "imu.h" #include "imu.h"
#include "ble.h" #include "ble.h"
uint8_t orientation;
int main(void) { int main(void) {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment