diff --git a/data-logger.py b/data-logger.py new file mode 100644 index 0000000..451f90f --- /dev/null +++ b/data-logger.py @@ -0,0 +1,60 @@ +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 + 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() + + +def read_serial_data(): + + # create Serial object + 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/include/defs.h b/include/defs.h new file mode 100644 index 0000000..0afbf5d --- /dev/null +++ b/include/defs.h @@ -0,0 +1,45 @@ +#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 +#define TASK_DELAY 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 +#define ALTITUDE_OFFSET 1500 /* this value is the altitude at rocket launch site */ + +/* tasks constants */ +#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 + +/* MQTT constants */ +const char* MQTT_SERVER = "192.168.1.100"; +#define MQTT_BUFFER_SIZE 300 +#define MQTT_PORT 1883 + +/* WIFI credentials */ +const char* SSID = "onboard"; +const char* PASSWORD = "123456789"; + +#endif \ No newline at end of file diff --git a/include/kalman.cpp b/include/kalman.cpp new file mode 100644 index 0000000..f477b95 --- /dev/null +++ b/include/kalman.cpp @@ -0,0 +1,79 @@ +#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}; + + +/* 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(2); + + // 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/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/platformio.ini b/platformio.ini index 75725ee..4d5156c 100644 --- a/platformio.ini +++ b/platformio.ini @@ -11,4 +11,13 @@ [env:nodemcu-32s] platform = espressif32 board = nodemcu-32s +monitor_speed=115200 framework = arduino +; 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 + tomstewart89/BasicLinearAlgebra@^3.7 + Knolleary/PubSubClient@^2.8 + diff --git a/sensor-data.csv b/sensor-data.csv new file mode 100644 index 0000000..ba018ca --- /dev/null +++ b/sensor-data.csv @@ -0,0 +1,340 @@ +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 diff --git a/src/main.cpp b/src/main.cpp index d615472..b38c4b7 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,11 +1,445 @@ #include +#include +#include +#include +#include +#include +#include +#include "sensors.h" +#include "defs.h" -void setup() -{ - // TODO: Implement +/* create Wi-Fi Client */ +WiFiClient wifi_client; + +/* create MQTT publish-subscribe client */ +PubSubClient mqtt_client(wifi_client); + +/* create gyroscope object */ +Adafruit_MPU6050 gyroscope; + +/* create altimeter objects */ +Adafruit_BMP085 altimeter; + +/* integration variables */ +long long current_time = 0; +long long previous_time = 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(){ + /* attempt to initialize MPU6050 */ + if(!gyroscope.begin(0x68)){ + 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); + + delay(SETUP_DELAY); +} + +void initialize_altimeter(){ + if (!altimeter.begin()) { + debugln("[-]Could not find a valid altimeter sensor"); + while (1) {} + + } + + debugln("[+]Altimeter initialized"); +} + +/* data variables */ +/* gyroscope data */ + +struct Acceleration_Data{ + float ax; + float ay; + float az; +}; + +struct Altimeter_Data{ + int32_t pressure; + float altitude; + double velocity; + double AGL; /* altitude above ground level */ +}; + +/* create queue to store altimeter data + * store pressure and altitude + * */ +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 loop() +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){ + /* Read pressure. + * This is the pressure from the sea level. + * */ + struct Altimeter_Data altimeter_data; + + /* Read pressure + * This is the pressure from the sea level + * */ + altimeter_data.pressure = altimeter.readSealevelPressure(); + + /* Read altitude + * This is the altitude from the sea level + * */ + 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_data.altitude - ALTITUDE_OFFSET; + y_velocity = (new_y_displacement - old_y_displacement) / (current_time - previous_time); + + /* update integration variables */ + 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.velocity = y_velocity; + altimeter_data.AGL = rocket_height; + + /* send data to altimeter queue */ + if(xQueueSend(altimeter_data_queue, &altimeter_data, portMAX_DELAY) != pdPASS){ + debugln("[-]Altimeter queue full"); + } + + delay(TASK_DELAY); + } +} + +void readGyroscope(void* pvParameters){ + + while(true){ + sensors_event_t a, g, temp; + gyroscope.getEvent(&a, &g, &temp); + + struct Acceleration_Data gyro_data; + /* + * Read accelerations on all axes + * */ + gyro_data.ax = a.acceleration.x; + gyro_data.ay = a.acceleration.y; + gyro_data.az = a.acceleration.z; + + /* send data to gyroscope queue */ + if(xQueueSend(gyroscope_data_queue, &gyro_data, portMAX_DELAY) != pdPASS){ + debugln("[-]Gyro queue full"); + } + + delay(TASK_DELAY); + } +} + +void displayData(void* pvParameters){ + while(true){ + struct Acceleration_Data gyroscope_buffer; + struct Altimeter_Data altimeter_buffer; + + if(xQueueReceive(gyroscope_data_queue, &gyroscope_buffer, portMAX_DELAY) == pdPASS){ + debugln("------------------------------"); + 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(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.velocity); debugln(); + debug("AGL: "); debug(altimeter_buffer.AGL); debugln(); + + }else{ + /* no queue */ + } + + delay(10); + } +} + +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.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", telemetry_data); + // mqtt_client +} + +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_data_receive; + struct Altimeter_Data altimeter_data_receive; + + /* receive data into respective queues */ + 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 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 */ + +// 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", +// 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", "Hello from flight!"); +// 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 testTelemetry(void * pvParameter){ + while(1){ + mqtt_client.publish("n3/telemetry-data", "Hello from flight!"); + } +} + +void reconnect() { - // TODO: Implement -} \ No newline at end of file + 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); + + /* connect to WiFi*/ + connectToWifi(); + + /* initialize sensors */ + initialize_gyroscope(); + initialize_altimeter(); + // todo: initialize flash memory + + mqtt_client.setBufferSize(MQTT_BUFFER_SIZE); + mqtt_client.setServer(MQTT_SERVER, MQTT_PORT); + mqtt_client.setCallback(callback); + + + 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)); + + /* 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"); + } + + if(altimeter_data_queue == NULL){ + 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! + * 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( + 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 */ + 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 + debugln("[-]Read-Altimeter task creation failed!"); + + }else{ + debugln("[+]Read-Altimeter task creation success"); + } + + /* TASK 2: READ GYROSCOPE DATA */ + if(xTaskCreate( + readGyroscope, + "readGyroscope", + STACK_SIZE, + NULL, + 2, + NULL + ) != pdPASS){ + debugln("[-]Read-Gyroscope task creation failed!"); + } else{ + debugln("[+]Read-Gyroscope task creation success!"); + } + + /* 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!"); +// } + + /* TASK 4: TRANSMIT TELEMETRY DATA */ + if(xTaskCreate( + testTelemetry, + "test", + STACK_SIZE, + NULL, + 1, + NULL + ) != pdPASS){ + debugln("[-]Display data task creation failed!"); + }else{ + debugln("[+]Display data task creation success!"); + } + +} + +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); +}