From baf9c02afccb1a111f0d47aa9786c413a0cc6af5 Mon Sep 17 00:00:00 2001 From: Mike-Kimani Date: Mon, 27 Jun 2022 21:40:10 +0300 Subject: [PATCH] Commits reflecting Launch branch on old N2-AvionicsFlightSoftware at this time --- .gitignore | 5 + .vscode/extensions.json | 10 ++ README.md | 63 +++++++++++ data/flightdata.txt | 0 include/checkState.h | 120 ++++++++++++++++++++ include/defs.h | 128 +++++++++++++++++++++ include/functions.h | 55 +++++++++ include/kalmanfilter.h | 78 +++++++++++++ include/logdata.h | 88 +++++++++++++++ include/readsensors.h | 191 ++++++++++++++++++++++++++++++++ include/transmitwifi.h | 112 +++++++++++++++++++ lib/README | 46 ++++++++ platformio.ini | 29 +++++ src/main.cpp | 239 ++++++++++++++++++++++++++++++++++++++++ test/README | 11 ++ 15 files changed, 1175 insertions(+) create mode 100644 .gitignore create mode 100644 .vscode/extensions.json create mode 100644 README.md create mode 100644 data/flightdata.txt create mode 100644 include/checkState.h create mode 100644 include/defs.h create mode 100644 include/functions.h create mode 100644 include/kalmanfilter.h create mode 100644 include/logdata.h create mode 100644 include/readsensors.h create mode 100644 include/transmitwifi.h create mode 100644 lib/README create mode 100644 platformio.ini create mode 100644 src/main.cpp create mode 100644 test/README diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 0000000..080e70d --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,10 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/README.md b/README.md new file mode 100644 index 0000000..e944404 --- /dev/null +++ b/README.md @@ -0,0 +1,63 @@ +# N2-flight-software +## Introduction +This repository contains code used for flight software for Nakuja Rocket N2. The following description provides a detailed +description of program flow, project structure, library dependencies and/or any other necessary documentation needed to run this +code successfully. + +## Project structure +*** + +```asm +├── data +│ └── flightdata.txt +├── include +│ ├── checkState.h +│ ├── defs.h +│ ├── functions.h +│ ├── kalmanfilter.h +│ ├── logdata.h +│ ├── readsensors.h +│ └── transmitwifi.h +├── lib +│ └── README +├── LICENSE +├── platformio.ini +├── README.md +├── src +│ └── main.cpp +└── test + └── README +``` + +### 1. Folders Description +| Folder | Description | +|---|---| +| include | Contains header files | +| lib | Project libraries| +|src| Contains main.cpp source file run by the flight computer | +|test| Contains unit test files | + +### 2. Files Description +| File | Description | +| ---------| ----------- | +|defs.h | Contains constants declaration | +|functions.h| Contains general utility functions and prototype function declarations | +|logdata.h | code for SD card mounting and logging | +|checkState.h| Contains state machine logic | +|kalmanfilter.h | Contains kalman filter for accelerometer values | +|readsensors.h | Contains code for sensors on board including the MPU,BMP,GPS | +|transmitwifi.h | Contains WiFi set-up and MQTT code | + + +## Flight Logic +The flight program is structured as a state machine with the following states: adopted from BPS space +| State | Description | Waiting for event | +|---|---| -----| +| 0 | Pre-Flight Ground | 20m Above Ground Level | +| 1 | Powered Flight | accelerationZ <=2m/s2 for 0.1s | +| 2 | Coasting | velecity less than zero to detect apogee| +| 3 | Ballistic Descent | 20m displacement below apogee to deploy parachute | +| 4 | Chute Descent| < 20m Above Ground| +| 5 | Post-flight Ground | No more events | + + diff --git a/data/flightdata.txt b/data/flightdata.txt new file mode 100644 index 0000000..e69de29 diff --git a/include/checkState.h b/include/checkState.h new file mode 100644 index 0000000..c14b197 --- /dev/null +++ b/include/checkState.h @@ -0,0 +1,120 @@ +#ifndef CHECKSTATE_H +#define CHECKSTATE_H + +#include "functions.h" +#include "defs.h" + +// variable for detected apogee height +float MAX_ALTITUDE = 0; + +// This checks that we have started ascent +// If we have a positive 20 metres displacement upwards +int checkInPoweredFlight(float altitude) +{ + float displacement = altitude - BASE_ALTITUDE; + if (displacement > GROUND_STATE_DISPLACEMENT) + { + return POWERED_FLIGHT_STATE; + } + else + { + return PRE_FLIGHT_GROUND_STATE; + } +} + +// This detects fuel burnout +// if z-acceleration is less than or equals 2m/s^2 +int checkForBurnOut(float acceleration) +{ + if (acceleration <= 2) + { + return COASTING_STATE; + } + else + { + return POWERED_FLIGHT_STATE; + } +} + +// This checks that we have reached apogee +// At apogee velocity is zero so we check for velocity less than or equal to zero +// As redundancy we check if previous altitude is greater than current altitude +int checkForApogee(float velocity, float currentAltitude, float previousAltitude) +{ + if (currentAltitude < previousAltitude) + { + + MAX_ALTITUDE = currentAltitude; + return BALLISTIC_DESCENT_STATE; + } + else if (velocity <= 0) + { + MAX_ALTITUDE = currentAltitude; + return BALLISTIC_DESCENT_STATE; + } + else + { + return COASTING_STATE; + } +} + +// Deploys parachute if we moved down 20 metres below apogee +int deployChute(float altitude) +{ + float displacement = MAX_ALTITUDE - altitude; + if (displacement > BELOW_APOGEE_LEVEL_DISPLACEMENT) + { + // Fires ejection charge + ejection(); + return CHUTE_DESCENT_STATE; + } + else + { + return BALLISTIC_DESCENT_STATE; + } +} + +// This checks that we have reached the ground +// detects landing of the rocket +// TODO: BASE_ALTITUDE might be different from the original base altitude +int checkGround(float altitude) +{ + float displacement = altitude - BASE_ALTITUDE; + if (displacement < GROUND_STATE_DISPLACEMENT) + { + return POST_FLIGHT_GROUND_STATE; + } + else + { + return CHUTE_DESCENT_STATE; + } +} + +// Updates the state-machine state +// We check if rocket has launched to move from PRE_FLIGHT_GROUND_STATE to POWERED_FLIGHT_STATE +// We check if fuel has been burnt completely to move to COASTING_STATE +// We check if we have reached apogee to move to BALLISTIC_DESCENT_STATE +// We deploy parachute to move to CHUTE_DESCENT_STATE +// We check if we have reached the ground to move to POST_FLIGHT_GROUND_STATE +int checkState(float currentAltitude, float previousAltitude, float velocity, float acceleration, int state) +{ + switch (state) + { + case PRE_FLIGHT_GROUND_STATE: + return checkInPoweredFlight(currentAltitude); + case POWERED_FLIGHT_STATE: + return checkForBurnOut(acceleration); + case COASTING_STATE: + return checkForApogee(velocity, currentAltitude, previousAltitude); + case BALLISTIC_DESCENT_STATE: + return deployChute(currentAltitude); + case CHUTE_DESCENT_STATE: + return checkGround(currentAltitude); + case POST_FLIGHT_GROUND_STATE: + return POST_FLIGHT_GROUND_STATE; + default: + return checkInPoweredFlight(currentAltitude); + } +} + +#endif \ No newline at end of file diff --git a/include/defs.h b/include/defs.h new file mode 100644 index 0000000..c07a5be --- /dev/null +++ b/include/defs.h @@ -0,0 +1,128 @@ +#ifndef DEFINITIONS_H +#define DEFINITIONS_H + +#include +#include +#include + +#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 + +#define SEA_LEVEL_PRESSURE 102400 + +// Timing delays +#define SETUP_DELAY 5000 + +#define SHORT_DELAY 10 + +#define BAUD_RATE 115200 + +#define GPS_BAUD_RATE 9600 + +#define SD_CS_PIN 5 + +// Pin to start ejection charge +#define EJECTION_PIN 4 + +const uint8_t GPS_TX_PIN = 17; +const uint8_t GPS_RX_PIN = 16; + +const BaseType_t pro_cpu = 0; +const BaseType_t app_cpu = 1; + +const char *ssid = "S7 edge"; +const char *password = "almg76061"; + +// MQTT Broker IP address +const char *mqtt_server = "192.168.43.133"; + +const int MQQT_PORT = 1883; + +WiFiClient espClient; +PubSubClient client(espClient); + +extern float BASE_ALTITUDE; +extern float MAX_ALTITUDE; + +const int PRE_FLIGHT_GROUND_STATE = 0; +const int POWERED_FLIGHT_STATE = 1; +const int COASTING_STATE = 2; +const int BALLISTIC_DESCENT_STATE = 3; +const int CHUTE_DESCENT_STATE = 4; +const int POST_FLIGHT_GROUND_STATE = 5; + +const int GROUND_STATE_DISPLACEMENT = 20; +const int BELOW_APOGEE_LEVEL_DISPLACEMENT = 20; + +// This struct is used to save all our datapoints. +// It includes rocket altitude, accelerations in the x, y and z directions +// Gryroscope values in the x, y and z direcion +// filtered altitude, velocity and acceleration +// GPS longitude, laltitude and altitude and state +struct LogData +{ + uint64_t timeStamp; + float altitude; + float ax; + float ay; + float az; + float gx; + float gy; + float gz; + float filtered_s; + float filtered_v; + float filtered_a; + int state; + float latitude; + float longitude; + float gpsAltitude; +}; +// SensorReadings contains the measurement we are getting +// from the sensors bmp and mpu +struct SensorReadings +{ + float altitude; + float ax; + float ay; + float az; + float gx; + float gy; + float gz; +}; +// GPSReadings contains the gps informations that is +// latitude, longitude, speed, number of satellites and altitude +struct GPSReadings +{ + float latitude; + float longitude; + float speed; + int satellites; + float altitude; +}; + +// FilteredValues contains filtered values from the kalman filter +struct FilteredValues +{ + float displacement; + float velocity; + float acceleration; +}; +// SendValues contains the data points we will be sending over lora +struct SendValues +{ + uint64_t timeStamp; + float altitude; + uint16_t state; + float latitude; + float longitude; +}; + +#endif \ No newline at end of file diff --git a/include/functions.h b/include/functions.h new file mode 100644 index 0000000..d9edec7 --- /dev/null +++ b/include/functions.h @@ -0,0 +1,55 @@ +#ifndef FUNCTIONS_H +#define FUNCTIONS_H + +#include "defs.h" +#include "readsensors.h" + +void ejection(); +void ejectionTimerCallback(TimerHandle_t ejectionTimerHandle); + +// formats data that we are going to save to SD card +// We save all the data points we are collecting +struct LogData formart_SD_data(SensorReadings readings, FilteredValues filtered_values) +{ + struct LogData ld = {0}; + ld.altitude = readings.altitude; + ld.ax = readings.ax; + ld.ay = readings.ay; + ld.az = readings.az; + ld.gx = readings.gx; + ld.gy = readings.gy; + ld.gz = readings.gz; + ld.filtered_s = filtered_values.displacement; + ld.filtered_a = filtered_values.acceleration; + ld.filtered_v = filtered_values.velocity; + return ld; +} + +// formart_send_data This formats data we are going to send over LoRa +// Currently we are sending altitude, state, timeStamp, longitude and latitude +struct SendValues formart_send_data(LogData readings) +{ + struct SendValues sv = {0}; + sv.altitude = readings.altitude; + sv.state = readings.state; + sv.timeStamp = readings.timeStamp; + sv.latitude = readings.latitude; + sv.longitude = readings.longitude; + return sv; +} + +// get_base_altitude Finds the average of the current altitude from 100 readings +float get_base_altitude() +{ + float altitude = 0; + SensorReadings readings; + for (int i = 0; i < 100; i++) + { + readings = get_readings(); + altitude = altitude + readings.altitude; + } + altitude = altitude / 100.0; + debugf("Base Altitude is %.3f\n", altitude); + return altitude; +} +#endif \ No newline at end of file diff --git a/include/kalmanfilter.h b/include/kalmanfilter.h new file mode 100644 index 0000000..c6f2c91 --- /dev/null +++ b/include/kalmanfilter.h @@ -0,0 +1,78 @@ +#ifndef KALMANFILTER_H +#define KALMANFILTER_H + +#include +#include "defs.h" + +using namespace BLA; + +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 filteres our altitude and acceleration values +struct FilteredValues kalmanUpdate(float altitude, float acceleration) +{ + struct FilteredValues return_val; + + // Measurement matrix + BLA::Matrix<2, 1> Z = {altitude, + 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); + + return_val.displacement = x_hat(0); + return_val.velocity = x_hat(1); + return_val.acceleration = x_hat(2); + + return return_val; +} + +#endif \ No newline at end of file diff --git a/include/logdata.h b/include/logdata.h new file mode 100644 index 0000000..6721f4c --- /dev/null +++ b/include/logdata.h @@ -0,0 +1,88 @@ +#ifndef LOGDATA_H +#define LOGDATA_H + +#include "FS.h" +#include "SD.h" +#include + +File dataFile; + +void initSDCard() +{ + if (!SD.begin()) + { + debugln("Card Mount Failed"); + return; + } + uint8_t cardType = SD.cardType(); + + if (cardType == CARD_NONE) + { + debugln("No SD card attached"); + return; + } + debug("SD Card Type: "); + if (cardType == CARD_MMC) + { + debugln("MMC"); + } + else if (cardType == CARD_SD) + { + debugln("SDSC"); + } + else if (cardType == CARD_SDHC) + { + debugln("SDHC"); + } + else + { + debugln("UNKNOWN"); + } + uint64_t cardSize = SD.cardSize() / (1024 * 1024); + debugf("SD Card Size: %lluMB\n", cardSize); +} + +char *printSDMessage(LogData ld) +{ + // The assigned size is calculated to fit the string + char *message = (char *)pvPortMalloc(256); + if (!message) + return NULL; + snprintf(message, 256, "{\"timestamp\":%lld,\"sensor altitude\":%.3f,\"ax\":%.3f,\"ay\":%.3f,\"az\":%.3f,\"gx\":%.3f,\"gy\":%.3f,\"gz\":%.3f,\"filtered s\":%.3f,\"filtered v\":%.3f,\"filtered a\":%.3f,\"state\":%d,\"gps altitude\":%.3f,\"longitude\":%.8f,\"latitude\":%.8f}\n", ld.timeStamp, ld.altitude, ld.ax, ld.ay, ld.az, ld.gx, ld.gy, ld.gz, ld.filtered_s, ld.filtered_v, ld.filtered_a, ld.state, ld.gpsAltitude, ld.longitude, ld.latitude); + return message; +} + +// Append data to the SD card (DON'T MODIFY THIS FUNCTION) +void appendToFile(LogData ld[5]) +{ + + dataFile = SD.open("/telmetry.txt", FILE_APPEND); + if (!dataFile) + { + debugln("Failed to open file for appending"); + return; + } + char combinedMessage[1280]; + strcpy(combinedMessage, ""); + for (int i = 0; i < 5; i++) + { + + char *message = printSDMessage(ld[i]); + strcat(combinedMessage, message); + vPortFree(message); + } + + if (dataFile.println(combinedMessage)) + { + debugln("Message appended"); + } + else + { + debugln("Append failed"); + } + + dataFile.close(); +} + + +#endif diff --git a/include/readsensors.h b/include/readsensors.h new file mode 100644 index 0000000..4551e75 --- /dev/null +++ b/include/readsensors.h @@ -0,0 +1,191 @@ +#ifndef READSENSORS_H +#define READSENSORS_H + +#include +#include +#include +#include +#include +#include "defs.h" +#include +#include + + +// using uart 2 for serial communication +SoftwareSerial GPSModule(GPS_RX_PIN, GPS_TX_PIN); + +Adafruit_BMP085 bmp; +Adafruit_MPU6050 mpu; + +void init_gps() +{ + GPSModule.begin(GPS_BAUD_RATE); +} + +void init_mpu() +{ + debugln("MPU6050 test!"); + if (!mpu.begin()) + { + debugln("Could not find a valid MPU6050 sensor, check wiring!"); + while (1) + { + //TODO: add beep to notify + } + } + else + { + debugln("MPU6050 FOUND"); + } + + mpu.setAccelerometerRange(MPU6050_RANGE_8_G); + mpu.setGyroRange(MPU6050_RANGE_500_DEG); + mpu.setFilterBandwidth(MPU6050_BAND_5_HZ); +} + +void init_bmp() +{ + debugln("BMP180 INITIALIZATION"); + if (!bmp.begin()) + { + debugln("Could not find a valid BMP085 sensor, check wiring!"); + while (1) + { + // TODO: add beep to notify + } + } + else + { + debugln("BMP180 FOUND"); + } +} + +// function to initialize bmp, mpu, lora module and the sd card module +void init_sensors() +{ + init_gps(); + init_bmp(); + init_mpu(); +} + +String ConvertLat(String nmea[15]) +{ + String posneg = ""; + if (nmea[3] == "S") + { + posneg = "-"; + } + String latfirst; + float latsecond; + for (int i = 0; i < nmea[2].length(); i++) + { + if (nmea[2].substring(i, i + 1) == ".") + { + latfirst = nmea[2].substring(0, i - 2); + latsecond = nmea[2].substring(i - 2).toFloat(); + } + } + latsecond = latsecond / 60; + String CalcLat = ""; + + char charVal[9]; + dtostrf(latsecond, 4, 6, charVal); + for (int i = 0; i < sizeof(charVal); i++) + { + CalcLat += charVal[i]; + } + latfirst += CalcLat.substring(1); + latfirst = posneg += latfirst; + return latfirst; +} + +String ConvertLng(String nmea[15]) +{ + String posneg = ""; + if (nmea[5] == "W") + { + posneg = "-"; + } + + String lngfirst; + float lngsecond; + for (int i = 0; i < nmea[4].length(); i++) + { + if (nmea[4].substring(i, i + 1) == ".") + { + lngfirst = nmea[4].substring(0, i - 2); + // debugln(lngfirst); + lngsecond = nmea[4].substring(i - 2).toFloat(); + // debugln(lngsecond); + } + } + lngsecond = lngsecond / 60; + String CalcLng = ""; + char charVal[9]; + dtostrf(lngsecond, 4, 6, charVal); + for (int i = 0; i < sizeof(charVal); i++) + { + CalcLng += charVal[i]; + } + lngfirst += CalcLng.substring(1); + lngfirst = posneg += lngfirst; + return lngfirst; +} + +// Get the gps readings from the gps sensor +struct GPSReadings get_gps_readings() +{ + String nmea[15]; + int stringplace = 0; + int pos = 0; + struct GPSReadings gpsReadings; + GPSModule.flush(); + while (GPSModule.available() > 0) + { + GPSModule.read(); + } + if (GPSModule.find("$GPRMC,")) + { + debug("here"); + String tempMsg = GPSModule.readStringUntil('\n'); + for (int i = 0; i < tempMsg.length(); i++) + { + if (tempMsg.substring(i, i + 1) == ",") + { + nmea[pos] = tempMsg.substring(stringplace, i); + stringplace = i + 1; + pos++; + } + if (i == tempMsg.length() - 1) + { + nmea[pos] = tempMsg.substring(stringplace, i); + } + } + float lati = ConvertLat(nmea).toFloat(); + float lngi = ConvertLng(nmea).toFloat(); + gpsReadings.latitude = lati; + gpsReadings.longitude = lngi; + } + + return gpsReadings; +} + +// Get the sensor readings +struct SensorReadings get_readings() +{ + struct SensorReadings return_val; + sensors_event_t a, g, temp; + mpu.getEvent(&a, &g, &temp); + return_val.altitude = bmp.readAltitude(SEA_LEVEL_PRESSURE); + return_val.ax = a.acceleration.x; + return_val.ay = a.acceleration.y; + return_val.az = a.acceleration.z; + + return_val.gx = g.gyro.x; + return_val.gy = g.gyro.y; + return_val.gz = g.gyro.z; + + return return_val; +} + +#endif \ No newline at end of file diff --git a/include/transmitwifi.h b/include/transmitwifi.h new file mode 100644 index 0000000..b86945c --- /dev/null +++ b/include/transmitwifi.h @@ -0,0 +1,112 @@ +#ifndef TRANSMITWIFI_H +#define TRANSMITWIFI_H + +#include +#include "defs.h" +#include "functions.h" + +void mqttCallback(char *topic, byte *message, unsigned int length) +{ + debug("Message arrived on topic: "); + debug(topic); + debug(". Message: "); + String messageTemp; + + for (int i = 0; i < length; i++) + { + debug((char)message[i]); + messageTemp += (char)message[i]; + } + debugln(); + + // Feel free to add more if statements to control more GPIOs with MQTT + + // If a message is received on the topic esp32/output, you check if the message is either "on" or "off". + // Changes the output state according to the message + if (String(topic) == "esp32/ejection") + { + debug("Changing output to "); + if (messageTemp == "on") + { + debugln("on"); + digitalWrite(EJECTION_PIN, HIGH); + } + else if (messageTemp == "off") + { + debugln("off"); + digitalWrite(EJECTION_PIN, LOW); + } + } +} + +void setup_wifi() +{ + // Connect to a WiFi network + debugln(); + debug("Connecting to "); + debugln(ssid); + + WiFi.begin(ssid, password); + + while (WiFi.status() != WL_CONNECTED) + { + delay(500); + debug("."); + } + + debugln(""); + debugln("WiFi connected"); + debugln("IP address: "); + debugln(WiFi.localIP()); + + client.setServer(mqtt_server, MQQT_PORT); + client.setCallback(mqttCallback); +} + +void reconnect() +{ + // Loop until we're reconnected + while (!client.connected()) + { + debugln("Attempting MQTT connection..."); + // Attempt to connect + if (client.connect("ESP8266Client")) + { + debugln("connected"); + // Subscribe + client.subscribe("esp32/ejection"); + } + else + { + debug("failed, rc="); + debug(client.state()); + debugln(" try again in 50 milliseconds"); + // Wait 5 seconds before retrying + delay(50); + } + } +} + +void sendTelemetryWiFi(SendValues sv[5]) +{ + + for (int i = 0; i < 5; i++) + { + // publish whole message i json + char mqttMessage[200]; + sprintf(mqttMessage, "{\"timestamp\":%lld,\"altitude\":%.3f,\"state\":%d,\"longitude\":%.8f,\"latitude\":%.8f}", sv[i].timeStamp, sv[i].altitude, sv[i].state, sv[i].longitude, sv[i].latitude); + client.publish("esp32/message", mqttMessage); + } +} + +void handleWiFi(SendValues sv[5]) +{ + if (!client.connected()) + { + reconnect(); + } + client.loop(); + sendTelemetryWiFi(sv); +} + +#endif \ No newline at end of file diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..96b724b --- /dev/null +++ b/platformio.ini @@ -0,0 +1,29 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:nodemcu-32s] +platform = espressif32 +board = nodemcu-32s +board_build.mcu = esp32 +board_build.f_cpu = 240000000L +framework = arduino +monitor_speed = 115200 +monitor_filters = esp32_exception_decoder +lib_deps = + adafruit/Adafruit BMP085 Library@^1.2.1 + adafruit/Adafruit MPU6050@^2.0.6 + tomstewart89/BasicLinearAlgebra@^3.5 + bblanchon/ArduinoJson@^6.19.3 + sandeepmistry/LoRa@^0.8.0 + mikalhart/TinyGPSPlus@^1.0.3 + plerup/EspSoftwareSerial@^6.15.2 + knolleary/PubSubClient@^2.8 + +board_build.filesystem = littlefs diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..762c2c6 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,239 @@ +#include +#include "../include/checkState.h" +#include "../include/logdata.h" +#include "../include/readsensors.h" +#include "../include/transmitwifi.h" +#include "../include/defs.h" +#include "../include/kalmanfilter.h" + +TimerHandle_t ejectionTimerHandle = NULL; + +portMUX_TYPE mutex = portMUX_INITIALIZER_UNLOCKED; + +TaskHandle_t WiFiTelemetryTaskHandle; + +TaskHandle_t GetDataTaskHandle; + +TaskHandle_t SDWriteTaskHandle; + +TaskHandle_t GPSTaskHandle; + +// if 1 chute has been deployed +uint8_t isChuteDeployed = 0; + +float BASE_ALTITUDE = 0; + +float previousAltitude; + +volatile int state = 0; + +static uint16_t wifi_queue_length = 100; +static uint16_t sd_queue_length = 500; +static uint16_t gps_queue_length = 100; + +static QueueHandle_t wifi_telemetry_queue; +static QueueHandle_t sdwrite_queue; +static QueueHandle_t gps_queue; + +// callback for done ejection +void ejectionTimerCallback(TimerHandle_t ejectionTimerHandle) +{ + digitalWrite(EJECTION_PIN, LOW); + isChuteDeployed = 1; +} + +// Ejection fires the explosive charge using a relay or a mosfet +void ejection() +{ + if (isChuteDeployed == 0) + { + digitalWrite(EJECTION_PIN, HIGH); + // TODO: is 3 seconds enough? + ejectionTimerHandle = xTimerCreate("EjectionTimer", 3000 / portTICK_PERIOD_MS, pdFALSE, (void *)0, ejectionTimerCallback); + xTimerStart(ejectionTimerHandle, portMAX_DELAY); + } +} + +struct LogData readData() +{ + struct LogData ld = {0}; + struct SensorReadings readings = {0}; + struct FilteredValues filtered_values = {0}; + + readings = get_readings(); + + // TODO: very important to know the orientation of the altimeter + filtered_values = kalmanUpdate(readings.altitude, readings.ay); + + // using mutex to modify state + portENTER_CRITICAL(&mutex); + state = checkState(filtered_values.displacement, previousAltitude, filtered_values.velocity, filtered_values.acceleration, state); + portEXIT_CRITICAL(&mutex); + previousAltitude = filtered_values.displacement; + + ld = formart_SD_data(readings, filtered_values); + ld.state = state; + ld.timeStamp = millis(); + + return ld; +} + +/* +**********Time Taken for each Task****************** + Get Data Task - 36ms + WiFiTelemetryTask -74ms + GPS Task - 1000ms + SD Write Task - 60ms +*/ + +void GetDataTask(void *parameter) +{ + + struct LogData ld = {0}; + struct SendValues sv = {0}; + + static int droppedWiFiPackets = 0; + static int droppedSDPackets = 0; + + for (;;) + { + + ld = readData(); + sv = formart_send_data(ld); + + if (xQueueSend(wifi_telemetry_queue, (void *)&sv, 0) != pdTRUE) + { + droppedWiFiPackets++; + } + if (xQueueSend(sdwrite_queue, (void *)&ld, 0) != pdTRUE) + { + droppedSDPackets++; + } + + debugf("Dropped WiFi Packets : %d\n", droppedWiFiPackets); + debugf("Dropped SD Packets : %d\n", droppedSDPackets); + + // yield to WiFi Telemetry task + vTaskDelay(74 / portTICK_PERIOD_MS); + } +} +void readGPSTask(void *parameter) +{ + + struct GPSReadings gpsReadings = {0}; + + static int droppedGPSPackets = 0; + + for (;;) + { + gpsReadings = get_gps_readings(); + + if (xQueueSend(gps_queue, (void *)&gpsReadings, 0) != pdTRUE) + { + droppedGPSPackets++; + } + + debugf("Dropped GPS Packets : %d\n", droppedGPSPackets); + + // yield SD Write task + // TODO: increase this up from 60 to 1000 in steps of 60 to improve queue performance at the expense of GPS + //GPS will send 1 reading in 2s when set to 1000 + vTaskDelay(960 / portTICK_PERIOD_MS); + } +} + +void WiFiTelemetryTask(void *parameter) +{ + struct SendValues sv = {0}; + struct SendValues svRecords[5]; + struct GPSReadings gpsReadings = {0}; + float latitude = 0; + float longitude = 0; + + for (;;) + { + + for (int i = 0; i < 5; i++) + { + xQueueReceive(wifi_telemetry_queue, (void *)&sv, 10); + svRecords[i] = sv; + svRecords[i].latitude = latitude; + svRecords[i].longitude = longitude; + + if (xQueueReceive(gps_queue, (void *)&gpsReadings, 10) == pdTRUE) + { + latitude = gpsReadings.latitude; + longitude = gpsReadings.longitude; + } + } + + handleWiFi(svRecords); + + // yield to Get Data task + vTaskDelay(36 / portTICK_PERIOD_MS); + } +} + +void SDWriteTask(void *parameter) +{ + + struct LogData ld = {0}; + struct LogData ldRecords[5]; + struct GPSReadings gps = {0}; + float latitude = 0; + float longitude = 0; + + for (;;) + { + + for (int i = 0; i < 5; i++) + { + xQueueReceive(sdwrite_queue, (void *)&ld, 10); + + ldRecords[i] = ld; + ldRecords[i].latitude = latitude; + ldRecords[i].longitude = longitude; + + if (xQueueReceive(gps_queue, (void *)&gps, 10) == pdTRUE) + { + latitude = gps.latitude; + longitude = gps.longitude; + } + } + appendToFile(ldRecords); + + // yield to GPS Task + vTaskDelay(1000 / portTICK_PERIOD_MS); + } +} + +void setup() +{ + + Serial.begin(BAUD_RATE); + + // set up ejection pin + pinMode(EJECTION_PIN, OUTPUT); + + setup_wifi(); + + init_sensors(); + + initSDCard(); + + // get the base_altitude + BASE_ALTITUDE = get_base_altitude(); + + wifi_telemetry_queue = xQueueCreate(wifi_queue_length, sizeof(SendValues)); + sdwrite_queue = xQueueCreate(sd_queue_length, sizeof(LogData)); + gps_queue = xQueueCreate(gps_queue_length, sizeof(GPSReadings)); + + // initialize core tasks + xTaskCreatePinnedToCore(GetDataTask, "GetDataTask", 3000, NULL, 1, &GetDataTaskHandle, 0); + xTaskCreatePinnedToCore(WiFiTelemetryTask, "WiFiTelemetryTask", 4000, NULL, 1, &WiFiTelemetryTaskHandle, 0); + xTaskCreatePinnedToCore(readGPSTask, "ReadGPSTask", 3000, NULL, 1, &GPSTaskHandle, 1); + xTaskCreatePinnedToCore(SDWriteTask, "SDWriteTask", 4000, NULL, 1, &SDWriteTaskHandle, 1); +} +void loop() +{ +} \ No newline at end of file diff --git a/test/README b/test/README new file mode 100644 index 0000000..b94d089 --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Unit Testing and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/page/plus/unit-testing.html