From 35c58e1f3cde02c01f277e951743510604912715 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Mon, 6 Mar 2023 11:54:06 +0300 Subject: [PATCH 01/16] created sensor initialization code --- include/defs.h | 27 ++++++++++++++++++++++ include/sensors.cpp | 15 ++++++++++++ include/sensors.h | 16 +++++++++++++ platformio.ini | 4 ++++ src/main.cpp | 56 +++++++++++++++++++++++++++++++++++++++++++-- 5 files changed, 116 insertions(+), 2 deletions(-) create mode 100644 include/defs.h create mode 100644 include/sensors.cpp create mode 100644 include/sensors.h diff --git a/include/defs.h b/include/defs.h new file mode 100644 index 0000000..6e18b07 --- /dev/null +++ b/include/defs.h @@ -0,0 +1,27 @@ +#ifndef DEFS_H +#define DEFS_H + +/* debug enable for use during testing */ +#define DEBUG 1 + +#if DEBUG == 1 +#define debug(x) Serial.print(x) +#define debugln(x) Serial.println(x) +#define debugf(x, y) Serial.printf(x, y) + +#else + +#define debug(x) +#define debugln(x) +#define debugf(x, y) + +#endif + +/* timing constant */ +#define SETUP_DELAY 300 + +/* flight constants */ +#define EJECTION_HEIGHT 1000 // eject at 1000m AGL + + +#endif \ No newline at end of file diff --git a/include/sensors.cpp b/include/sensors.cpp new file mode 100644 index 0000000..6d73cc2 --- /dev/null +++ b/include/sensors.cpp @@ -0,0 +1,15 @@ +/** + * @file sensors.cpp + * @author Edwin Mwiti (emwiti658@gmail.com) + * @brief + * @version 0.1 + * @date 2023-03-06 + * + * @copyright Copyright (c) 2023 + * + * Implement sensor initialization functions + * + */ + +#include "sensors.h" + diff --git a/include/sensors.h b/include/sensors.h new file mode 100644 index 0000000..6304620 --- /dev/null +++ b/include/sensors.h @@ -0,0 +1,16 @@ +/* +Initialize sensors to be used during flight +1. MPU6050 +2. BMP180 + +*/ + +#ifndef SENSORS_H +#define SENSORS_H + +void _initialize_gyroscope(); +void _initialize_altimeter(); + + + +#endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 75725ee..2e7294f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,3 +12,7 @@ platform = espressif32 board = nodemcu-32s framework = arduino +lib_deps = + adafruit/Adafruit MPU6050@^2.2.4 + adafruit/Adafruit Unified Sensor@^1.1.7 + adafruit/Adafruit BMP085 Library @ ^1.2.2 diff --git a/src/main.cpp b/src/main.cpp index d615472..fba84ad 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,11 +1,63 @@ #include +#include +#include +#include +#include +#include "defs.h" +#include "sensors.h" + +/* create gyroscope object */ +Adafruit_MPU6050 gyroscope; + +/* create altimeter objects */ +Adafruit_BMP085 altimeter; + +void _initialize_gyroscope(){ + /* attempt to initialize MPU6050 */ + if(!gyroscope.begin(0x68)){ + debugln("MPU6050 allocation failed!"); + + // loop forever until found + for(;;); + } + + gyroscope.setAccelerometerRange(MPU6050_RANGE_8_G); + gyroscope.setGyroRange(MPU6050_RANGE_500_DEG); + gyroscope.setFilterBandwidth(MPU6050_BAND_5_HZ); + + delay(SETUP_DELAY); +} + +void _initialize_altimeter(){ + /* attempt to initialize BMP180 altimeter */ + if(!altimeter.begin(0x68)){ + debugln("Altimeter allocation failed!"); + + // loop forever until found + for(;;); + } +} + + void setup() { - // TODO: Implement + /* initialize serial */ + Serial.begin(115200); + + /* initialize sensors */ + _initialize_gyroscope() + _initialize_altimeter(); + + /* Create tasks */ + /* TASK 1: Read the pressure from the altimeter */ + xTaskCreatePinnedToCore( + + ); + } void loop() { - // TODO: Implement + } \ No newline at end of file From 80fd8f289a059ffa8ad692a974ec6d0d503d1c78 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Mon, 6 Mar 2023 12:57:17 +0300 Subject: [PATCH 02/16] added altimeter read task --- include/defs.h | 4 ++++ include/sensors.cpp | 15 --------------- include/sensors.h | 16 ---------------- src/main.cpp | 45 +++++++++++++++++++++++++++++++++++++++------ 4 files changed, 43 insertions(+), 37 deletions(-) delete mode 100644 include/sensors.cpp delete mode 100644 include/sensors.h diff --git a/include/defs.h b/include/defs.h index 6e18b07..878a835 100644 --- a/include/defs.h +++ b/include/defs.h @@ -22,6 +22,10 @@ /* flight constants */ #define EJECTION_HEIGHT 1000 // eject at 1000m AGL +#define SEA_LEVEL_PRESSURE 101325 // Assume the sea level pressure is 101325 Pascals - this can change with weather +/* tasks constants */ +#define STACK_SIZE 1024 + #endif \ No newline at end of file diff --git a/include/sensors.cpp b/include/sensors.cpp deleted file mode 100644 index 6d73cc2..0000000 --- a/include/sensors.cpp +++ /dev/null @@ -1,15 +0,0 @@ -/** - * @file sensors.cpp - * @author Edwin Mwiti (emwiti658@gmail.com) - * @brief - * @version 0.1 - * @date 2023-03-06 - * - * @copyright Copyright (c) 2023 - * - * Implement sensor initialization functions - * - */ - -#include "sensors.h" - diff --git a/include/sensors.h b/include/sensors.h deleted file mode 100644 index 6304620..0000000 --- a/include/sensors.h +++ /dev/null @@ -1,16 +0,0 @@ -/* -Initialize sensors to be used during flight -1. MPU6050 -2. BMP180 - -*/ - -#ifndef SENSORS_H -#define SENSORS_H - -void _initialize_gyroscope(); -void _initialize_altimeter(); - - - -#endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index fba84ad..cf51b26 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,7 +4,6 @@ #include #include #include "defs.h" -#include "sensors.h" /* create gyroscope object */ Adafruit_MPU6050 gyroscope; @@ -12,6 +11,11 @@ Adafruit_MPU6050 gyroscope; /* create altimeter objects */ Adafruit_BMP085 altimeter; +/* create queue to store altimeter data + * store pressure and altitude + * */ +QueueHandle_t altimeter_data_queue; + void _initialize_gyroscope(){ /* attempt to initialize MPU6050 */ if(!gyroscope.begin(0x68)){ @@ -34,11 +38,24 @@ void _initialize_altimeter(){ debugln("Altimeter allocation failed!"); // loop forever until found - for(;;); + while(true){} } } +void _readAltimeter(void* pvParameters){ + + while(true){ + /* Read pressure. + * This is the pressure from the sea level. + * */ + altimeter.readSealevelPressure(); + /* Read altitude + * This is the altitude from the sea level + * */ + altimeter.readAltitude(SEA_LEVEL_PRESSURE); + } +} void setup() { @@ -46,15 +63,31 @@ void setup() Serial.begin(115200); /* initialize sensors */ - _initialize_gyroscope() + _initialize_gyroscope(); _initialize_altimeter(); + // todo: initialize flash memory + - /* Create tasks */ - /* TASK 1: Read the pressure from the altimeter */ + /* Create tasks + * + * All tasks have a stack size of 1024 words - not bytes! + * ESP32 is 32 bit, therefore 32bits x 1024 = 4096 bytes + * So the stack size is 4096 bytes + * */ + + /* TASK 1: READ PRESSURE FROM THE ALTIMETER */ xTaskCreatePinnedToCore( - + _readAltimeter, /* function that executes this task*/ + "_readPressureFromAltimeter",/* Function name - for debugging */ + STACK_SIZE, /* Stack depth in words */ + (void *) altimeter_arr[], /* parameter to be passed to the task */ + tskIDLE_PRIORITY + 1, /* Task priority - in this case 1 */ + &readAltimeterTaskHandle /* task handle that can be passed to other tasks to reference the task */ ); + /* TASK 2: READ ACCELERATION FROM THE GYROSCOPE */ + + } void loop() From 3293953dc4265f6c9d3f8163c86997b911fa656f Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Mon, 6 Mar 2023 14:07:34 +0300 Subject: [PATCH 03/16] added data debug task --- include/defs.h | 4 +++- platformio.ini | 1 + src/main.cpp | 61 +++++++++++++++++++++++++++++++++++++++++--------- 3 files changed, 54 insertions(+), 12 deletions(-) diff --git a/include/defs.h b/include/defs.h index 878a835..5a46d9c 100644 --- a/include/defs.h +++ b/include/defs.h @@ -19,6 +19,7 @@ /* timing constant */ #define SETUP_DELAY 300 +#define TASK_DELAY 10 /* flight constants */ #define EJECTION_HEIGHT 1000 // eject at 1000m AGL @@ -26,6 +27,7 @@ /* tasks constants */ -#define STACK_SIZE 1024 +#define STACK_SIZE 512 +#define ALTIMETER_QUEUE_LENGTH 1 // todo: change to 2 items #endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 2e7294f..65f126f 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,6 +12,7 @@ platform = espressif32 board = nodemcu-32s framework = arduino +upload_port = COM4 lib_deps = adafruit/Adafruit MPU6050@^2.2.4 adafruit/Adafruit Unified Sensor@^1.1.7 diff --git a/src/main.cpp b/src/main.cpp index cf51b26..f71485c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -11,6 +11,12 @@ Adafruit_MPU6050 gyroscope; /* create altimeter objects */ Adafruit_BMP085 altimeter; +/* data variables */ +/* altimeter data */ +typedef struct { + float pressure; +} AltimeterData; + /* create queue to store altimeter data * store pressure and altitude * */ @@ -22,7 +28,7 @@ void _initialize_gyroscope(){ debugln("MPU6050 allocation failed!"); // loop forever until found - for(;;); + while(true){} } gyroscope.setAccelerometerRange(MPU6050_RANGE_8_G); @@ -48,26 +54,51 @@ void _readAltimeter(void* pvParameters){ /* Read pressure. * This is the pressure from the sea level. * */ - altimeter.readSealevelPressure(); + AltimeterData alt_data; + alt_data.pressure = altimeter.readSealevelPressure(); /* Read altitude * This is the altitude from the sea level * */ - altimeter.readAltitude(SEA_LEVEL_PRESSURE); +// altimeter.readAltitude(SEA_LEVEL_PRESSURE); + + /* send data to altimeter queue */ + if(xQueueSend(altimeter_data_queue, &alt_data, 0) != pdPASS){ + debugln("Queue full"); + } + + delay(TASK_DELAY); + } +} + +void _displayData(void* pvParameters){ + while(true){ + int buffer; + + if(xQueueReceive(altimeter_data_queue, &buffer, 0) == pdPASS){ + debugln(buffer); + }else{ + /* no queue */ + } + + delay(TASK_DELAY); } } + void setup() { /* initialize serial */ Serial.begin(115200); /* initialize sensors */ - _initialize_gyroscope(); +// _initialize_gyroscope(); _initialize_altimeter(); // todo: initialize flash memory - + /* create altimeter_data_queue */ + altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(AltimeterData)); + /* Create tasks * * All tasks have a stack size of 1024 words - not bytes! @@ -80,17 +111,25 @@ void setup() _readAltimeter, /* function that executes this task*/ "_readPressureFromAltimeter",/* Function name - for debugging */ STACK_SIZE, /* Stack depth in words */ - (void *) altimeter_arr[], /* parameter to be passed to the task */ + NULL, /* parameter to be passed to the task */ tskIDLE_PRIORITY + 1, /* Task priority - in this case 1 */ - &readAltimeterTaskHandle /* task handle that can be passed to other tasks to reference the task */ + NULL, /* task handle that can be passed to other tasks to reference the task */ + 1 /* run on core 1*/ ); /* TASK 2: READ ACCELERATION FROM THE GYROSCOPE */ - + /* TASK 3: DISPLAY DATA ON SERIAL MONITOR - FOR DEBUGGING */ + xTaskCreatePinnedToCore( + _displayData, + "_displayData", + STACK_SIZE, + NULL, + tskIDLE_PRIORITY+1, + NULL, + 1 + ); } -void loop() -{ - +void loop(){ } \ No newline at end of file From 6b8fb4d6529de275b4aa9becdc1d55d2acb343df Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Tue, 7 Mar 2023 11:27:44 +0300 Subject: [PATCH 04/16] changed queue data to struct pointer --- include/defs.h | 6 +- include/sensors.cpp | 4 + include/sensors.h | 11 +++ src/main.cpp | 185 +++++++++++++++++++++++++++++++------------- 4 files changed, 148 insertions(+), 58 deletions(-) create mode 100644 include/sensors.cpp create mode 100644 include/sensors.h diff --git a/include/defs.h b/include/defs.h index 5a46d9c..9fb80f5 100644 --- a/include/defs.h +++ b/include/defs.h @@ -25,9 +25,9 @@ #define EJECTION_HEIGHT 1000 // eject at 1000m AGL #define SEA_LEVEL_PRESSURE 101325 // Assume the sea level pressure is 101325 Pascals - this can change with weather - /* tasks constants */ -#define STACK_SIZE 512 -#define ALTIMETER_QUEUE_LENGTH 1 // todo: change to 2 items +#define STACK_SIZE 2048 +#define ALTIMETER_QUEUE_LENGTH 10 // todo: change to 2 items +#define GYROSCOPE_QUEUE_LENGTH 10 #endif \ No newline at end of file diff --git a/include/sensors.cpp b/include/sensors.cpp new file mode 100644 index 0000000..779ed99 --- /dev/null +++ b/include/sensors.cpp @@ -0,0 +1,4 @@ +// +// Created by Edwin Mwiti on 3/6/2023. +// + diff --git a/include/sensors.h b/include/sensors.h new file mode 100644 index 0000000..bc9004a --- /dev/null +++ b/include/sensors.h @@ -0,0 +1,11 @@ +// +// Created by Edwin Miwiri on 3/6/2023. +// + +#ifndef SENSORS_H +#define SENSORS_H + +void initialize_gyroscope(); +void initialize_altimeter(); + +#endif diff --git a/src/main.cpp b/src/main.cpp index f71485c..c63d09b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,6 +3,7 @@ #include #include #include +#include "sensors.h" #include "defs.h" /* create gyroscope object */ @@ -11,26 +12,15 @@ Adafruit_MPU6050 gyroscope; /* create altimeter objects */ Adafruit_BMP085 altimeter; -/* data variables */ -/* altimeter data */ -typedef struct { - float pressure; -} AltimeterData; - -/* create queue to store altimeter data - * store pressure and altitude - * */ -QueueHandle_t altimeter_data_queue; - -void _initialize_gyroscope(){ +void initialize_gyroscope(){ /* attempt to initialize MPU6050 */ if(!gyroscope.begin(0x68)){ - debugln("MPU6050 allocation failed!"); - + debugln("[-]Gyroscope allocation failed!"); // loop forever until found while(true){} } + debugln("[+]Gyroscope Initialized"); gyroscope.setAccelerometerRange(MPU6050_RANGE_8_G); gyroscope.setGyroRange(MPU6050_RANGE_500_DEG); gyroscope.setFilterBandwidth(MPU6050_BAND_5_HZ); @@ -38,23 +28,42 @@ void _initialize_gyroscope(){ delay(SETUP_DELAY); } -void _initialize_altimeter(){ - /* attempt to initialize BMP180 altimeter */ - if(!altimeter.begin(0x68)){ - debugln("Altimeter allocation failed!"); +void initialize_altimeter(){ + if (!altimeter.begin()) { + debugln("[-]Could not find a valid altimeter sensor"); + while (1) {} - // loop forever until found - while(true){} } + + debugln("[+]Altimeter initialized"); } -void _readAltimeter(void* pvParameters){ +/* data variables */ +/* gyroscope data */ + +struct Acceleration_Data{ + float ax; + float ay; + float az; +} ; + +struct Altimeter_Data{ + float pressure; +} ; + +/* create queue to store altimeter data + * store pressure and altitude + * */ +QueueHandle_t gyroscope_data_queue; +QueueHandle_t altimeter_data_queue; + +void readAltimeter(void* pvParameters){ while(true){ /* Read pressure. * This is the pressure from the sea level. * */ - AltimeterData alt_data; + Altimeter_Data alt_data; alt_data.pressure = altimeter.readSealevelPressure(); /* Read altitude @@ -64,27 +73,55 @@ void _readAltimeter(void* pvParameters){ /* send data to altimeter queue */ if(xQueueSend(altimeter_data_queue, &alt_data, 0) != pdPASS){ - debugln("Queue full"); + debugln("[-]Altimeter queue full"); } delay(TASK_DELAY); } } -void _displayData(void* pvParameters){ +void readGyroscope(void* pvParameters){ + while(true){ - int buffer; + Acceleration_Data gyro_data; + + sensors_event_t a, g, temp; + gyroscope.getEvent(&a, &g, &temp); + + /* Read acceleration + * */ + + gyro_data.ax = a.acceleration.x; - if(xQueueReceive(altimeter_data_queue, &buffer, 0) == pdPASS){ - debugln(buffer); - }else{ - /* no queue */ + /* Read altitude + * This is the altitude from the sea level + * */ +// altimeter.readAltitude(SEA_LEVEL_PRESSURE); + + /* send data to altimeter queue */ + if(xQueueSend(gyroscope_data_queue, &gyro_data, 0) != pdPASS){ + debugln("[-]Gyro queue full"); } delay(TASK_DELAY); } } +void displayData(void* pvParameters){ + while(true){ + Acceleration_Data buffer; + + if(xQueueReceive(gyroscope_data_queue, &buffer, 0) == pdPASS){ + debug("x: "); debug(buffer.ax); debugln(); + debug("y: "); debug(buffer.ay); debugln(); + debug("z: "); debug(buffer.az); debugln(); + }else{ + /* no queue */ + } + + delay(TASK_DELAY); + } +} void setup() { @@ -92,44 +129,82 @@ void setup() Serial.begin(115200); /* initialize sensors */ -// _initialize_gyroscope(); - _initialize_altimeter(); + initialize_gyroscope(); + initialize_altimeter(); // todo: initialize flash memory - /* create altimeter_data_queue */ - altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(AltimeterData)); + debugln("Creating queues..."); + /* create gyroscope data queue */ + gyroscope_data_queue = xQueueCreate(GYROSCOPE_QUEUE_LENGTH, sizeof(struct Acceleration_Data *)); + + /* check if the queues were created successfully */ + if(gyroscope_data_queue == NULL){ + debugln("[+]Gyroscope data queue creation failed!"); + } else{ + debugln("[+]Gyroscope data queue creation success"); + } + + + debugln("Creating tasks..."); + /* create altimeter_data_queue */ + altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(struct Altimeter_Data *)); + if(altimeter_data_queue == NULL){ + debugln("[+]Altimeter data queue creation failed!"); + } else{ + debugln("[+]Altimeter data queue creation success"); + } /* Create tasks - * * All tasks have a stack size of 1024 words - not bytes! * ESP32 is 32 bit, therefore 32bits x 1024 = 4096 bytes * So the stack size is 4096 bytes * */ - /* TASK 1: READ PRESSURE FROM THE ALTIMETER */ - xTaskCreatePinnedToCore( - _readAltimeter, /* function that executes this task*/ - "_readPressureFromAltimeter",/* Function name - for debugging */ - STACK_SIZE, /* Stack depth in words */ - NULL, /* parameter to be passed to the task */ - tskIDLE_PRIORITY + 1, /* Task priority - in this case 1 */ - NULL, /* task handle that can be passed to other tasks to reference the task */ - 1 /* run on core 1*/ - ); - - /* TASK 2: READ ACCELERATION FROM THE GYROSCOPE */ + /* TASK 1: READ ALTIMETER DATA */ + if(xTaskCreate( + readAltimeter, /* function that executes this task*/ + "readAltimeter",/* Function name - for debugging */ + STACK_SIZE, /* Stack depth in words */ + NULL, /* parameter to be passed to the task */ + tskIDLE_PRIORITY + 1, /* Task priority - in this case 1 */ + NULL /* task handle that can be passed to other tasks to reference the task */ + ) != pdPASS){ + // if task creation is not successful + debugln("[-]Read-Altimeter task creation failed!"); + }else{ + debugln("[+]Read-Altimeter task creation success"); + } + + /* TASK 2: READ GYROSCPE DATA */ + if(xTaskCreate( + readGyroscope, + "readGyroscope", + STACK_SIZE, + NULL, + 1, + NULL + ) != pdPASS){ + debugln("[-]Read-Gyroscope task creation failed!"); + } else{ + debugln("[+]Read-Gyroscope task creation success!"); + } /* TASK 3: DISPLAY DATA ON SERIAL MONITOR - FOR DEBUGGING */ - xTaskCreatePinnedToCore( - _displayData, - "_displayData", - STACK_SIZE, - NULL, - tskIDLE_PRIORITY+1, - NULL, - 1 - ); + if(xTaskCreate( + displayData, + "displayData", + STACK_SIZE, + NULL, + 1, + NULL + ) != pdPASS){ + debugln("[-]Display data task creation failed!"); + }else{ + debugln("[+]Display data task creation success!"); + } + } void loop(){ + delay(1); } \ No newline at end of file From 8edf43dd4b3ed60f9642177636af055c6b089611 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Tue, 7 Mar 2023 14:03:42 +0300 Subject: [PATCH 05/16] changed queue type from struct pointer to passing by value --- src/main.cpp | 74 +++++++++++++++++++++++++++++++--------------------- 1 file changed, 44 insertions(+), 30 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index c63d09b..631c0aa 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -45,11 +45,12 @@ struct Acceleration_Data{ float ax; float ay; float az; -} ; +}; struct Altimeter_Data{ - float pressure; -} ; + int32_t pressure; + float altitude; +}; /* create queue to store altimeter data * store pressure and altitude @@ -63,16 +64,20 @@ void readAltimeter(void* pvParameters){ /* Read pressure. * This is the pressure from the sea level. * */ - Altimeter_Data alt_data; + struct Altimeter_Data alt_data; + + /* Read pressure + * This is the pressure from the sea level + * */ alt_data.pressure = altimeter.readSealevelPressure(); /* Read altitude * This is the altitude from the sea level * */ -// altimeter.readAltitude(SEA_LEVEL_PRESSURE); + alt_data.altitude = altimeter.readAltitude(SEA_LEVEL_PRESSURE); /* send data to altimeter queue */ - if(xQueueSend(altimeter_data_queue, &alt_data, 0) != pdPASS){ + if(xQueueSend(altimeter_data_queue, &alt_data, portMAX_DELAY) != pdPASS){ debugln("[-]Altimeter queue full"); } @@ -81,25 +86,22 @@ void readAltimeter(void* pvParameters){ } void readGyroscope(void* pvParameters){ + while(true){ - Acceleration_Data gyro_data; - sensors_event_t a, g, temp; gyroscope.getEvent(&a, &g, &temp); - - /* Read acceleration - * */ - gyro_data.ax = a.acceleration.x; - - /* Read altitude - * This is the altitude from the sea level + struct Acceleration_Data gyro_data; + /* + * Read acceleration * */ -// altimeter.readAltitude(SEA_LEVEL_PRESSURE); + gyro_data.ax = a.acceleration.x; + gyro_data.ay = a.acceleration.y; + gyro_data.az = a.acceleration.z; /* send data to altimeter queue */ - if(xQueueSend(gyroscope_data_queue, &gyro_data, 0) != pdPASS){ + if(xQueueSend(gyroscope_data_queue, &gyro_data, portMAX_DELAY) != pdPASS){ debugln("[-]Gyro queue full"); } @@ -109,20 +111,33 @@ void readGyroscope(void* pvParameters){ void displayData(void* pvParameters){ while(true){ - Acceleration_Data buffer; + struct Acceleration_Data gyroscope_buffer; + struct Altimeter_Data altimeter_buffer; + + if(xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS){ + debug("x: "); debug(gyroscope_buffer.ax); debugln(); + debug("y: "); debug(gyroscope_buffer.ay); debugln(); + debug("z: "); debug(gyroscope_buffer.az); debugln(); + + }else{ + /* no queue */ + } - if(xQueueReceive(gyroscope_data_queue, &buffer, 0) == pdPASS){ - debug("x: "); debug(buffer.ax); debugln(); - debug("y: "); debug(buffer.ay); debugln(); - debug("z: "); debug(buffer.az); debugln(); + if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ + debug("pressure: "); debug(altimeter_buffer.pressure); debugln(); + debug("altitude: "); debug(altimeter_buffer.altitude); debugln(); + }else{ /* no queue */ } - delay(TASK_DELAY); + delay(10); } } + + + void setup() { /* initialize serial */ @@ -135,7 +150,9 @@ void setup() debugln("Creating queues..."); /* create gyroscope data queue */ - gyroscope_data_queue = xQueueCreate(GYROSCOPE_QUEUE_LENGTH, sizeof(struct Acceleration_Data *)); + gyroscope_data_queue = xQueueCreate(GYROSCOPE_QUEUE_LENGTH, sizeof(struct Acceleration_Data)); + /* create altimeter_data_queue */ + altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(struct Altimeter_Data)); /* check if the queues were created successfully */ if(gyroscope_data_queue == NULL){ @@ -143,22 +160,19 @@ void setup() } else{ debugln("[+]Gyroscope data queue creation success"); } - - - debugln("Creating tasks..."); - /* create altimeter_data_queue */ - altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(struct Altimeter_Data *)); + if(altimeter_data_queue == NULL){ debugln("[+]Altimeter data queue creation failed!"); } else{ debugln("[+]Altimeter data queue creation success"); } - + /* Create tasks * All tasks have a stack size of 1024 words - not bytes! * ESP32 is 32 bit, therefore 32bits x 1024 = 4096 bytes * So the stack size is 4096 bytes * */ + debugln("Creating tasks..."); /* TASK 1: READ ALTIMETER DATA */ if(xTaskCreate( From b868df6e22f8fa85fba455667f605bcc0110f9c0 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Tue, 7 Mar 2023 16:48:25 +0300 Subject: [PATCH 06/16] added csv to log data --- csv-ouput.csv | 351 +++++++++++++++++++++++++++++ log-data/data-grapher.py | 26 +++ log-data/log-data | 467 +++++++++++++++++++++++++++++++++++++++ src/main.cpp | 20 +- 4 files changed, 853 insertions(+), 11 deletions(-) create mode 100644 csv-ouput.csv create mode 100644 log-data/data-grapher.py create mode 100644 log-data/log-data diff --git a/csv-ouput.csv b/csv-ouput.csv new file mode 100644 index 0000000..b585d07 --- /dev/null +++ b/csv-ouput.csv @@ -0,0 +1,351 @@ +16:45:07,b'[+]Gyroscope Initialized\r\n' +16:45:07,b'[+]Altimeter initialized\r\n' +16:45:08,b'Creating queues...\r\n' +16:45:08,b'[+]Gyroscope data queue creation success\r\n' +16:45:08,b'[+]Altimeter data queue creation success\r\n' +16:45:08,b'Creating tasks...\r\n' +16:45:08,b'[+]Read-Altimeter task creation success\r\n' +16:45:08,b'[+]Read-Gyroscope task creation success!\r\n' +16:45:08,b'[+]Display data task creation success!\r\n' +16:45:08,b'0.02\r\n' +16:45:08,b'-0.00\r\n' +16:45:08,b'-0.02\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.03\r\n' +16:45:08,b'-0.01\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.02\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'-0.01\r\n' +16:45:08,b'-0.02\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.03\r\n' +16:45:08,b'-0.01\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.02\r\n' +16:45:08,b'0.04\r\n' +16:45:08,b'0.03\r\n' +16:45:08,b'0.02\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'-0.01\r\n' +16:45:08,b'-0.03\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.06\r\n' +16:45:08,b'-0.06\r\n' +16:45:08,b'-0.07\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.01\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.04\r\n' +16:45:08,b'0.05\r\n' +16:45:08,b'0.06\r\n' +16:45:08,b'0.05\r\n' +16:45:08,b'0.03\r\n' +16:45:08,b'0.02\r\n' +16:45:08,b'-0.00\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.06\r\n' +16:45:08,b'-0.06\r\n' +16:45:08,b'-0.05\r\n' +16:45:08,b'-0.04\r\n' +16:45:08,b'-0.03\r\n' +16:45:08,b'-0.02\r\n' +16:45:08,b'-0.00\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.01\r\n' +16:45:08,b'0.00\r\n' +16:45:08,b'0.00\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'0.00\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'0.00\r\n' +16:45:09,b'0.02\r\n' +16:45:09,b'0.02\r\n' +16:45:09,b'0.02\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.04\r\n' +16:45:09,b'-0.04\r\n' +16:45:09,b'-0.05\r\n' +16:45:09,b'-0.04\r\n' +16:45:09,b'-0.03\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.02\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.00\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'0.00\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.03\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'0.00\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'0.02\r\n' +16:45:09,b'0.02\r\n' +16:45:09,b'0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.00\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.02\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:09,b'-0.01\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.03\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.01\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.02\r\n' +16:45:10,b'-0.04\r\n' +16:45:10,b'-0.04\r\n' +16:45:10,b'-0.03\r\n' +16:45:10,b'-0.03\r\n' +16:45:10,b'-0.01\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'0.03\r\n' +16:45:10,b'0.04\r\n' +16:45:10,b'0.05\r\n' +16:45:10,b'0.04\r\n' +16:45:10,b'0.04\r\n' +16:45:10,b'0.03\r\n' +16:45:10,b'0.00\r\n' +16:45:10,b'-0.03\r\n' +16:45:10,b'-0.06\r\n' +16:45:10,b'-0.08\r\n' +16:45:10,b'-0.08\r\n' +16:45:10,b'-0.08\r\n' +16:45:10,b'-0.06\r\n' +16:45:10,b'-0.04\r\n' +16:45:10,b'-0.00\r\n' +16:45:10,b'0.02\r\n' +16:45:10,b'0.03\r\n' +16:45:10,b'0.03\r\n' +16:45:10,b'0.04\r\n' +16:45:10,b'0.03\r\n' +16:45:11,b'0.02\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'0.02\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.03\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'-0.02\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.02\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'0.00\r\n' +16:45:11,b'0.02\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.05\r\n' +16:45:11,b'0.06\r\n' +16:45:11,b'0.05\r\n' +16:45:11,b'0.03\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'-0.00\r\n' +16:45:11,b'-0.00\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'0.00\r\n' +16:45:11,b'-0.00\r\n' +16:45:11,b'-0.02\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.06\r\n' +16:45:11,b'-0.06\r\n' +16:45:11,b'-0.06\r\n' +16:45:11,b'-0.05\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'0.02\r\n' +16:45:11,b'0.03\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.04\r\n' +16:45:11,b'0.02\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'-0.03\r\n' +16:45:11,b'-0.06\r\n' +16:45:11,b'-0.08\r\n' +16:45:11,b'-0.10\r\n' +16:45:11,b'-0.09\r\n' +16:45:11,b'-0.08\r\n' +16:45:11,b'-0.07\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'0.00\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'-0.02\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.06\r\n' +16:45:11,b'-0.05\r\n' +16:45:11,b'-0.05\r\n' +16:45:11,b'-0.04\r\n' +16:45:11,b'-0.01\r\n' +16:45:11,b'0.01\r\n' +16:45:11,b'0.02\r\n' +16:45:11,b'0.03\r\n' +16:45:12,b'0.03\r\n' +16:45:12,b'0.02\r\n' +16:45:12,b'-0.00\r\n' +16:45:12,b'-0.02\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.04\r\n' +16:45:12,b'-0.04\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.01\r\n' +16:45:12,b'-0.00\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'-0.00\r\n' +16:45:12,b'-0.00\r\n' +16:45:12,b'-0.01\r\n' +16:45:12,b'-0.02\r\n' +16:45:12,b'-0.02\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.04\r\n' +16:45:12,b'-0.04\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.02\r\n' +16:45:12,b'-0.01\r\n' +16:45:12,b'-0.00\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'-0.01\r\n' +16:45:12,b'-0.02\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.03\r\n' +16:45:12,b'-0.02\r\n' +16:45:12,b'-0.01\r\n' +16:45:12,b'-0.00\r\n' +16:45:12,b'0.01\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'0.00\r\n' +16:45:12,b'-0.00\r\n' diff --git a/log-data/data-grapher.py b/log-data/data-grapher.py new file mode 100644 index 0000000..d6113ea --- /dev/null +++ b/log-data/data-grapher.py @@ -0,0 +1,26 @@ +from serial import Serial +import csv +from datetime import datetime + +port = "COM4" +baud_rate = 115200 + +def read_serial_data(): + serial_port = Serial(port, baud_rate) + + # csv writer object + output_file = open("csv-ouput.csv", "w", newline="") + output_writer = csv.writer(output_file) + + # attempt to opern port + while(True): + now = datetime.now() + current_time = now.strftime("%H:%M:%S") + + # read x acceleration into a csv file + line = serial_port.readline() + + output_writer.writerow([current_time, line]) + + +read_serial_data() \ No newline at end of file diff --git a/log-data/log-data b/log-data/log-data new file mode 100644 index 0000000..22d49d2 --- /dev/null +++ b/log-data/log-data @@ -0,0 +1,467 @@ +=~=~=~=~=~=~=~=~=~=~=~= PuTTY log 2023.03.07 13:44:21 =~=~=~=~=~=~=~=~=~=~=~= +pressure: 84810 +altitude: 1475.71 +x: -0.08 +y: 0.23 +z: 10.36 +pressure: 84808 +altitude: 1476.00 +x: -0.07 +y: 0.23 +z: 10.37 +pressure: 84807 +altitude: 1476.09 +x: -0.07 +y: 0.23 +z: 10.38 +pressure: 84807 +altitude: 1475.71 +x: -0.08 +y: 0.24 +z: 10.36 +pressure: 84808 +altitude: 1475.71 +x: -0.08 +y: 0.24 +z: 10.37 +pressure: 84810 +altitude: 1475.81 +x: -0.07 +y: 0.23 +z: 10.37 +pressure: 84814 +altitude: 1475.81 +x: -0.06 +y: 0.24 +z: 10.38 +pressure: 84806 +altitude: 1475.52 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84808 +altitude: 1475.71 +x: -0.08 +y: 0.23 +z: 10.37 +pressure: 84813 +altitude: 1475.52 +x: -0.06 +y: 0.23 +z: 10.36 +pressure: 84811 +altitude: 1476.00 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84812 +altitude: 1475.61 +x: -0.07 +y: 0.23 +z: 10.37 +pressure: 84808 +altitude: 1475.90 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84813 +altitude: 1475.61 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84803 +altitude: 1475.61 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84804 +altitude: 1476.19 +x: -0.06 +y: 0.23 +z: 10.37 +pressure: 84809 +altitude: 1475.61 +x: -0.07 +y: 0.23 +z: 10.37 +pressure: 84809 +altitude: 1475.71 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84808 +altitude: 1475.61 +x: -0.07 +y: 0.23 +z: 10.38 +pressure: 84816 +altitude: 1475.61 +x: -0.06 +y: 0.23 +z: 10.37 +pressure: 84811 +altitude: 1475.71 +x: -0.06 +y: 0.23 +z: 10.35 +pressure: 84807 +altitude: 1475.61 +x: -0.07 +y: 0.23 +z: 10.36 +pressure: 84806 +altitude: 1475.81 +x: -0.06 +y: 0.23 +z: 10.36 +pressure: 84807 +altitude: 1476.09 +x: -0.06 +y: 0.23 +z: 10.36 +pressure: 84811 +altitude: 1475.52 +x: -0.06 +y: 0.24 +z: 10.36 +pressure: 84805 +altitude: 1476.19 +x: -0.06 +y: 0.23 +z: 10.37 +pressure: 84806 +altitude: 1475.81 +x: -0.08 +y: 0.23 +z: 10.36 +pressure: 84807 +altitude: 1476.19 +x: -0.06 +y: 0.23 +z: 10.37 +pressure: 84809 +altitude: 1475.42 +x: -0.07 +y: 0.22 +z: 10.36 +pressure: 84811 +altitude: 1475.81 +x: -0.07 +y: 0.23 +z: 10.39 +pressure: 84808 +altitude: 1475.61 +x: -0.07 +y: 0.24 +z: 10.37 +pressure: 84811 +altitude: 1475.81 +x: -0.07 +y: 0.23 +z: 10.37 +pressure: 84807 +altitude: 1476.09 +x: -0.08 +y: 0.24 +z: 10.37 +pressure: 84807 +altitude: 1476.00 +x: -0.07 +y: 0.23 +z: 10.38 +pressure: 84810 +altitude: 1476.00 +x: -0.07 +y: 0.23 +z: 10.37 +pressure: 84809 +altitude: 1475.81 +x: -0.05 +y: 0.24 +z: 10.37 +pressure: 84808 +altitude: 1476.00 +x: -0.20 +y: 0.28 +z: 10.38 +pressure: 84810 +altitude: 1475.42 +x: -0.17 +y: 0.27 +z: 10.37 +pressure: 84809 +altitude: 1476.00 +x: -0.14 +y: 0.25 +z: 10.35 +pressure: 84810 +altitude: 1475.71 +x: -1.12 +y: -0.09 +z: 9.65 +pressure: 84809 +altitude: 1475.81 +x: -2.01 +y: 0.77 +z: 11.38 +pressure: 84811 +altitude: 1475.90 +x: 0.42 +y: -2.29 +z: 14.81 +pressure: 84809 +altitude: 1476.00 +x: 5.97 +y: 1.61 +z: -1.77 +pressure: 84810 +altitude: 1475.90 +x: 6.57 +y: 2.21 +z: -7.43 +pressure: 84807 +altitude: 1476.19 +x: 4.66 +y: 0.76 +z: -8.45 +pressure: 84805 +altitude: 1475.71 +x: 4.73 +y: 1.56 +z: -8.62 +pressure: 84804 +altitude: 1475.71 +x: 4.23 +y: 1.71 +z: -8.90 +pressure: 84802 +altitude: 1475.81 +x: 3.36 +y: 2.09 +z: -8.77 +pressure: 84813 +altitude: 1475.81 +x: 3.12 +y: 2.00 +z: -9.64 +pressure: 84808 +altitude: 1475.71 +x: 3.41 +y: 2.33 +z: -8.75 +pressure: 84810 +altitude: 1475.90 +x: 3.61 +y: 2.33 +z: -9.05 +pressure: 84809 +altitude: 1475.61 +x: 3.95 +y: 2.41 +z: -8.32 +pressure: 84808 +altitude: 1475.52 +x: 5.33 +y: 1.65 +z: -8.34 +pressure: 84809 +altitude: 1475.90 +x: 5.66 +y: 1.73 +z: -7.59 +pressure: 84813 +altitude: 1475.90 +x: 5.84 +y: 1.56 +z: -8.03 +pressure: 84811 +altitude: 1476.00 +x: 5.54 +y: 1.71 +z: -8.10 +pressure: 84808 +altitude: 1475.71 +x: 5.44 +y: 1.83 +z: -8.13 +pressure: 84812 +altitude: 1475.71 +x: 5.89 +y: 1.87 +z: -7.87 +pressure: 84804 +altitude: 1475.71 +x: 7.66 +y: 2.03 +z: -8.58 +pressure: 84808 +altitude: 1475.90 +x: 7.88 +y: 2.95 +z: 1.44 +pressure: 84808 +altitude: 1475.90 +x: 6.72 +y: 4.59 +z: 5.80 +pressure: 84809 +altitude: 1475.81 +x: 3.67 +y: 3.37 +z: 10.31 +pressure: 84810 +altitude: 1475.81 +x: 1.69 +y: 5.76 +z: 7.55 +pressure: 84808 +altitude: 1476.19 +x: -8.16 +y: 3.46 +z: 8.68 +pressure: 84814 +altitude: 1475.32 +x: -9.45 +y: 3.26 +z: 2.03 +pressure: 84813 +altitude: 1475.52 +x: -7.85 +y: 1.57 +z: 1.98 +pressure: 84809 +altitude: 1475.32 +x: -9.33 +y: 1.70 +z: 2.05 +pressure: 84817 +altitude: 1475.32 +x: -9.32 +y: 1.76 +z: 1.79 +pressure: 84816 +altitude: 1476.00 +x: -9.39 +y: 1.72 +z: 1.89 +pressure: 84814 +altitude: 1475.61 +x: -9.36 +y: 1.79 +z: 1.70 +pressure: 84814 +altitude: 1475.32 +x: -9.40 +y: 1.85 +z: 1.63 +pressure: 84811 +altitude: 1475.23 +x: -9.34 +y: 1.86 +z: 1.60 +pressure: 84812 +altitude: 1475.52 +x: -9.38 +y: 1.81 +z: 1.68 +pressure: 84817 +altitude: 1475.81 +x: -9.36 +y: 1.78 +z: 1.72 +pressure: 84809 +altitude: 1475.42 +x: -9.40 +y: 1.80 +z: 1.66 +pressure: 84812 +altitude: 1475.81 +x: -9.26 +y: 1.77 +z: 1.70 +pressure: 84816 +altitude: 1475.42 +x: -9.40 +y: 1.85 +z: 1.61 +pressure: 84808 +altitude: 1475.13 +x: -10.30 +y: 0.25 +z: 1.19 +pressure: 84815 +altitude: 1475.52 +x: -6.49 +y: -3.54 +z: 6.23 +pressure: 84812 +altitude: 1475.81 +x: -2.99 +y: -5.95 +z: 7.57 +pressure: 84812 +altitude: 1475.90 +x: -1.85 +y: -5.86 +z: 7.87 +pressure: 84814 +altitude: 1475.81 +x: -1.12 +y: -5.58 +z: 8.92 +pressure: 84810 +altitude: 1475.81 +x: -0.90 +y: -1.34 +z: 9.22 +pressure: 84807 +altitude: 1476.09 +x: -2.34 +y: 0.76 +z: 11.73 +pressure: 84808 +altitude: 1475.90 +x: -2.07 +y: 2.01 +z: 9.87 +pressure: 84807 +altitude: 1475.90 +x: -2.22 +y: 2.42 +z: 9.97 +pressure: 84809 +altitude: 1476.00 +x: -2.25 +y: 2.54 +z: 9.90 +pressure: 84809 +altitude: 1475.52 +x: -2.48 +y: 2.50 +z: 9.80 +pressure: 84814 +altitude: 1475.81 +x: -2.43 +y: 2.73 +z: 9.64 +pressure: 84804 +altitude: 1476.09 +x: -2.63 +y: 3.36 +z: 9.36 +pressure: 84810 +altitude: 1475.90 +x: -2.35 +y: 3.49 +z: 9.20 +pressure: 84808 +altitude: 1476.09 +x: -2.56 +y: 3.81 +z: 9.15 +pressure: 84809 +altitude: 1475.81 +x: -2.31 +y: 3.78 +z: 9.31 +p \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 631c0aa..0718607 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -87,7 +87,6 @@ void readAltimeter(void* pvParameters){ void readGyroscope(void* pvParameters){ - while(true){ sensors_event_t a, g, temp; gyroscope.getEvent(&a, &g, &temp); @@ -115,21 +114,21 @@ void displayData(void* pvParameters){ struct Altimeter_Data altimeter_buffer; if(xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS){ - debug("x: "); debug(gyroscope_buffer.ax); debugln(); - debug("y: "); debug(gyroscope_buffer.ay); debugln(); - debug("z: "); debug(gyroscope_buffer.az); debugln(); + debug(gyroscope_buffer.ax); debugln(); + // debug("y: "); debug(gyroscope_buffer.ay); debugln(); + // debug("z: "); debug(gyroscope_buffer.az); debugln(); }else{ /* no queue */ } - if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ - debug("pressure: "); debug(altimeter_buffer.pressure); debugln(); - debug("altitude: "); debug(altimeter_buffer.altitude); debugln(); + // if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ + // debug("pressure: "); debug(altimeter_buffer.pressure); debugln(); + // debug("altitude: "); debug(altimeter_buffer.altitude); debugln(); - }else{ - /* no queue */ - } + // }else{ + // /* no queue */ + // } delay(10); } @@ -137,7 +136,6 @@ void displayData(void* pvParameters){ - void setup() { /* initialize serial */ From 92f4a78275d6297dcf5ef11687fd5a599dc777cf Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Fri, 10 Mar 2023 22:34:00 +0300 Subject: [PATCH 07/16] changed time to epoch --- csv-ouput.csv | 351 ----------------------------- data-logger.py | 52 +++++ graph.py | 24 ++ log-data/data-grapher.py | 26 --- log-data/log-data | 467 --------------------------------------- sensor-data.csv | 338 ++++++++++++++++++++++++++++ 6 files changed, 414 insertions(+), 844 deletions(-) delete mode 100644 csv-ouput.csv create mode 100644 data-logger.py create mode 100644 graph.py delete mode 100644 log-data/data-grapher.py delete mode 100644 log-data/log-data create mode 100644 sensor-data.csv diff --git a/csv-ouput.csv b/csv-ouput.csv deleted file mode 100644 index b585d07..0000000 --- a/csv-ouput.csv +++ /dev/null @@ -1,351 +0,0 @@ -16:45:07,b'[+]Gyroscope Initialized\r\n' -16:45:07,b'[+]Altimeter initialized\r\n' -16:45:08,b'Creating queues...\r\n' -16:45:08,b'[+]Gyroscope data queue creation success\r\n' -16:45:08,b'[+]Altimeter data queue creation success\r\n' -16:45:08,b'Creating tasks...\r\n' -16:45:08,b'[+]Read-Altimeter task creation success\r\n' -16:45:08,b'[+]Read-Gyroscope task creation success!\r\n' -16:45:08,b'[+]Display data task creation success!\r\n' -16:45:08,b'0.02\r\n' -16:45:08,b'-0.00\r\n' -16:45:08,b'-0.02\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.03\r\n' -16:45:08,b'-0.01\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.02\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'-0.01\r\n' -16:45:08,b'-0.02\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.03\r\n' -16:45:08,b'-0.01\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.02\r\n' -16:45:08,b'0.04\r\n' -16:45:08,b'0.03\r\n' -16:45:08,b'0.02\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'-0.01\r\n' -16:45:08,b'-0.03\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.06\r\n' -16:45:08,b'-0.06\r\n' -16:45:08,b'-0.07\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.01\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.04\r\n' -16:45:08,b'0.05\r\n' -16:45:08,b'0.06\r\n' -16:45:08,b'0.05\r\n' -16:45:08,b'0.03\r\n' -16:45:08,b'0.02\r\n' -16:45:08,b'-0.00\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.06\r\n' -16:45:08,b'-0.06\r\n' -16:45:08,b'-0.05\r\n' -16:45:08,b'-0.04\r\n' -16:45:08,b'-0.03\r\n' -16:45:08,b'-0.02\r\n' -16:45:08,b'-0.00\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.01\r\n' -16:45:08,b'0.00\r\n' -16:45:08,b'0.00\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'0.00\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'0.00\r\n' -16:45:09,b'0.02\r\n' -16:45:09,b'0.02\r\n' -16:45:09,b'0.02\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.04\r\n' -16:45:09,b'-0.04\r\n' -16:45:09,b'-0.05\r\n' -16:45:09,b'-0.04\r\n' -16:45:09,b'-0.03\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.02\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.00\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'0.00\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.03\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'0.00\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'0.02\r\n' -16:45:09,b'0.02\r\n' -16:45:09,b'0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.00\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.02\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:09,b'-0.01\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.03\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.01\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.02\r\n' -16:45:10,b'-0.04\r\n' -16:45:10,b'-0.04\r\n' -16:45:10,b'-0.03\r\n' -16:45:10,b'-0.03\r\n' -16:45:10,b'-0.01\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'0.03\r\n' -16:45:10,b'0.04\r\n' -16:45:10,b'0.05\r\n' -16:45:10,b'0.04\r\n' -16:45:10,b'0.04\r\n' -16:45:10,b'0.03\r\n' -16:45:10,b'0.00\r\n' -16:45:10,b'-0.03\r\n' -16:45:10,b'-0.06\r\n' -16:45:10,b'-0.08\r\n' -16:45:10,b'-0.08\r\n' -16:45:10,b'-0.08\r\n' -16:45:10,b'-0.06\r\n' -16:45:10,b'-0.04\r\n' -16:45:10,b'-0.00\r\n' -16:45:10,b'0.02\r\n' -16:45:10,b'0.03\r\n' -16:45:10,b'0.03\r\n' -16:45:10,b'0.04\r\n' -16:45:10,b'0.03\r\n' -16:45:11,b'0.02\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'0.02\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.03\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'-0.02\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.02\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'0.00\r\n' -16:45:11,b'0.02\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.05\r\n' -16:45:11,b'0.06\r\n' -16:45:11,b'0.05\r\n' -16:45:11,b'0.03\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'-0.00\r\n' -16:45:11,b'-0.00\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'0.00\r\n' -16:45:11,b'-0.00\r\n' -16:45:11,b'-0.02\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.06\r\n' -16:45:11,b'-0.06\r\n' -16:45:11,b'-0.06\r\n' -16:45:11,b'-0.05\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'0.02\r\n' -16:45:11,b'0.03\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.04\r\n' -16:45:11,b'0.02\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'-0.03\r\n' -16:45:11,b'-0.06\r\n' -16:45:11,b'-0.08\r\n' -16:45:11,b'-0.10\r\n' -16:45:11,b'-0.09\r\n' -16:45:11,b'-0.08\r\n' -16:45:11,b'-0.07\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'0.00\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'-0.02\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.06\r\n' -16:45:11,b'-0.05\r\n' -16:45:11,b'-0.05\r\n' -16:45:11,b'-0.04\r\n' -16:45:11,b'-0.01\r\n' -16:45:11,b'0.01\r\n' -16:45:11,b'0.02\r\n' -16:45:11,b'0.03\r\n' -16:45:12,b'0.03\r\n' -16:45:12,b'0.02\r\n' -16:45:12,b'-0.00\r\n' -16:45:12,b'-0.02\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.04\r\n' -16:45:12,b'-0.04\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.01\r\n' -16:45:12,b'-0.00\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'-0.00\r\n' -16:45:12,b'-0.00\r\n' -16:45:12,b'-0.01\r\n' -16:45:12,b'-0.02\r\n' -16:45:12,b'-0.02\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.04\r\n' -16:45:12,b'-0.04\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.02\r\n' -16:45:12,b'-0.01\r\n' -16:45:12,b'-0.00\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'-0.01\r\n' -16:45:12,b'-0.02\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.03\r\n' -16:45:12,b'-0.02\r\n' -16:45:12,b'-0.01\r\n' -16:45:12,b'-0.00\r\n' -16:45:12,b'0.01\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'0.00\r\n' -16:45:12,b'-0.00\r\n' diff --git a/data-logger.py b/data-logger.py new file mode 100644 index 0000000..af9dd9f --- /dev/null +++ b/data-logger.py @@ -0,0 +1,52 @@ +from serial import Serial +from datetime import datetime +import matplotlib.pyplot as plt +import matplotlib.cbook as cbook +import numpy as np +import csv +import atexit + + +port = "COM4" +baud_rate = 115200 + +def plot_data(): + + # read file + data_file = cbook.get_sample_data("sensor-data.csv", asfileobj=False) + with cbook.get_sample_data("sensor-data.csv") as file: + arr = np.loadtxt(file, delimiter=",", unpack="True") + + plt.plot(arr[0], arr[1]) + plt.title("Unfiltered x acceleration") + plt.xlabel("Time (ms)") + plt.ylabel("X acceleration (m/s^2)") + + plt.show() + + +def read_serial_data(): + + serial_port = Serial(port, baud_rate) + + # csv writer object + output_file = open("sensor-data.csv", "w", newline="") + output_writer = csv.writer(output_file) + + # register function to run when the function exits + # atexit.register(plot_data) + + while(True): + now = datetime.now() + current_time = now.timestamp() + + # read x acceleration into a csv file + data = serial_port.readline() + + if data: + data = float(data) + + output_writer.writerow([current_time, data]) + + +read_serial_data() \ No newline at end of file diff --git a/graph.py b/graph.py new file mode 100644 index 0000000..4b34178 --- /dev/null +++ b/graph.py @@ -0,0 +1,24 @@ +import matplotlib.pyplot as plt +import matplotlib.cbook as cbook +import numpy as np +import os + +def plot_data(): + # plot the data once the is done running + + if(os.path.exists("sensor-data.csv")): + print("File found") + else: + print("File does not exist") + + # read file + arr = np.loadtxt("sensor-data.csv", delimiter=",", unpack=True) + + plt.plot(arr[0], arr[1]) + plt.title("Unfiltered x acceleration") + plt.xlabel("Time (ms)") + plt.ylabel("X acceleration (m/s^2)") + + plt.show() + +plot_data() diff --git a/log-data/data-grapher.py b/log-data/data-grapher.py deleted file mode 100644 index d6113ea..0000000 --- a/log-data/data-grapher.py +++ /dev/null @@ -1,26 +0,0 @@ -from serial import Serial -import csv -from datetime import datetime - -port = "COM4" -baud_rate = 115200 - -def read_serial_data(): - serial_port = Serial(port, baud_rate) - - # csv writer object - output_file = open("csv-ouput.csv", "w", newline="") - output_writer = csv.writer(output_file) - - # attempt to opern port - while(True): - now = datetime.now() - current_time = now.strftime("%H:%M:%S") - - # read x acceleration into a csv file - line = serial_port.readline() - - output_writer.writerow([current_time, line]) - - -read_serial_data() \ No newline at end of file diff --git a/log-data/log-data b/log-data/log-data deleted file mode 100644 index 22d49d2..0000000 --- a/log-data/log-data +++ /dev/null @@ -1,467 +0,0 @@ -=~=~=~=~=~=~=~=~=~=~=~= PuTTY log 2023.03.07 13:44:21 =~=~=~=~=~=~=~=~=~=~=~= -pressure: 84810 -altitude: 1475.71 -x: -0.08 -y: 0.23 -z: 10.36 -pressure: 84808 -altitude: 1476.00 -x: -0.07 -y: 0.23 -z: 10.37 -pressure: 84807 -altitude: 1476.09 -x: -0.07 -y: 0.23 -z: 10.38 -pressure: 84807 -altitude: 1475.71 -x: -0.08 -y: 0.24 -z: 10.36 -pressure: 84808 -altitude: 1475.71 -x: -0.08 -y: 0.24 -z: 10.37 -pressure: 84810 -altitude: 1475.81 -x: -0.07 -y: 0.23 -z: 10.37 -pressure: 84814 -altitude: 1475.81 -x: -0.06 -y: 0.24 -z: 10.38 -pressure: 84806 -altitude: 1475.52 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84808 -altitude: 1475.71 -x: -0.08 -y: 0.23 -z: 10.37 -pressure: 84813 -altitude: 1475.52 -x: -0.06 -y: 0.23 -z: 10.36 -pressure: 84811 -altitude: 1476.00 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84812 -altitude: 1475.61 -x: -0.07 -y: 0.23 -z: 10.37 -pressure: 84808 -altitude: 1475.90 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84813 -altitude: 1475.61 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84803 -altitude: 1475.61 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84804 -altitude: 1476.19 -x: -0.06 -y: 0.23 -z: 10.37 -pressure: 84809 -altitude: 1475.61 -x: -0.07 -y: 0.23 -z: 10.37 -pressure: 84809 -altitude: 1475.71 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84808 -altitude: 1475.61 -x: -0.07 -y: 0.23 -z: 10.38 -pressure: 84816 -altitude: 1475.61 -x: -0.06 -y: 0.23 -z: 10.37 -pressure: 84811 -altitude: 1475.71 -x: -0.06 -y: 0.23 -z: 10.35 -pressure: 84807 -altitude: 1475.61 -x: -0.07 -y: 0.23 -z: 10.36 -pressure: 84806 -altitude: 1475.81 -x: -0.06 -y: 0.23 -z: 10.36 -pressure: 84807 -altitude: 1476.09 -x: -0.06 -y: 0.23 -z: 10.36 -pressure: 84811 -altitude: 1475.52 -x: -0.06 -y: 0.24 -z: 10.36 -pressure: 84805 -altitude: 1476.19 -x: -0.06 -y: 0.23 -z: 10.37 -pressure: 84806 -altitude: 1475.81 -x: -0.08 -y: 0.23 -z: 10.36 -pressure: 84807 -altitude: 1476.19 -x: -0.06 -y: 0.23 -z: 10.37 -pressure: 84809 -altitude: 1475.42 -x: -0.07 -y: 0.22 -z: 10.36 -pressure: 84811 -altitude: 1475.81 -x: -0.07 -y: 0.23 -z: 10.39 -pressure: 84808 -altitude: 1475.61 -x: -0.07 -y: 0.24 -z: 10.37 -pressure: 84811 -altitude: 1475.81 -x: -0.07 -y: 0.23 -z: 10.37 -pressure: 84807 -altitude: 1476.09 -x: -0.08 -y: 0.24 -z: 10.37 -pressure: 84807 -altitude: 1476.00 -x: -0.07 -y: 0.23 -z: 10.38 -pressure: 84810 -altitude: 1476.00 -x: -0.07 -y: 0.23 -z: 10.37 -pressure: 84809 -altitude: 1475.81 -x: -0.05 -y: 0.24 -z: 10.37 -pressure: 84808 -altitude: 1476.00 -x: -0.20 -y: 0.28 -z: 10.38 -pressure: 84810 -altitude: 1475.42 -x: -0.17 -y: 0.27 -z: 10.37 -pressure: 84809 -altitude: 1476.00 -x: -0.14 -y: 0.25 -z: 10.35 -pressure: 84810 -altitude: 1475.71 -x: -1.12 -y: -0.09 -z: 9.65 -pressure: 84809 -altitude: 1475.81 -x: -2.01 -y: 0.77 -z: 11.38 -pressure: 84811 -altitude: 1475.90 -x: 0.42 -y: -2.29 -z: 14.81 -pressure: 84809 -altitude: 1476.00 -x: 5.97 -y: 1.61 -z: -1.77 -pressure: 84810 -altitude: 1475.90 -x: 6.57 -y: 2.21 -z: -7.43 -pressure: 84807 -altitude: 1476.19 -x: 4.66 -y: 0.76 -z: -8.45 -pressure: 84805 -altitude: 1475.71 -x: 4.73 -y: 1.56 -z: -8.62 -pressure: 84804 -altitude: 1475.71 -x: 4.23 -y: 1.71 -z: -8.90 -pressure: 84802 -altitude: 1475.81 -x: 3.36 -y: 2.09 -z: -8.77 -pressure: 84813 -altitude: 1475.81 -x: 3.12 -y: 2.00 -z: -9.64 -pressure: 84808 -altitude: 1475.71 -x: 3.41 -y: 2.33 -z: -8.75 -pressure: 84810 -altitude: 1475.90 -x: 3.61 -y: 2.33 -z: -9.05 -pressure: 84809 -altitude: 1475.61 -x: 3.95 -y: 2.41 -z: -8.32 -pressure: 84808 -altitude: 1475.52 -x: 5.33 -y: 1.65 -z: -8.34 -pressure: 84809 -altitude: 1475.90 -x: 5.66 -y: 1.73 -z: -7.59 -pressure: 84813 -altitude: 1475.90 -x: 5.84 -y: 1.56 -z: -8.03 -pressure: 84811 -altitude: 1476.00 -x: 5.54 -y: 1.71 -z: -8.10 -pressure: 84808 -altitude: 1475.71 -x: 5.44 -y: 1.83 -z: -8.13 -pressure: 84812 -altitude: 1475.71 -x: 5.89 -y: 1.87 -z: -7.87 -pressure: 84804 -altitude: 1475.71 -x: 7.66 -y: 2.03 -z: -8.58 -pressure: 84808 -altitude: 1475.90 -x: 7.88 -y: 2.95 -z: 1.44 -pressure: 84808 -altitude: 1475.90 -x: 6.72 -y: 4.59 -z: 5.80 -pressure: 84809 -altitude: 1475.81 -x: 3.67 -y: 3.37 -z: 10.31 -pressure: 84810 -altitude: 1475.81 -x: 1.69 -y: 5.76 -z: 7.55 -pressure: 84808 -altitude: 1476.19 -x: -8.16 -y: 3.46 -z: 8.68 -pressure: 84814 -altitude: 1475.32 -x: -9.45 -y: 3.26 -z: 2.03 -pressure: 84813 -altitude: 1475.52 -x: -7.85 -y: 1.57 -z: 1.98 -pressure: 84809 -altitude: 1475.32 -x: -9.33 -y: 1.70 -z: 2.05 -pressure: 84817 -altitude: 1475.32 -x: -9.32 -y: 1.76 -z: 1.79 -pressure: 84816 -altitude: 1476.00 -x: -9.39 -y: 1.72 -z: 1.89 -pressure: 84814 -altitude: 1475.61 -x: -9.36 -y: 1.79 -z: 1.70 -pressure: 84814 -altitude: 1475.32 -x: -9.40 -y: 1.85 -z: 1.63 -pressure: 84811 -altitude: 1475.23 -x: -9.34 -y: 1.86 -z: 1.60 -pressure: 84812 -altitude: 1475.52 -x: -9.38 -y: 1.81 -z: 1.68 -pressure: 84817 -altitude: 1475.81 -x: -9.36 -y: 1.78 -z: 1.72 -pressure: 84809 -altitude: 1475.42 -x: -9.40 -y: 1.80 -z: 1.66 -pressure: 84812 -altitude: 1475.81 -x: -9.26 -y: 1.77 -z: 1.70 -pressure: 84816 -altitude: 1475.42 -x: -9.40 -y: 1.85 -z: 1.61 -pressure: 84808 -altitude: 1475.13 -x: -10.30 -y: 0.25 -z: 1.19 -pressure: 84815 -altitude: 1475.52 -x: -6.49 -y: -3.54 -z: 6.23 -pressure: 84812 -altitude: 1475.81 -x: -2.99 -y: -5.95 -z: 7.57 -pressure: 84812 -altitude: 1475.90 -x: -1.85 -y: -5.86 -z: 7.87 -pressure: 84814 -altitude: 1475.81 -x: -1.12 -y: -5.58 -z: 8.92 -pressure: 84810 -altitude: 1475.81 -x: -0.90 -y: -1.34 -z: 9.22 -pressure: 84807 -altitude: 1476.09 -x: -2.34 -y: 0.76 -z: 11.73 -pressure: 84808 -altitude: 1475.90 -x: -2.07 -y: 2.01 -z: 9.87 -pressure: 84807 -altitude: 1475.90 -x: -2.22 -y: 2.42 -z: 9.97 -pressure: 84809 -altitude: 1476.00 -x: -2.25 -y: 2.54 -z: 9.90 -pressure: 84809 -altitude: 1475.52 -x: -2.48 -y: 2.50 -z: 9.80 -pressure: 84814 -altitude: 1475.81 -x: -2.43 -y: 2.73 -z: 9.64 -pressure: 84804 -altitude: 1476.09 -x: -2.63 -y: 3.36 -z: 9.36 -pressure: 84810 -altitude: 1475.90 -x: -2.35 -y: 3.49 -z: 9.20 -pressure: 84808 -altitude: 1476.09 -x: -2.56 -y: 3.81 -z: 9.15 -pressure: 84809 -altitude: 1475.81 -x: -2.31 -y: 3.78 -z: 9.31 -p \ No newline at end of file diff --git a/sensor-data.csv b/sensor-data.csv new file mode 100644 index 0000000..a0507f2 --- /dev/null +++ b/sensor-data.csv @@ -0,0 +1,338 @@ +1678476474.893631,-1.51 +1678476474.899297,-2.04 +1678476474.914385,-2.63 +1678476474.924397,-3.48 +1678476474.940385,-4.37 +1678476474.954117,-5.18 +1678476474.962759,-5.75 +1678476474.979768,-6.14 +1678476474.993354,-6.4 +1678476475.005853,-6.48 +1678476475.019,-6.45 +1678476475.029561,-6.53 +1678476475.044559,-6.77 +1678476475.054123,-7.04 +1678476475.064121,-7.41 +1678476475.079522,-7.68 +1678476475.097014,-8.07 +1678476475.111014,-8.38 +1678476475.12312,-8.63 +1678476475.131702,-8.81 +1678476475.148163,-8.89 +1678476475.158187,-8.93 +1678476475.174748,-8.68 +1678476475.184766,-8.31 +1678476475.198785,-7.86 +1678476475.214157,-7.27 +1678476475.227142,-6.56 +1678476475.240245,-5.98 +1678476475.253257,-5.32 +1678476475.262845,-4.65 +1678476475.277953,-4.07 +1678476475.287952,-3.45 +1678476475.303994,-2.79 +1678476475.314152,-2.26 +1678476475.330211,-1.88 +1678476475.343926,-1.6 +1678476475.355509,-1.42 +1678476475.365503,-1.08 +1678476475.381633,-0.49 +1678476475.393663,0.23 +1678476475.407873,0.81 +1678476475.417868,1.18 +1678476475.43272,1.51 +1678476475.447706,1.81 +1678476475.460067,2.15 +1678476475.469145,2.54 +1678476475.48412,2.98 +1678476475.498908,3.28 +1678476475.512901,3.6 +1678476475.525002,3.99 +1678476475.533144,4.41 +1678476475.551247,4.75 +1678476475.564258,5.08 +1678476475.574271,5.37 +1678476475.590394,5.66 +1678476475.602939,5.94 +1678476475.612959,6.16 +1678476475.628159,6.32 +1678476475.642914,6.47 +1678476475.65298,6.73 +1678476475.666759,7.05 +1678476475.676776,7.31 +1678476475.694242,7.54 +1678476475.704261,7.76 +1678476475.719243,7.89 +1678476475.733301,7.99 +1678476475.746365,8.2 +1678476475.759357,8.44 +1678476475.772369,8.7 +1678476475.782436,8.97 +1678476475.797414,9.22 +1678476475.808505,9.35 +1678476475.823356,9.37 +1678476475.838083,9.3 +1678476475.845167,9.22 +1678476475.863319,9.13 +1678476475.872931,9.09 +1678476475.889019,9.05 +1678476475.898998,8.93 +1678476475.913009,8.69 +1678476475.928693,8.25 +1678476475.938229,7.69 +1678476475.953225,7.06 +1678476475.965738,6.39 +1678476475.9805,5.73 +1678476475.993746,5.15 +1678476476.006898,4.63 +1678476476.019869,4.15 +1678476476.02897,3.78 +1678476476.042978,3.54 +1678476476.054121,3.27 +1678476476.068188,2.92 +1678476476.083187,2.48 +1678476476.093187,2.12 +1678476476.108187,1.91 +1678476476.122788,1.66 +1678476476.13501,1.37 +1678476476.145004,1.06 +1678476476.158008,0.67 +1678476476.174126,0.24 +1678476476.184143,-0.17 +1678476476.197256,-0.53 +1678476476.214486,-0.85 +1678476476.224503,-1.09 +1678476476.235623,-1.28 +1678476476.252774,-1.52 +1678476476.266615,-1.79 +1678476476.27962,-2.05 +1678476476.292905,-2.29 +1678476476.302892,-2.55 +1678476476.31902,-2.79 +1678476476.330945,-2.98 +1678476476.340944,-3.17 +1678476476.354635,-3.5 +1678476476.368142,-3.95 +1678476476.381545,-4.52 +1678476476.394117,-5.14 +1678476476.407942,-5.73 +1678476476.422942,-6.39 +1678476476.436015,-6.86 +1678476476.446012,-7.09 +1678476476.461017,-7.15 +1678476476.473027,-7.15 +1678476476.487888,-7.25 +1678476476.500858,-7.4 +1678476476.513851,-7.56 +1678476476.526844,-7.6 +1678476476.53674,-7.53 +1678476476.551728,-7.43 +1678476476.566483,-7.33 +1678476476.577484,-7.26 +1678476476.588002,-7.12 +1678476476.603109,-6.92 +1678476476.614115,-6.71 +1678476476.6314,-6.46 +1678476476.644413,-6.17 +1678476476.657276,-5.88 +1678476476.670284,-5.59 +1678476476.683324,-5.24 +1678476476.695407,-4.79 +1678476476.708491,-4.36 +1678476476.718202,-3.96 +1678476476.733289,-3.67 +1678476476.744124,-3.49 +1678476476.758186,-3.35 +1678476476.77424,-3.27 +1678476476.785379,-3.23 +1678476476.799597,-3.16 +1678476476.812583,-3.03 +1678476476.826215,-2.9 +1678476476.837997,-2.73 +1678476476.849146,-2.48 +1678476476.864242,-2.2 +1678476476.874238,-1.93 +1678476476.889246,-1.64 +1678476476.904256,-1.32 +1678476476.914249,-0.97 +1678476476.929884,-0.6 +1678476476.943409,-0.3 +1678476476.955496,-0.07 +1678476476.968488,0.15 +1678476476.981479,0.35 +1678476476.994472,0.53 +1678476477.007747,0.75 +1678476477.017878,1.02 +1678476477.032993,1.3 +1678476477.044502,1.6 +1678476477.059675,1.92 +1678476477.072734,2.27 +1678476477.084878,2.55 +1678476477.094891,2.76 +1678476477.109728,3.0 +1678476477.124402,3.31 +1678476477.137868,3.62 +1678476477.144628,3.86 +1678476477.163647,4.09 +1678476477.174749,4.3 +1678476477.18772,4.53 +1678476477.202711,4.8 +1678476477.214393,5.1 +1678476477.226527,5.41 +1678476477.238035,5.78 +1678476477.253176,6.15 +1678476477.264379,6.53 +1678476477.279861,6.91 +1678476477.29367,7.21 +1678476477.305133,7.47 +1678476477.315147,7.72 +1678476477.332855,7.97 +1678476477.34499,8.1 +1678476477.357981,8.11 +1678476477.368056,8.03 +1678476477.383054,7.84 +1678476477.393054,7.63 +1678476477.408052,7.39 +1678476477.423099,7.11 +1678476477.434505,6.81 +1678476477.447721,6.51 +1678476477.462706,6.25 +1678476477.475783,5.98 +1678476477.488756,5.77 +1678476477.501747,5.58 +1678476477.514863,5.41 +1678476477.526796,5.32 +1678476477.53869,5.44 +1678476477.553487,5.55 +1678476477.564497,5.56 +1678476477.578002,5.47 +1678476477.588191,5.24 +1678476477.602732,4.87 +1678476477.617749,4.46 +1678476477.627744,4.36 +1678476477.643112,4.65 +1678476477.653121,4.89 +1678476477.668099,4.85 +1678476477.679143,4.51 +1678476477.694132,4.15 +1678476477.704119,4.0 +1678476477.722173,4.11 +1678476477.734225,4.36 +1678476477.748464,4.24 +1678476477.754626,3.68 +1678476477.774488,2.99 +1678476477.787864,2.13 +1678476477.797894,2.16 +1678476477.812889,3.09 +1678476477.822887,3.83 +1678476477.838993,3.9 +1678476477.85262,3.63 +1678476477.865483,3.31 +1678476477.878623,3.22 +1678476477.891618,3.29 +1678476477.90477,3.58 +1678476477.916636,3.87 +1678476477.926629,4.0 +1678476477.939268,3.93 +1678476477.953327,3.66 +1678476477.969408,3.18 +1678476477.982398,2.72 +1678476477.995606,2.49 +1678476478.008618,2.38 +1678476478.017731,2.24 +1678476478.032734,2.07 +1678476478.047714,1.9 +1678476478.057893,1.81 +1678476478.072891,1.74 +1678476478.08289,1.66 +1678476478.09791,1.6 +1678476478.10901,1.46 +1678476478.123015,1.26 +1678476478.138028,1.13 +1678476478.148051,1.01 +1678476478.163217,0.9 +1678476478.173164,0.73 +1678476478.18818,0.61 +1678476478.203552,0.53 +1678476478.21464,0.34 +1678476478.226455,0.27 +1678476478.24145,0.14 +1678476478.253049,-0.1 +1678476478.269144,-0.35 +1678476478.279122,-0.66 +1678476478.291065,-1.0 +1678476478.30412,-1.31 +1678476478.319117,-1.55 +1678476478.334115,-1.78 +1678476478.344265,-2.1 +1678476478.35817,-2.47 +1678476478.373291,-2.82 +1678476478.385898,-2.95 +1678476478.398899,-3.06 +1678476478.411883,-3.41 +1678476478.424874,-3.83 +1678476478.438036,-4.17 +1678476478.448174,-4.73 +1678476478.464801,-5.2 +1678476478.476795,-5.0 +1678476478.489898,-4.56 +1678476478.502881,-4.46 +1678476478.513016,-4.81 +1678476478.527984,-5.4 +1678476478.537982,-5.88 +1678476478.552982,-5.87 +1678476478.566754,-5.44 +1678476478.577878,-4.96 +1678476478.59288,-4.62 +1678476478.602897,-4.53 +1678476478.617734,-4.54 +1678476478.632705,-4.37 +1678476478.644373,-3.67 +1678476478.657769,-2.66 +1678476478.667847,-1.93 +1678476478.681427,-1.77 +1678476478.693999,-1.89 +1678476478.708995,-1.86 +1678476478.723993,-1.6 +1678476478.733988,-1.12 +1678476478.748154,-0.59 +1678476478.759246,-0.26 +1678476478.776157,-0.16 +1678476478.787247,-0.15 +1678476478.802246,0.07 +1678476478.813997,0.61 +1678476478.823994,1.38 +1678476478.836256,2.09 +1678476478.853257,2.48 +1678476478.862771,2.44 +1678476478.877532,2.27 +1678476478.888264,2.33 +1678476478.904377,2.48 +1678476478.918875,2.54 +1678476478.931886,2.42 +1678476478.944992,2.41 +1678476478.957789,2.66 +1678476478.970761,2.85 +1678476478.983756,2.83 +1678476478.995866,2.59 +1678476479.009934,2.26 +1678476479.019495,2.18 +1678476479.035902,2.31 +1678476479.04803,2.43 +1678476479.058079,2.44 +1678476479.07412,2.32 +1678476479.084112,2.17 +1678476479.100143,1.98 +1678476479.11328,2.01 +1678476479.124251,2.21 +1678476479.139374,2.34 +1678476479.152939,2.12 +1678476479.162938,1.71 +1678476479.174128,1.4 +1678476479.189118,1.25 +1678476479.203145,1.05 +1678476479.213124,0.95 +1678476479.228123,1.08 +1678476479.243458,1.21 +1678476479.254628,1.07 +1678476479.265752,0.69 From 791b0d9b190036d1ab1376deb8cc3394af24b516 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Fri, 10 Mar 2023 23:28:01 +0300 Subject: [PATCH 08/16] added untested Kalman filter --- data-logger.py | 26 +- include/kalman.cpp | 78 ++++++ include/kalman.h | 12 + platformio.ini | 3 +- sensor-data.csv | 678 +++++++++++++++++++++++---------------------- 5 files changed, 449 insertions(+), 348 deletions(-) create mode 100644 include/kalman.cpp create mode 100644 include/kalman.h diff --git a/data-logger.py b/data-logger.py index af9dd9f..451f90f 100644 --- a/data-logger.py +++ b/data-logger.py @@ -1,32 +1,40 @@ from serial import Serial + from datetime import datetime import matplotlib.pyplot as plt import matplotlib.cbook as cbook import numpy as np import csv + import atexit port = "COM4" baud_rate = 115200 +@atexit.register def plot_data(): + # plot the data once the is done running + + if(os.path.exists("sensor-data.csv")): + print("File found") + else: + print("File does not exist") # read file - data_file = cbook.get_sample_data("sensor-data.csv", asfileobj=False) - with cbook.get_sample_data("sensor-data.csv") as file: - arr = np.loadtxt(file, delimiter=",", unpack="True") + arr = np.loadtxt("sensor-data.csv", delimiter=",", unpack=True) - plt.plot(arr[0], arr[1]) - plt.title("Unfiltered x acceleration") - plt.xlabel("Time (ms)") - plt.ylabel("X acceleration (m/s^2)") + plt.plot(arr[0], arr[1]) + plt.title("Unfiltered x acceleration") + plt.xlabel("Time (ms)") + plt.ylabel("X acceleration (m/s^2)") - plt.show() + plt.show() def read_serial_data(): - + + # create Serial object serial_port = Serial(port, baud_rate) # csv writer object diff --git a/include/kalman.cpp b/include/kalman.cpp new file mode 100644 index 0000000..897673e --- /dev/null +++ b/include/kalman.cpp @@ -0,0 +1,78 @@ +#include +#include "defs.h" +#include "kalman.h" + +float q = 0.0001; +float T = 0.1; + +// The system dynamics +BLA::Matrix<3, 3> A = {1.0, 0.1, 0.005, + 0, 1.0, 0.1, + 0, 0, 1.0}; + +// Relationship between measurement and states +BLA::Matrix<2, 3> H = {1.0, 0, 0, + 0, 0, 1.0}; + +// Initial posteriori estimate error covariance +BLA::Matrix<3, 3> P = {1, 0, 0, + 0, 1, 0, + 0, 0, 1}; + +// Measurement error covariance +BLA::Matrix<2, 2> R = {0.25, 0, + 0, 0.75}; + +// Process noise covariance +BLA::Matrix<3, 3> Q = {q, q, q, + q, q, q, + q, q, q}; + +// Identity Matrix +BLA::Matrix<3, 3> I = {1, 0, 0, + 0, 1, 0, + 0, 0, 1}; + +BLA::Matrix<3, 1> x_hat = {1500.0, + 0.0, + 0.0}; + +BLA::Matrix<2, 1> Y = {0.0, + 0.0}; + +// kalmanUpdate This filters our altitude and acceleration values +struct Filtered_Data filterData(float x_acceleration){ + /* this struct will store the filtered data values */ + struct Filtered_Data filtered_values; + + // Measurement matrix + BLA::Matrix<2, 1> Z = {0, x_acceleration}; + + // Predicted state estimate + BLA::Matrix<3, 1> x_hat_minus = A * x_hat; + + // Predicted estimate covariance + BLA::Matrix<3, 3> P_minus = A * P * (~A) + Q; + + // Kalman gain + BLA::Matrix<2, 2> con = (H * P_minus * (~H) + R); + BLA::Matrix<3, 2> K = P_minus * (~H) * Invert(con); + + // Measurement residual + Y = Z - (H * x_hat_minus); + + // Updated state estimate + x_hat = x_hat_minus + K * Y; + + // Updated estimate covariance + P = (I - K * H) * P_minus; + Y = Z - (H * x_hat_minus); + + filtered_values.x_acceleration = x_hat(0); + + // return_val.displacement = x_hat(0); + // return_val.velocity = x_hat(1); + // return_val.acceleration = x_hat(2); + + return filtered_values; +} diff --git a/include/kalman.h b/include/kalman.h new file mode 100644 index 0000000..0ea1cc7 --- /dev/null +++ b/include/kalman.h @@ -0,0 +1,12 @@ + +#pragma once + +/* define struct to hold filtered data*/ +struct Filtered_Data{ + float x_acceleration; +}; + + +/* this function returns Kalman-filtered data */ +struct Filtered_Data filterData(float); + diff --git a/platformio.ini b/platformio.ini index 65f126f..65cbc00 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,4 +16,5 @@ upload_port = COM4 lib_deps = adafruit/Adafruit MPU6050@^2.2.4 adafruit/Adafruit Unified Sensor@^1.1.7 - adafruit/Adafruit BMP085 Library @ ^1.2.2 + adafruit/Adafruit BMP085 Library @ ^1.2.2 + tomstewart89/BasicLinearAlgebra@^3.7 diff --git a/sensor-data.csv b/sensor-data.csv index a0507f2..ba018ca 100644 --- a/sensor-data.csv +++ b/sensor-data.csv @@ -1,338 +1,340 @@ -1678476474.893631,-1.51 -1678476474.899297,-2.04 -1678476474.914385,-2.63 -1678476474.924397,-3.48 -1678476474.940385,-4.37 -1678476474.954117,-5.18 -1678476474.962759,-5.75 -1678476474.979768,-6.14 -1678476474.993354,-6.4 -1678476475.005853,-6.48 -1678476475.019,-6.45 -1678476475.029561,-6.53 -1678476475.044559,-6.77 -1678476475.054123,-7.04 -1678476475.064121,-7.41 -1678476475.079522,-7.68 -1678476475.097014,-8.07 -1678476475.111014,-8.38 -1678476475.12312,-8.63 -1678476475.131702,-8.81 -1678476475.148163,-8.89 -1678476475.158187,-8.93 -1678476475.174748,-8.68 -1678476475.184766,-8.31 -1678476475.198785,-7.86 -1678476475.214157,-7.27 -1678476475.227142,-6.56 -1678476475.240245,-5.98 -1678476475.253257,-5.32 -1678476475.262845,-4.65 -1678476475.277953,-4.07 -1678476475.287952,-3.45 -1678476475.303994,-2.79 -1678476475.314152,-2.26 -1678476475.330211,-1.88 -1678476475.343926,-1.6 -1678476475.355509,-1.42 -1678476475.365503,-1.08 -1678476475.381633,-0.49 -1678476475.393663,0.23 -1678476475.407873,0.81 -1678476475.417868,1.18 -1678476475.43272,1.51 -1678476475.447706,1.81 -1678476475.460067,2.15 -1678476475.469145,2.54 -1678476475.48412,2.98 -1678476475.498908,3.28 -1678476475.512901,3.6 -1678476475.525002,3.99 -1678476475.533144,4.41 -1678476475.551247,4.75 -1678476475.564258,5.08 -1678476475.574271,5.37 -1678476475.590394,5.66 -1678476475.602939,5.94 -1678476475.612959,6.16 -1678476475.628159,6.32 -1678476475.642914,6.47 -1678476475.65298,6.73 -1678476475.666759,7.05 -1678476475.676776,7.31 -1678476475.694242,7.54 -1678476475.704261,7.76 -1678476475.719243,7.89 -1678476475.733301,7.99 -1678476475.746365,8.2 -1678476475.759357,8.44 -1678476475.772369,8.7 -1678476475.782436,8.97 -1678476475.797414,9.22 -1678476475.808505,9.35 -1678476475.823356,9.37 -1678476475.838083,9.3 -1678476475.845167,9.22 -1678476475.863319,9.13 -1678476475.872931,9.09 -1678476475.889019,9.05 -1678476475.898998,8.93 -1678476475.913009,8.69 -1678476475.928693,8.25 -1678476475.938229,7.69 -1678476475.953225,7.06 -1678476475.965738,6.39 -1678476475.9805,5.73 -1678476475.993746,5.15 -1678476476.006898,4.63 -1678476476.019869,4.15 -1678476476.02897,3.78 -1678476476.042978,3.54 -1678476476.054121,3.27 -1678476476.068188,2.92 -1678476476.083187,2.48 -1678476476.093187,2.12 -1678476476.108187,1.91 -1678476476.122788,1.66 -1678476476.13501,1.37 -1678476476.145004,1.06 -1678476476.158008,0.67 -1678476476.174126,0.24 -1678476476.184143,-0.17 -1678476476.197256,-0.53 -1678476476.214486,-0.85 -1678476476.224503,-1.09 -1678476476.235623,-1.28 -1678476476.252774,-1.52 -1678476476.266615,-1.79 -1678476476.27962,-2.05 -1678476476.292905,-2.29 -1678476476.302892,-2.55 -1678476476.31902,-2.79 -1678476476.330945,-2.98 -1678476476.340944,-3.17 -1678476476.354635,-3.5 -1678476476.368142,-3.95 -1678476476.381545,-4.52 -1678476476.394117,-5.14 -1678476476.407942,-5.73 -1678476476.422942,-6.39 -1678476476.436015,-6.86 -1678476476.446012,-7.09 -1678476476.461017,-7.15 -1678476476.473027,-7.15 -1678476476.487888,-7.25 -1678476476.500858,-7.4 -1678476476.513851,-7.56 -1678476476.526844,-7.6 -1678476476.53674,-7.53 -1678476476.551728,-7.43 -1678476476.566483,-7.33 -1678476476.577484,-7.26 -1678476476.588002,-7.12 -1678476476.603109,-6.92 -1678476476.614115,-6.71 -1678476476.6314,-6.46 -1678476476.644413,-6.17 -1678476476.657276,-5.88 -1678476476.670284,-5.59 -1678476476.683324,-5.24 -1678476476.695407,-4.79 -1678476476.708491,-4.36 -1678476476.718202,-3.96 -1678476476.733289,-3.67 -1678476476.744124,-3.49 -1678476476.758186,-3.35 -1678476476.77424,-3.27 -1678476476.785379,-3.23 -1678476476.799597,-3.16 -1678476476.812583,-3.03 -1678476476.826215,-2.9 -1678476476.837997,-2.73 -1678476476.849146,-2.48 -1678476476.864242,-2.2 -1678476476.874238,-1.93 -1678476476.889246,-1.64 -1678476476.904256,-1.32 -1678476476.914249,-0.97 -1678476476.929884,-0.6 -1678476476.943409,-0.3 -1678476476.955496,-0.07 -1678476476.968488,0.15 -1678476476.981479,0.35 -1678476476.994472,0.53 -1678476477.007747,0.75 -1678476477.017878,1.02 -1678476477.032993,1.3 -1678476477.044502,1.6 -1678476477.059675,1.92 -1678476477.072734,2.27 -1678476477.084878,2.55 -1678476477.094891,2.76 -1678476477.109728,3.0 -1678476477.124402,3.31 -1678476477.137868,3.62 -1678476477.144628,3.86 -1678476477.163647,4.09 -1678476477.174749,4.3 -1678476477.18772,4.53 -1678476477.202711,4.8 -1678476477.214393,5.1 -1678476477.226527,5.41 -1678476477.238035,5.78 -1678476477.253176,6.15 -1678476477.264379,6.53 -1678476477.279861,6.91 -1678476477.29367,7.21 -1678476477.305133,7.47 -1678476477.315147,7.72 -1678476477.332855,7.97 -1678476477.34499,8.1 -1678476477.357981,8.11 -1678476477.368056,8.03 -1678476477.383054,7.84 -1678476477.393054,7.63 -1678476477.408052,7.39 -1678476477.423099,7.11 -1678476477.434505,6.81 -1678476477.447721,6.51 -1678476477.462706,6.25 -1678476477.475783,5.98 -1678476477.488756,5.77 -1678476477.501747,5.58 -1678476477.514863,5.41 -1678476477.526796,5.32 -1678476477.53869,5.44 -1678476477.553487,5.55 -1678476477.564497,5.56 -1678476477.578002,5.47 -1678476477.588191,5.24 -1678476477.602732,4.87 -1678476477.617749,4.46 -1678476477.627744,4.36 -1678476477.643112,4.65 -1678476477.653121,4.89 -1678476477.668099,4.85 -1678476477.679143,4.51 -1678476477.694132,4.15 -1678476477.704119,4.0 -1678476477.722173,4.11 -1678476477.734225,4.36 -1678476477.748464,4.24 -1678476477.754626,3.68 -1678476477.774488,2.99 -1678476477.787864,2.13 -1678476477.797894,2.16 -1678476477.812889,3.09 -1678476477.822887,3.83 -1678476477.838993,3.9 -1678476477.85262,3.63 -1678476477.865483,3.31 -1678476477.878623,3.22 -1678476477.891618,3.29 -1678476477.90477,3.58 -1678476477.916636,3.87 -1678476477.926629,4.0 -1678476477.939268,3.93 -1678476477.953327,3.66 -1678476477.969408,3.18 -1678476477.982398,2.72 -1678476477.995606,2.49 -1678476478.008618,2.38 -1678476478.017731,2.24 -1678476478.032734,2.07 -1678476478.047714,1.9 -1678476478.057893,1.81 -1678476478.072891,1.74 -1678476478.08289,1.66 -1678476478.09791,1.6 -1678476478.10901,1.46 -1678476478.123015,1.26 -1678476478.138028,1.13 -1678476478.148051,1.01 -1678476478.163217,0.9 -1678476478.173164,0.73 -1678476478.18818,0.61 -1678476478.203552,0.53 -1678476478.21464,0.34 -1678476478.226455,0.27 -1678476478.24145,0.14 -1678476478.253049,-0.1 -1678476478.269144,-0.35 -1678476478.279122,-0.66 -1678476478.291065,-1.0 -1678476478.30412,-1.31 -1678476478.319117,-1.55 -1678476478.334115,-1.78 -1678476478.344265,-2.1 -1678476478.35817,-2.47 -1678476478.373291,-2.82 -1678476478.385898,-2.95 -1678476478.398899,-3.06 -1678476478.411883,-3.41 -1678476478.424874,-3.83 -1678476478.438036,-4.17 -1678476478.448174,-4.73 -1678476478.464801,-5.2 -1678476478.476795,-5.0 -1678476478.489898,-4.56 -1678476478.502881,-4.46 -1678476478.513016,-4.81 -1678476478.527984,-5.4 -1678476478.537982,-5.88 -1678476478.552982,-5.87 -1678476478.566754,-5.44 -1678476478.577878,-4.96 -1678476478.59288,-4.62 -1678476478.602897,-4.53 -1678476478.617734,-4.54 -1678476478.632705,-4.37 -1678476478.644373,-3.67 -1678476478.657769,-2.66 -1678476478.667847,-1.93 -1678476478.681427,-1.77 -1678476478.693999,-1.89 -1678476478.708995,-1.86 -1678476478.723993,-1.6 -1678476478.733988,-1.12 -1678476478.748154,-0.59 -1678476478.759246,-0.26 -1678476478.776157,-0.16 -1678476478.787247,-0.15 -1678476478.802246,0.07 -1678476478.813997,0.61 -1678476478.823994,1.38 -1678476478.836256,2.09 -1678476478.853257,2.48 -1678476478.862771,2.44 -1678476478.877532,2.27 -1678476478.888264,2.33 -1678476478.904377,2.48 -1678476478.918875,2.54 -1678476478.931886,2.42 -1678476478.944992,2.41 -1678476478.957789,2.66 -1678476478.970761,2.85 -1678476478.983756,2.83 -1678476478.995866,2.59 -1678476479.009934,2.26 -1678476479.019495,2.18 -1678476479.035902,2.31 -1678476479.04803,2.43 -1678476479.058079,2.44 -1678476479.07412,2.32 -1678476479.084112,2.17 -1678476479.100143,1.98 -1678476479.11328,2.01 -1678476479.124251,2.21 -1678476479.139374,2.34 -1678476479.152939,2.12 -1678476479.162938,1.71 -1678476479.174128,1.4 -1678476479.189118,1.25 -1678476479.203145,1.05 -1678476479.213124,0.95 -1678476479.228123,1.08 -1678476479.243458,1.21 -1678476479.254628,1.07 -1678476479.265752,0.69 +1678477531.607055,0.27 +1678477531.612044,0.26 +1678477531.625057,0.27 +1678477531.638737,0.26 +1678477531.651784,0.27 +1678477531.663801,0.27 +1678477531.67761,0.26 +1678477531.690603,0.27 +1678477531.703596,0.27 +1678477531.716291,0.27 +1678477531.729438,0.26 +1678477531.74243,0.26 +1678477531.755555,0.27 +1678477531.767528,0.26 +1678477531.781539,0.27 +1678477531.793793,0.27 +1678477531.806685,0.27 +1678477531.820694,0.27 +1678477531.833685,0.26 +1678477531.845846,0.26 +1678477531.858857,0.26 +1678477531.871932,0.27 +1678477531.885759,0.27 +1678477531.897786,0.27 +1678477531.911789,0.27 +1678477531.923781,0.27 +1678477531.936884,0.28 +1678477531.950043,0.27 +1678477531.963056,0.28 +1678477531.976181,0.27 +1678477531.989449,0.27 +1678477532.002441,0.27 +1678477532.015434,0.27 +1678477532.028553,0.27 +1678477532.041545,0.27 +1678477532.054662,0.27 +1678477532.067408,0.27 +1678477532.080412,0.28 +1678477532.093561,0.28 +1678477532.106567,0.28 +1678477532.119286,0.28 +1678477532.1323,0.28 +1678477532.14527,0.28 +1678477532.158264,0.28 +1678477532.171277,0.28 +1678477532.184287,0.28 +1678477532.196899,0.27 +1678477532.209898,0.28 +1678477532.222915,0.27 +1678477532.236662,0.28 +1678477532.248693,0.28 +1678477532.262665,0.28 +1678477532.274682,0.27 +1678477532.287704,0.27 +1678477532.300796,0.27 +1678477532.314815,0.26 +1678477532.32689,0.26 +1678477532.340564,0.26 +1678477532.353558,0.26 +1678477532.366247,0.27 +1678477532.379445,0.26 +1678477532.392455,0.28 +1678477532.405563,0.28 +1678477532.417686,0.28 +1678477532.430657,0.28 +1678477532.4438,0.28 +1678477532.456943,0.28 +1678477532.469958,0.28 +1678477532.483061,0.28 +1678477532.495828,0.28 +1678477532.508925,0.28 +1678477532.521929,0.28 +1678477532.534916,0.28 +1678477532.548161,0.27 +1678477532.561178,0.27 +1678477532.574146,0.28 +1678477532.587161,0.28 +1678477532.600164,0.28 +1678477532.613155,0.28 +1678477532.626151,0.28 +1678477532.639663,0.28 +1678477532.65268,0.28 +1678477532.664786,0.28 +1678477532.678666,0.28 +1678477532.690683,0.28 +1678477532.703819,0.29 +1678477532.717702,0.29 +1678477532.730694,0.27 +1678477532.743687,0.28 +1678477532.75668,0.26 +1678477532.769673,0.27 +1678477532.782665,0.26 +1678477532.795657,0.27 +1678477532.808528,0.26 +1678477532.821519,0.27 +1678477532.834512,0.26 +1678477532.847412,0.26 +1678477532.860295,0.27 +1678477532.873289,0.26 +1678477532.886281,0.26 +1678477532.899273,0.26 +1678477532.912266,0.26 +1678477532.92526,0.27 +1678477532.938232,0.26 +1678477532.951272,0.27 +1678477532.964264,0.27 +1678477532.97726,0.26 +1678477532.990252,0.27 +1678477533.003253,0.27 +1678477533.016244,0.27 +1678477533.029757,0.27 +1678477533.042754,0.27 +1678477533.055378,0.26 +1678477533.068539,0.27 +1678477533.08155,0.27 +1678477533.094526,0.27 +1678477533.10756,0.27 +1678477533.120532,0.27 +1678477533.133644,0.27 +1678477533.146396,0.27 +1678477533.159288,0.27 +1678477533.172305,0.26 +1678477533.185277,0.28 +1678477533.198236,0.27 +1678477533.211275,0.27 +1678477533.224268,0.27 +1678477533.237792,0.26 +1678477533.250791,0.26 +1678477533.263401,0.26 +1678477533.276395,0.25 +1678477533.289387,0.25 +1678477533.30238,0.26 +1678477533.315371,0.27 +1678477533.328366,0.27 +1678477533.341358,0.28 +1678477533.35456,0.27 +1678477533.366931,0.27 +1678477533.379929,0.28 +1678477533.392916,0.27 +1678477533.405839,0.27 +1678477533.418933,0.27 +1678477533.43204,0.27 +1678477533.445337,0.28 +1678477533.458329,0.28 +1678477533.471301,0.28 +1678477533.484421,0.28 +1678477533.497412,0.36 +1678477533.510405,0.41 +1678477533.523535,0.49 +1678477533.536552,0.34 +1678477533.548566,0.11 +1678477533.56169,0.06 +1678477533.575341,-0.06 +1678477533.588415,0.01 +1678477533.601562,0.15 +1678477533.614575,0.28 +1678477533.626831,0.36 +1678477533.640445,0.56 +1678477533.653439,0.96 +1678477533.666312,1.43 +1678477533.679411,1.59 +1678477533.692424,1.5 +1678477533.704741,1.62 +1678477533.718565,2.13 +1678477533.730764,2.62 +1678477533.743916,2.9 +1678477533.757056,2.92 +1678477533.770569,3.04 +1678477533.783539,3.34 +1678477533.79654,3.43 +1678477533.808945,3.46 +1678477533.822059,4.14 +1678477533.835077,4.48 +1678477533.848189,4.57 +1678477533.861316,4.12 +1678477533.874391,3.66 +1678477533.886844,3.45 +1678477533.899957,3.41 +1678477533.91293,3.98 +1678477533.925772,4.27 +1678477533.938742,3.8 +1678477533.951742,3.09 +1678477533.965332,2.64 +1678477533.978319,2.78 +1678477533.991332,3.24 +1678477534.004438,3.1 +1678477534.017563,3.04 +1678477534.030534,3.06 +1678477534.043527,3.36 +1678477534.056532,3.15 +1678477534.06941,2.77 +1678477534.082402,2.7 +1678477534.095394,2.77 +1678477534.108543,2.71 +1678477534.121535,2.53 +1678477534.134536,2.32 +1678477534.146552,2.2 +1678477534.16073,2.13 +1678477534.173486,2.05 +1678477534.186797,2.05 +1678477534.198808,1.99 +1678477534.212787,1.83 +1678477534.225785,1.62 +1678477534.237835,1.47 +1678477534.250915,1.41 +1678477534.263738,1.3 +1678477534.276836,1.17 +1678477534.290386,1.05 +1678477534.303383,0.82 +1678477534.316311,0.62 +1678477534.329434,0.42 +1678477534.342426,0.22 +1678477534.355627,-0.02 +1678477534.368599,-0.27 +1678477534.38164,-0.55 +1678477534.394635,-0.76 +1678477534.407909,-0.89 +1678477534.420923,-0.96 +1678477534.433905,-1.06 +1678477534.446528,-1.15 +1678477534.460005,-1.05 +1678477534.473055,-0.75 +1678477534.485671,-0.4 +1678477534.498677,-0.11 +1678477534.511671,0.11 +1678477534.523689,0.19 +1678477534.5378,0.34 +1678477534.549789,0.57 +1678477534.562782,0.92 +1678477534.575912,1.25 +1678477534.588904,1.4 +1678477534.601899,1.33 +1678477534.614932,1.13 +1678477534.627902,1.01 +1678477534.641317,0.93 +1678477534.654311,0.88 +1678477534.667302,0.77 +1678477534.680295,0.63 +1678477534.693287,0.48 +1678477534.706245,0.24 +1678477534.719308,0.04 +1678477534.732301,-0.12 +1678477534.745415,-0.3 +1678477534.758542,-0.48 +1678477534.771535,-0.67 +1678477534.784642,-0.88 +1678477534.79792,-1.12 +1678477534.810441,-1.35 +1678477534.823519,-1.51 +1678477534.837514,-1.68 +1678477534.84951,-1.82 +1678477534.862665,-1.78 +1678477534.875683,-1.66 +1678477534.888664,-1.65 +1678477534.901665,-1.86 +1678477534.914682,-2.09 +1678477534.927824,-2.26 +1678477534.940945,-2.3 +1678477534.953963,-2.3 +1678477534.966736,-2.32 +1678477534.979813,-2.33 +1678477534.992291,-2.38 +1678477535.005332,-2.46 +1678477535.018558,-2.41 +1678477535.031535,-2.33 +1678477535.044533,-2.33 +1678477535.057526,-2.42 +1678477535.071022,-2.4 +1678477535.084145,-2.36 +1678477535.096931,-2.35 +1678477535.109915,-2.38 +1678477535.122931,-2.41 +1678477535.135707,-2.36 +1678477535.148503,-2.27 +1678477535.161663,-2.26 +1678477535.174357,-2.29 +1678477535.187329,-2.29 +1678477535.200394,-2.2 +1678477535.213389,-2.05 +1678477535.226522,-1.96 +1678477535.239543,-1.89 +1678477535.252559,-1.79 +1678477535.265249,-1.69 +1678477535.278435,-1.56 +1678477535.291429,-1.38 +1678477535.304556,-1.18 +1678477535.317944,-1.04 +1678477535.329962,-0.89 +1678477535.343076,-0.76 +1678477535.35679,-0.65 +1678477535.3698,-0.56 +1678477535.382816,-0.39 +1678477535.395928,-0.21 +1678477535.408924,-0.05 +1678477535.42194,0.13 +1678477535.433935,0.28 +1678477535.447569,0.44 +1678477535.459607,0.58 +1678477535.472663,0.71 +1678477535.486801,0.81 +1678477535.498646,0.96 +1678477535.511638,1.11 +1678477535.524656,1.25 +1678477535.538697,1.44 +1678477535.550664,1.62 +1678477535.563658,1.76 +1678477535.577061,1.87 +1678477535.590437,1.92 +1678477535.602628,1.92 +1678477535.616247,1.91 +1678477535.629308,1.92 +1678477535.642301,2.03 +1678477535.655437,2.39 +1678477535.668559,2.67 +1678477535.681555,2.64 +1678477535.694573,2.45 +1678477535.706544,2.33 +1678477535.720558,2.36 +1678477535.733289,2.48 +1678477535.746247,2.56 +1678477535.759331,2.6 +1678477535.772419,2.6 +1678477535.785413,2.59 +1678477535.798419,2.54 +1678477535.811411,2.53 +1678477535.824274,2.53 +1678477535.837419,2.52 +1678477535.850405,2.52 +1678477535.863398,2.51 +1678477535.876541,2.54 +1678477535.888692,2.56 +1678477535.901697,2.54 +1678477535.914795,2.47 +1678477535.927948,2.35 +1678477535.941237,2.27 +1678477535.954649,2.22 +1678477535.966814,2.18 +1678477535.979812,2.16 +1678477535.993301,2.14 +1678477536.005854,2.09 From 84fc6343e4816fe6239f85f0bc6e15efa567ec83 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Fri, 10 Mar 2023 23:57:35 +0300 Subject: [PATCH 09/16] added call to filter data function --- include/defs.h | 1 + include/kalman.cpp | 4 ++-- src/main.cpp | 29 +++++++++++++++++++++++++---- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/include/defs.h b/include/defs.h index 9fb80f5..299aebb 100644 --- a/include/defs.h +++ b/include/defs.h @@ -29,5 +29,6 @@ #define STACK_SIZE 2048 #define ALTIMETER_QUEUE_LENGTH 10 // todo: change to 2 items #define GYROSCOPE_QUEUE_LENGTH 10 +#define FILTERED_DATA_QUEUE_LENGTH 10 #endif \ No newline at end of file diff --git a/include/kalman.cpp b/include/kalman.cpp index 897673e..dbeb43b 100644 --- a/include/kalman.cpp +++ b/include/kalman.cpp @@ -40,7 +40,7 @@ BLA::Matrix<3, 1> x_hat = {1500.0, BLA::Matrix<2, 1> Y = {0.0, 0.0}; -// kalmanUpdate This filters our altitude and acceleration values +/* This filters our altitude and acceleration values */ struct Filtered_Data filterData(float x_acceleration){ /* this struct will store the filtered data values */ struct Filtered_Data filtered_values; @@ -68,7 +68,7 @@ struct Filtered_Data filterData(float x_acceleration){ P = (I - K * H) * P_minus; Y = Z - (H * x_hat_minus); - filtered_values.x_acceleration = x_hat(0); + filtered_values.x_acceleration = x_hat(2); // return_val.displacement = x_hat(0); // return_val.velocity = x_hat(1); diff --git a/src/main.cpp b/src/main.cpp index 0718607..7f1db72 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,6 +5,7 @@ #include #include "sensors.h" #include "defs.h" +#include "kalman.h" /* create gyroscope object */ Adafruit_MPU6050 gyroscope; @@ -57,6 +58,7 @@ struct Altimeter_Data{ * */ QueueHandle_t gyroscope_data_queue; QueueHandle_t altimeter_data_queue; +QueueHandle_t filtered_data_queue; void readAltimeter(void* pvParameters){ @@ -99,6 +101,14 @@ void readGyroscope(void* pvParameters){ gyro_data.ay = a.acceleration.y; gyro_data.az = a.acceleration.z; + /* filter the data */ + struct Filtered_Data filtered_data = filterData(gyro_data.ax); + + /* send filtered value to filtered data queue */ + if(xQueueSend(filtered_data_queue, &filtered_data, portMAX_DELAY) != pdPASS){ + debugln("[-]Filtered data queue full"); + } + /* send data to altimeter queue */ if(xQueueSend(gyroscope_data_queue, &gyro_data, portMAX_DELAY) != pdPASS){ debugln("[-]Gyro queue full"); @@ -112,9 +122,11 @@ void displayData(void* pvParameters){ while(true){ struct Acceleration_Data gyroscope_buffer; struct Altimeter_Data altimeter_buffer; + struct Filtered_Data filtered_data; if(xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS){ - debug(gyroscope_buffer.ax); debugln(); + // debug(gyroscope_buffer.ax); debugln(); + debug(filtered_data.x_acceleration); debugln(); // debug("y: "); debug(gyroscope_buffer.ay); debugln(); // debug("z: "); debug(gyroscope_buffer.az); debugln(); @@ -149,21 +161,30 @@ void setup() debugln("Creating queues..."); /* create gyroscope data queue */ gyroscope_data_queue = xQueueCreate(GYROSCOPE_QUEUE_LENGTH, sizeof(struct Acceleration_Data)); + /* create altimeter_data_queue */ altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(struct Altimeter_Data)); + filtered_data_queue = xQueueCreate(FILTERED_DATA_QUEUE_LENGTH, sizeof(struct Filtered_Data)); + /* check if the queues were created successfully */ if(gyroscope_data_queue == NULL){ - debugln("[+]Gyroscope data queue creation failed!"); + debugln("[-]Gyroscope data queue creation failed!"); } else{ debugln("[+]Gyroscope data queue creation success"); } if(altimeter_data_queue == NULL){ - debugln("[+]Altimeter data queue creation failed!"); + debugln("[-]Altimeter data queue creation failed!"); } else{ debugln("[+]Altimeter data queue creation success"); } + + if(filtered_data_queue == NULL){ + debugln("[-]Filtered data queue creation failed!"); + } else{ + debugln("[+]Filtered data queue creation success"); + } /* Create tasks * All tasks have a stack size of 1024 words - not bytes! @@ -175,7 +196,7 @@ void setup() /* TASK 1: READ ALTIMETER DATA */ if(xTaskCreate( readAltimeter, /* function that executes this task*/ - "readAltimeter",/* Function name - for debugging */ + "readAltimeter", /* Function name - for debugging */ STACK_SIZE, /* Stack depth in words */ NULL, /* parameter to be passed to the task */ tskIDLE_PRIORITY + 1, /* Task priority - in this case 1 */ From 8d79bdb4e745fd408d9b7b4b755727739c59895a Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Tue, 14 Mar 2023 13:49:10 +0300 Subject: [PATCH 10/16] added velocity integration --- include/kalman.cpp | 1 + src/main.cpp | 54 ++++++++++++++++++++++++++-------------------- 2 files changed, 32 insertions(+), 23 deletions(-) diff --git a/include/kalman.cpp b/include/kalman.cpp index dbeb43b..f477b95 100644 --- a/include/kalman.cpp +++ b/include/kalman.cpp @@ -40,6 +40,7 @@ BLA::Matrix<3, 1> x_hat = {1500.0, BLA::Matrix<2, 1> Y = {0.0, 0.0}; + /* This filters our altitude and acceleration values */ struct Filtered_Data filterData(float x_acceleration){ /* this struct will store the filtered data values */ diff --git a/src/main.cpp b/src/main.cpp index 7f1db72..14db9bc 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,7 +5,6 @@ #include #include "sensors.h" #include "defs.h" -#include "kalman.h" /* create gyroscope object */ Adafruit_MPU6050 gyroscope; @@ -13,6 +12,13 @@ Adafruit_MPU6050 gyroscope; /* create altimeter objects */ Adafruit_BMP085 altimeter; +/* acceleration integration variables */ +long long current_time = 0; +long long previous_time = 0; +long velocity = 0; + + +/* functions to initialize sensors */ void initialize_gyroscope(){ /* attempt to initialize MPU6050 */ if(!gyroscope.begin(0x68)){ @@ -46,6 +52,7 @@ struct Acceleration_Data{ float ax; float ay; float az; + long velocity; }; struct Altimeter_Data{ @@ -95,19 +102,21 @@ void readGyroscope(void* pvParameters){ struct Acceleration_Data gyro_data; /* - * Read acceleration + * Read accelerations on all axes * */ gyro_data.ax = a.acceleration.x; gyro_data.ay = a.acceleration.y; gyro_data.az = a.acceleration.z; - /* filter the data */ - struct Filtered_Data filtered_data = filterData(gyro_data.ax); + /* approximate velocity from acceleration by integration */ + current_time = millis(); + velocity = velocity + a.acceleration.y * (current_time - previous_time); + previous_time = current_time; - /* send filtered value to filtered data queue */ - if(xQueueSend(filtered_data_queue, &filtered_data, portMAX_DELAY) != pdPASS){ - debugln("[-]Filtered data queue full"); - } + + + /* assign velocity value to gyroscope data */ + gyro_data.velocity = velocity; /* send data to altimeter queue */ if(xQueueSend(gyroscope_data_queue, &gyro_data, portMAX_DELAY) != pdPASS){ @@ -122,25 +131,25 @@ void displayData(void* pvParameters){ while(true){ struct Acceleration_Data gyroscope_buffer; struct Altimeter_Data altimeter_buffer; - struct Filtered_Data filtered_data; if(xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS){ - // debug(gyroscope_buffer.ax); debugln(); - debug(filtered_data.x_acceleration); debugln(); - // debug("y: "); debug(gyroscope_buffer.ay); debugln(); - // debug("z: "); debug(gyroscope_buffer.az); debugln(); - + debugln("------------------------------"); + debug("x: "); debug(gyroscope_buffer.ax); debugln(); + debug("y: "); debug(gyroscope_buffer.ay); debugln(); + debug("z: "); debug(gyroscope_buffer.az); debugln(); + debug("velocity: "); debug(gyroscope_buffer.velocity); debugln(); + }else{ /* no queue */ } - // if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ - // debug("pressure: "); debug(altimeter_buffer.pressure); debugln(); - // debug("altitude: "); debug(altimeter_buffer.altitude); debugln(); + if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ + debug("pressure: "); debug(altimeter_buffer.pressure); debugln(); + debug("altitude: "); debug(altimeter_buffer.altitude); debugln(); - // }else{ - // /* no queue */ - // } + }else{ + /* no queue */ + } delay(10); } @@ -165,8 +174,6 @@ void setup() /* create altimeter_data_queue */ altimeter_data_queue = xQueueCreate(ALTIMETER_QUEUE_LENGTH, sizeof(struct Altimeter_Data)); - filtered_data_queue = xQueueCreate(FILTERED_DATA_QUEUE_LENGTH, sizeof(struct Filtered_Data)); - /* check if the queues were created successfully */ if(gyroscope_data_queue == NULL){ debugln("[-]Gyroscope data queue creation failed!"); @@ -204,11 +211,12 @@ void setup() ) != pdPASS){ // if task creation is not successful debugln("[-]Read-Altimeter task creation failed!"); + }else{ debugln("[+]Read-Altimeter task creation success"); } - /* TASK 2: READ GYROSCPE DATA */ + /* TASK 2: READ GYROSCOPE DATA */ if(xTaskCreate( readGyroscope, "readGyroscope", From 42c2a8c64d0b85455dffd27f7e131572ad43bf2b Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Tue, 14 Mar 2023 14:42:36 +0300 Subject: [PATCH 11/16] added acceleration --- src/main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 14db9bc..08f5d9e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,6 +16,8 @@ Adafruit_BMP085 altimeter; long long current_time = 0; long long previous_time = 0; long velocity = 0; +float old_y_acceleration = 0.0; +float new_y_acceleration = 0.0; /* functions to initialize sensors */ @@ -108,12 +110,14 @@ void readGyroscope(void* pvParameters){ gyro_data.ay = a.acceleration.y; gyro_data.az = a.acceleration.z; - /* approximate velocity from acceleration by integration */ + /* approximate velocity from acceleration by integration for apogee detection */ current_time = millis(); - velocity = velocity + a.acceleration.y * (current_time - previous_time); - previous_time = current_time; - + new_y_acceleration = a.acceleration.y; + velocity = (old_y_acceleration - new_y_acceleration)/2 * (current_time - previous_time); + old_y_acceleration = new_y_acceleration; + previous_time = current_time; + old_y_acceleration = new_y_acceleration; /* assign velocity value to gyroscope data */ gyro_data.velocity = velocity; From e359298b1a345a13085faf18ea3162cd3b9b382a Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Tue, 14 Mar 2023 16:43:59 +0300 Subject: [PATCH 12/16] added apogee detection using altitude values --- include/defs.h | 2 ++ src/main.cpp | 66 +++++++++++++++++++++++++++++++------------------- 2 files changed, 43 insertions(+), 25 deletions(-) diff --git a/include/defs.h b/include/defs.h index 299aebb..af20053 100644 --- a/include/defs.h +++ b/include/defs.h @@ -24,6 +24,7 @@ /* flight constants */ #define EJECTION_HEIGHT 1000 // eject at 1000m AGL #define SEA_LEVEL_PRESSURE 101325 // Assume the sea level pressure is 101325 Pascals - this can change with weather +#define ALTITUDE_OFFSET 1500 /* this value is the altitude at rocket launch site */ /* tasks constants */ #define STACK_SIZE 2048 @@ -31,4 +32,5 @@ #define GYROSCOPE_QUEUE_LENGTH 10 #define FILTERED_DATA_QUEUE_LENGTH 10 + #endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 08f5d9e..710c86f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,13 +12,19 @@ Adafruit_MPU6050 gyroscope; /* create altimeter objects */ Adafruit_BMP085 altimeter; -/* acceleration integration variables */ +/* integration variables */ long long current_time = 0; long long previous_time = 0; -long velocity = 0; -float old_y_acceleration = 0.0; -float new_y_acceleration = 0.0; +/* velocity integration variables */ +double y_velocity = 0; +double y_displacement = 0; + +float new_y_displacement = 0.0; +float old_y_displacement = 0.0; +double old_y_velocity = 0.0; +double new_y_velocity = 0.0; +double total_y_displacement = 0.0; /* functions to initialize sensors */ void initialize_gyroscope(){ @@ -54,12 +60,14 @@ struct Acceleration_Data{ float ax; float ay; float az; - long velocity; + }; struct Altimeter_Data{ int32_t pressure; float altitude; + double y_velocity; + double total_y_displacement; }; /* create queue to store altimeter data @@ -75,20 +83,39 @@ void readAltimeter(void* pvParameters){ /* Read pressure. * This is the pressure from the sea level. * */ - struct Altimeter_Data alt_data; + struct Altimeter_Data altimeter_data; /* Read pressure * This is the pressure from the sea level * */ - alt_data.pressure = altimeter.readSealevelPressure(); + altimeter_data.pressure = altimeter.readSealevelPressure(); /* Read altitude * This is the altitude from the sea level * */ - alt_data.altitude = altimeter.readAltitude(SEA_LEVEL_PRESSURE); + altimeter_data.altitude = altimeter.readAltitude(SEA_LEVEL_PRESSURE); + + /*------------- APOGEE DETECTION ALGORITHM -------------------------------------*/ + + /* approximate velocity from acceleration by integration for apogee detection */ + current_time = millis(); + + /* differentiate displacement to get velocity */ + new_y_displacement = altimeter.readAltitude(SEA_LEVEL_PRESSURE) - ALTITUDE_OFFSET; + y_velocity = (new_y_displacement - old_y_displacement) / (current_time - previous_time); + + /* update integration variables */ + old_y_displacement = new_y_displacement; + previous_time = current_time; + + /* ------------------------ END OF APOGEE DETECTION ALGORITHM ------------------------ */ + + /* update altimeter data */ + altimeter_data.y_velocity = y_velocity; + altimeter_data.total_y_displacement = total_y_displacement; /* send data to altimeter queue */ - if(xQueueSend(altimeter_data_queue, &alt_data, portMAX_DELAY) != pdPASS){ + if(xQueueSend(altimeter_data_queue, &altimeter_data, portMAX_DELAY) != pdPASS){ debugln("[-]Altimeter queue full"); } @@ -110,19 +137,7 @@ void readGyroscope(void* pvParameters){ gyro_data.ay = a.acceleration.y; gyro_data.az = a.acceleration.z; - /* approximate velocity from acceleration by integration for apogee detection */ - current_time = millis(); - new_y_acceleration = a.acceleration.y; - velocity = (old_y_acceleration - new_y_acceleration)/2 * (current_time - previous_time); - old_y_acceleration = new_y_acceleration; - - previous_time = current_time; - old_y_acceleration = new_y_acceleration; - - /* assign velocity value to gyroscope data */ - gyro_data.velocity = velocity; - - /* send data to altimeter queue */ + /* send data to gyroscope queue */ if(xQueueSend(gyroscope_data_queue, &gyro_data, portMAX_DELAY) != pdPASS){ debugln("[-]Gyro queue full"); } @@ -141,15 +156,16 @@ void displayData(void* pvParameters){ debug("x: "); debug(gyroscope_buffer.ax); debugln(); debug("y: "); debug(gyroscope_buffer.ay); debugln(); debug("z: "); debug(gyroscope_buffer.az); debugln(); - debug("velocity: "); debug(gyroscope_buffer.velocity); debugln(); }else{ /* no queue */ } if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ - debug("pressure: "); debug(altimeter_buffer.pressure); debugln(); - debug("altitude: "); debug(altimeter_buffer.altitude); debugln(); + debug("Pressure: "); debug(altimeter_buffer.pressure); debugln(); + debug("Altitude: "); debug(altimeter_buffer.altitude); debugln(); + debug("Velocity: "); debug(altimeter_buffer.y_velocity); debugln(); + debug("Total displacement: "); debug(altimeter_buffer.total_y_displacement); debugln(); }else{ /* no queue */ From a1da6387edd897de474a663a9a477f135d7ecdfb Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Wed, 15 Mar 2023 13:28:25 +0300 Subject: [PATCH 13/16] added function to transmit data to ground --- include/defs.h | 6 +++ platformio.ini | 2 + src/main.cpp | 103 ++++++++++++++++++++++++++++++++++++++++++++----- 3 files changed, 102 insertions(+), 9 deletions(-) diff --git a/include/defs.h b/include/defs.h index af20053..8f6a4c1 100644 --- a/include/defs.h +++ b/include/defs.h @@ -5,6 +5,7 @@ #define DEBUG 1 #if DEBUG == 1 + #define debug(x) Serial.print(x) #define debugln(x) Serial.println(x) #define debugf(x, y) Serial.printf(x, y) @@ -32,5 +33,10 @@ #define GYROSCOPE_QUEUE_LENGTH 10 #define FILTERED_DATA_QUEUE_LENGTH 10 +/* MQTT constants */ +#define mqtt_server "" +#define MQTT_BUFFER_SIZE 300 +#define MQTT_PORT 1883 + #endif \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 65cbc00..21c12c7 100644 --- a/platformio.ini +++ b/platformio.ini @@ -18,3 +18,5 @@ lib_deps = adafruit/Adafruit Unified Sensor@^1.1.7 adafruit/Adafruit BMP085 Library @ ^1.2.2 tomstewart89/BasicLinearAlgebra@^3.7 + Knolleary/PubSubClient@^2.8 + diff --git a/src/main.cpp b/src/main.cpp index 710c86f..82fe2f0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,6 +6,12 @@ #include "sensors.h" #include "defs.h" +/* create WIFI Client */ +WiFiClient wifi_client; + +/* create MQTT publish-subscribe client */ +PubSubClient mqtt_client; + /* create gyroscope object */ Adafruit_MPU6050 gyroscope; @@ -53,6 +59,13 @@ void initialize_altimeter(){ debugln("[+]Altimeter initialized"); } +void initializeMQTTParameters(){ + /* this functions creates an MQTT client for transmitting telemetry data */ + client.setBufferSize(MQTT_BUFFER_SIZE); + client.setServer(mqtt_server, MQTT_PORT); + +} + /* data variables */ /* gyroscope data */ @@ -60,14 +73,13 @@ struct Acceleration_Data{ float ax; float ay; float az; - }; struct Altimeter_Data{ int32_t pressure; float altitude; - double y_velocity; - double total_y_displacement; + double velocity; + double AGL; /* altitude above ground level */ }; /* create queue to store altimeter data @@ -101,18 +113,21 @@ void readAltimeter(void* pvParameters){ current_time = millis(); /* differentiate displacement to get velocity */ - new_y_displacement = altimeter.readAltitude(SEA_LEVEL_PRESSURE) - ALTITUDE_OFFSET; + new_y_displacement = altimeter_data.altitude - ALTITUDE_OFFSET; y_velocity = (new_y_displacement - old_y_displacement) / (current_time - previous_time); /* update integration variables */ - old_y_displacement = new_y_displacement; previous_time = current_time; + old_y_displacement = new_y_displacement; /* ------------------------ END OF APOGEE DETECTION ALGORITHM ------------------------ */ + /* subtract current altitude to get the maximum height reached */ + float rocket_height = altimeter_data.altitude - ALTITUDE_OFFSET; + /* update altimeter data */ - altimeter_data.y_velocity = y_velocity; - altimeter_data.total_y_displacement = total_y_displacement; + altimeter_data.velocity = y_velocity; + altimeter_data.AGL = rocket_height; /* send data to altimeter queue */ if(xQueueSend(altimeter_data_queue, &altimeter_data, portMAX_DELAY) != pdPASS){ @@ -164,8 +179,8 @@ void displayData(void* pvParameters){ if(xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS){ debug("Pressure: "); debug(altimeter_buffer.pressure); debugln(); debug("Altitude: "); debug(altimeter_buffer.altitude); debugln(); - debug("Velocity: "); debug(altimeter_buffer.y_velocity); debugln(); - debug("Total displacement: "); debug(altimeter_buffer.total_y_displacement); debugln(); + debug("Velocity: "); debug(altimeter_buffer.velocity); debugln(); + debug("AGL: "); debug(altimeter_buffer.AGL); debugln(); }else{ /* no queue */ @@ -175,7 +190,60 @@ void displayData(void* pvParameters){ } } +void publishMQTTMessage(struct Altimeter_Data altimeter_data, struct Acceleration_Data acceleration_data){ + /* This function creates the message to be sent to the ground station over MQTT*/ + char telemetry_data[300]; + + /* build telemetry string message + * The data to be sent is: + * + * y-axis acceleration + * Velocity + * Altitude above ground level (AGL) + * Pressure + * + * */ + sprintf(telemetry_data, + "%.3f, %.3f, %.3f, %.3f\n", + acceleration_data.ay, + altimeter_data.velocity, + altimeter_data.AGL, + altimeter_data.pressure + ); + + /* publish the data to N3/TelemetryData MQTT channel */ + client.publish("N3/TelemetryData", "Y-Acceleration, Velocity, Altitude, Pressure"); + client.publish("N3/TelemetryData", telemetry_data); + +} + +void transmitTelemetry(void* pvParameters){ + /* This function sends data to the ground station */ + while(true){ + /* create two pointers to the data structures to be transmitted */ + struct Acceleration_Data gyroscope_buffer; + struct Altimeter_Data altimeter_buffer; + + /* receive data into respective queues */ + if( (xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS) || (xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS) ){ + /* this means all the data has been received successfully into any one of the queues and is ready for transmission + * + * todo: change this AND operator. the danger is that if one queue is not able to receive, we will not receive any data + * + * */ + /* publish the data to ground */ + publishMQTTMessage(altimeter_buffer, gyroscope_buffer); + + + }else{ + + + } + + + } +} void setup() { @@ -187,7 +255,10 @@ void setup() initialize_altimeter(); // todo: initialize flash memory + initializeMQTTParameters(); + debugln("Creating queues..."); + /* create gyroscope data queue */ gyroscope_data_queue = xQueueCreate(GYROSCOPE_QUEUE_LENGTH, sizeof(struct Acceleration_Data)); @@ -264,6 +335,20 @@ void setup() debugln("[+]Display data task creation success!"); } + /* TASK 4: TRANSMIT TELEMETRY DATA */ + if(xTaskCreate( + sendTelemetryToGroundStation, + "displayData", + STACK_SIZE, + NULL, + 1, + NULL + ) != pdPASS){ + debugln("[-]Display data task creation failed!"); + }else{ + debugln("[+]Display data task creation success!"); + } + } void loop(){ From ac75c3b5b761307d7260f62836b1025fdd3395ca Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Thu, 16 Mar 2023 02:43:40 +0300 Subject: [PATCH 14/16] added mqtt transmission --- include/defs.h | 6 +++- src/main.cpp | 94 ++++++++++++++++++++++++++++++++++++-------------- 2 files changed, 73 insertions(+), 27 deletions(-) diff --git a/include/defs.h b/include/defs.h index 8f6a4c1..7f8ccfb 100644 --- a/include/defs.h +++ b/include/defs.h @@ -34,9 +34,13 @@ #define FILTERED_DATA_QUEUE_LENGTH 10 /* MQTT constants */ -#define mqtt_server "" +const char* MQTT_SERVER = "192.168.8.41"; #define MQTT_BUFFER_SIZE 300 #define MQTT_PORT 1883 +/* WIFI credentials */ +const char* SSID = "Eduh"; +const char* PASSWORD = "password2"; + #endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 82fe2f0..18a553c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,12 +1,14 @@ #include #include +#include +#include #include #include #include #include "sensors.h" #include "defs.h" -/* create WIFI Client */ +/* create Wi-Fi Client */ WiFiClient wifi_client; /* create MQTT publish-subscribe client */ @@ -59,13 +61,6 @@ void initialize_altimeter(){ debugln("[+]Altimeter initialized"); } -void initializeMQTTParameters(){ - /* this functions creates an MQTT client for transmitting telemetry data */ - client.setBufferSize(MQTT_BUFFER_SIZE); - client.setServer(mqtt_server, MQTT_PORT); - -} - /* data variables */ /* gyroscope data */ @@ -89,6 +84,28 @@ QueueHandle_t gyroscope_data_queue; QueueHandle_t altimeter_data_queue; QueueHandle_t filtered_data_queue; +void connectToWifi(){ + /* Connect to a Wi-Fi network */ + debugln("[..]Scanning for network..."); + + WiFi.begin(SSID, PASSWORD); + + while (WiFi.status() != WL_CONNECTED) + { + delay(500); + debugln("[..]Scanning for network..."); + } + + debugln("[+]Network found");debug("[+]My IP address: "); debugln(); + debugln(WiFi.localIP()); +} + +void initializeMQTTParameters(){ + /* this functions creates an MQTT client for transmitting telemetry data */ + mqtt_client.setBufferSize(MQTT_BUFFER_SIZE); + mqtt_client.setServer(MQTT_SERVER, MQTT_PORT); +} + void readAltimeter(void* pvParameters){ while(true){ @@ -212,50 +229,74 @@ void publishMQTTMessage(struct Altimeter_Data altimeter_data, struct Acceleratio ); /* publish the data to N3/TelemetryData MQTT channel */ - client.publish("N3/TelemetryData", "Y-Acceleration, Velocity, Altitude, Pressure"); - client.publish("N3/TelemetryData", telemetry_data); - + mqtt_client.publish("n3/telemetry-data", "y-acceleration, velocity, altitude, pressure"); + mqtt_client.publish("n3/telemetry-data", telemetry_data); } void transmitTelemetry(void* pvParameters){ /* This function sends data to the ground station */ while(true){ /* create two pointers to the data structures to be transmitted */ - struct Acceleration_Data gyroscope_buffer; - struct Altimeter_Data altimeter_buffer; + struct Acceleration_Data gyroscope_data_receive; + struct Altimeter_Data altimeter_data_receive; /* receive data into respective queues */ - if( (xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS) || (xQueueReceive(altimeter_data_queue, &altimeter_buffer, portMAX_DELAY) == pdPASS) ){ + if( (xQueueReceive(gyroscope_data_queue, &gyroscope_data_receive, portMAX_DELAY) == pdPASS) || (xQueueReceive(altimeter_data_queue, &altimeter_data_receive, portMAX_DELAY) == pdPASS) ){ /* this means all the data has been received successfully into any one of the queues and is ready for transmission * - * todo: change this AND operator. the danger is that if one queue is not able to receive, we will not receive any data + * todo: change this OR operator. the danger is that if one queue is not able to receive, we will not receive any data + * * * */ /* publish the data to ground */ - publishMQTTMessage(altimeter_buffer, gyroscope_buffer); +// char telemetry_data[300]; - }else{ - + /* build telemetry string message + * The data to be sent is: + * + * y-axis acceleration + * Velocity + * Altitude above ground level (AGL) + * Pressure + * + * */ +// sprintf(telemetry_data, +// "%.3f, %.3f, %.3f, %.3f\n", +// gyroscope_data_receive.ay, +// altimeter_data_receive.velocity, +// altimeter_data_receive.AGL, +// altimeter_data_receive.pressure +// ); - } + debugln("[+]Sending data"); + /* publish the data to N3/TelemetryData MQTT channel */ + mqtt_client.publish("n3/telemetry-data", "test message"); +// mqtt_client.publish("n3/telemetry-data", "y-acceleration, velocity, altitude, pressure"); +// mqtt_client.publish("N3/TelemetryData", telemetry_data); + }else{ + debugln("[-]Failed to receive data for sending"); + } } } -void setup() -{ +void setup(){ /* initialize serial */ Serial.begin(115200); + /* connect to WiFi*/ + connectToWifi(); + /* initialize sensors */ initialize_gyroscope(); initialize_altimeter(); // todo: initialize flash memory - initializeMQTTParameters(); + mqtt_client.setBufferSize(MQTT_BUFFER_SIZE); + mqtt_client.setServer(MQTT_SERVER, 1883); debugln("Creating queues..."); @@ -297,7 +338,7 @@ void setup() "readAltimeter", /* Function name - for debugging */ STACK_SIZE, /* Stack depth in words */ NULL, /* parameter to be passed to the task */ - tskIDLE_PRIORITY + 1, /* Task priority - in this case 1 */ + 2, /* Task priority - in this case 1 */ NULL /* task handle that can be passed to other tasks to reference the task */ ) != pdPASS){ // if task creation is not successful @@ -313,7 +354,7 @@ void setup() "readGyroscope", STACK_SIZE, NULL, - 1, + 2, NULL ) != pdPASS){ debugln("[-]Read-Gyroscope task creation failed!"); @@ -327,7 +368,7 @@ void setup() "displayData", STACK_SIZE, NULL, - 1, + 2, NULL ) != pdPASS){ debugln("[-]Display data task creation failed!"); @@ -337,7 +378,7 @@ void setup() /* TASK 4: TRANSMIT TELEMETRY DATA */ if(xTaskCreate( - sendTelemetryToGroundStation, + transmitTelemetry, "displayData", STACK_SIZE, NULL, @@ -352,5 +393,6 @@ void setup() } void loop(){ + mqtt_client.publish("n3/telemetry-data", "Hello from flight"); delay(1); } \ No newline at end of file From de1f570b4a1914567c9617cf0daf3c958cf0bbc6 Mon Sep 17 00:00:00 2001 From: bytecod31 Date: Wed, 22 Mar 2023 15:30:59 +0300 Subject: [PATCH 15/16] latest changes for drone test --- include/defs.h | 7 +++---- src/main.cpp | 7 +++++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/defs.h b/include/defs.h index 7f8ccfb..e9218c7 100644 --- a/include/defs.h +++ b/include/defs.h @@ -34,13 +34,12 @@ #define FILTERED_DATA_QUEUE_LENGTH 10 /* MQTT constants */ -const char* MQTT_SERVER = "192.168.8.41"; +const char* MQTT_SERVER = "192.168.0.108"; #define MQTT_BUFFER_SIZE 300 #define MQTT_PORT 1883 /* WIFI credentials */ -const char* SSID = "Eduh"; -const char* PASSWORD = "password2"; - +const char* SSID = "onboard"; +const char* PASSWORD = "123456789"; #endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 18a553c..326a8e0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -222,14 +222,16 @@ void publishMQTTMessage(struct Altimeter_Data altimeter_data, struct Acceleratio * */ sprintf(telemetry_data, "%.3f, %.3f, %.3f, %.3f\n", + acceleration_data.ax, acceleration_data.ay, + acceleration_data.az, altimeter_data.velocity, altimeter_data.AGL, altimeter_data.pressure ); /* publish the data to N3/TelemetryData MQTT channel */ - mqtt_client.publish("n3/telemetry-data", "y-acceleration, velocity, altitude, pressure"); +// mqtt_client.publish("n3/telemetry-data", "y-acceleration, velocity, altitude, pressure"); mqtt_client.publish("n3/telemetry-data", telemetry_data); } @@ -273,7 +275,8 @@ void transmitTelemetry(void* pvParameters){ debugln("[+]Sending data"); /* publish the data to N3/TelemetryData MQTT channel */ - mqtt_client.publish("n3/telemetry-data", "test message"); + + mqtt_client.publish("n3/telemetry-data", "Hello from flight!"); // mqtt_client.publish("n3/telemetry-data", "y-acceleration, velocity, altitude, pressure"); // mqtt_client.publish("N3/TelemetryData", telemetry_data); From 6c5351d3a3352cfa162adcc30582494b5217b302 Mon Sep 17 00:00:00 2001 From: RuthNaibei Date: Wed, 22 Mar 2023 23:28:46 +0300 Subject: [PATCH 16/16] changes --- include/defs.h | 2 +- platformio.ini | 3 +- src/main.cpp | 78 +++++++++++++++++++++++++++++++++++++++----------- 3 files changed, 64 insertions(+), 19 deletions(-) diff --git a/include/defs.h b/include/defs.h index e9218c7..0afbf5d 100644 --- a/include/defs.h +++ b/include/defs.h @@ -34,7 +34,7 @@ #define FILTERED_DATA_QUEUE_LENGTH 10 /* MQTT constants */ -const char* MQTT_SERVER = "192.168.0.108"; +const char* MQTT_SERVER = "192.168.1.100"; #define MQTT_BUFFER_SIZE 300 #define MQTT_PORT 1883 diff --git a/platformio.ini b/platformio.ini index 21c12c7..4d5156c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,8 +11,9 @@ [env:nodemcu-32s] platform = espressif32 board = nodemcu-32s +monitor_speed=115200 framework = arduino -upload_port = COM4 +; upload_port = COM4 lib_deps = adafruit/Adafruit MPU6050@^2.2.4 adafruit/Adafruit Unified Sensor@^1.1.7 diff --git a/src/main.cpp b/src/main.cpp index 326a8e0..b38c4b7 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,7 +12,7 @@ WiFiClient wifi_client; /* create MQTT publish-subscribe client */ -PubSubClient mqtt_client; +PubSubClient mqtt_client(wifi_client); /* create gyroscope object */ Adafruit_MPU6050 gyroscope; @@ -233,6 +233,7 @@ void publishMQTTMessage(struct Altimeter_Data altimeter_data, struct Acceleratio /* publish the data to N3/TelemetryData MQTT channel */ // mqtt_client.publish("n3/telemetry-data", "y-acceleration, velocity, altitude, pressure"); mqtt_client.publish("n3/telemetry-data", telemetry_data); + // mqtt_client } void transmitTelemetry(void* pvParameters){ @@ -286,6 +287,42 @@ void transmitTelemetry(void* pvParameters){ } } +void testTelemetry(void * pvParameter){ + while(1){ + mqtt_client.publish("n3/telemetry-data", "Hello from flight!"); + } +} + +void reconnect() +{ + while (!mqtt_client.connected()) + { + // Serial.begin(115200); + debug("Attempting MQTT connection..."); + String clientId = "FCClient-"; + clientId += String(random(0xffff), HEX); + if (mqtt_client.connect(clientId.c_str())) + { + debugln("Connected"); + mqtt_client.publish("n3/telemetry-data","Connected"); + } + else + { + debug("failed,rc="); + debug(mqtt_client.state()); + debugln(" reconnecting"); + delay(500); + } + } +} + +void callback(char *topic, byte* message, unsigned int length) { +// client.subscribe(topic); +// client.publish(inTopic, "We are on ma nigga"); + +} + + void setup(){ /* initialize serial */ Serial.begin(115200); @@ -299,7 +336,9 @@ void setup(){ // todo: initialize flash memory mqtt_client.setBufferSize(MQTT_BUFFER_SIZE); - mqtt_client.setServer(MQTT_SERVER, 1883); + mqtt_client.setServer(MQTT_SERVER, MQTT_PORT); + mqtt_client.setCallback(callback); + debugln("Creating queues..."); @@ -366,23 +405,23 @@ void setup(){ } /* TASK 3: DISPLAY DATA ON SERIAL MONITOR - FOR DEBUGGING */ - if(xTaskCreate( - displayData, - "displayData", - STACK_SIZE, - NULL, - 2, - NULL - ) != pdPASS){ - debugln("[-]Display data task creation failed!"); - }else{ - debugln("[+]Display data task creation success!"); - } +// if(xTaskCreate( +// displayData, +// "displayData", +// STACK_SIZE, +// NULL, +// 2, +// NULL +// ) != pdPASS){ +// debugln("[-]Display data task creation failed!"); +// }else{ +// debugln("[+]Display data task creation success!"); +// } /* TASK 4: TRANSMIT TELEMETRY DATA */ if(xTaskCreate( - transmitTelemetry, - "displayData", + testTelemetry, + "test", STACK_SIZE, NULL, 1, @@ -397,5 +436,10 @@ void setup(){ void loop(){ mqtt_client.publish("n3/telemetry-data", "Hello from flight"); + if (!mqtt_client.connected()) // Reconnect if connection is lost + { + reconnect(); + } + mqtt_client.loop(); delay(1); -} \ No newline at end of file +}