From 4a72f0fbfa6ab6e7f0dce69f5e60612e917d8a13 Mon Sep 17 00:00:00 2001 From: Mikey Schoonmaker Date: Thu, 17 Apr 2025 16:24:59 -0400 Subject: [PATCH 1/5] init dir structure + files --- include/{ => common}/config.h | 8 ----- include/common/globals.h | 31 ++++++++++++++++ include/core/ApogeeControl.h | 4 +++ include/core/CommunicationVerification.h | 3 ++ include/core/Setup.h | 0 include/core/TelemetryProcessing.h | 0 src/ApogeeControl.cpp | 3 ++ src/CommunicationVerification.cpp | 3 ++ src/Setup.cpp | 3 ++ src/TelemetryProcessing.cpp | 3 ++ src/main.cpp | 46 ++++++++++++------------ 11 files changed, 74 insertions(+), 30 deletions(-) rename include/{ => common}/config.h (83%) create mode 100644 include/common/globals.h create mode 100644 include/core/ApogeeControl.h create mode 100644 include/core/CommunicationVerification.h create mode 100644 include/core/Setup.h create mode 100644 include/core/TelemetryProcessing.h create mode 100644 src/ApogeeControl.cpp create mode 100644 src/CommunicationVerification.cpp create mode 100644 src/Setup.cpp create mode 100644 src/TelemetryProcessing.cpp diff --git a/include/config.h b/include/common/config.h similarity index 83% rename from include/config.h rename to include/common/config.h index 006cf5c..68fdeb2 100644 --- a/include/config.h +++ b/include/common/config.h @@ -3,14 +3,6 @@ #include "ArduinoHAL.h" -#include "telemetry.h" -#include "servointerface.h" -#include "state_estimation/ApogeePredictor.h" -#include "state_estimation/BurnoutStateMachine.h" -#include "data_handling/DataNames.h" -#include "data_handling/DataPoint.h" -#include "data_handling/DataSaverBigSD.h" - #define TEST_FIN_DEPLOYMENT 1 // for testing fins // mission configuration variables diff --git a/include/common/globals.h b/include/common/globals.h new file mode 100644 index 0000000..5ca5e27 --- /dev/null +++ b/include/common/globals.h @@ -0,0 +1,31 @@ +#include "telemetry.h" +#include "servointerface.h" +#include "state_estimation/ApogeePredictor.h" +#include "state_estimation/BurnoutStateMachine.h" +#include "data_handling/DataNames.h" +#include "data_handling/DataPoint.h" +#include "data_handling/DataSaverBigSD.h" + + +VerticalVelocityEstimator* verticalVelocityEstimator; +LaunchPredictor *lp; +ApogeeDetector *ad; +ServoInterface ms24; + +// these values when initialized are going to be pointing to null val - they're better definined in setup +BurnoutStateMachine* sm = nullptr; +ApogeePredictor* ap = nullptr; // 0.2 is the alpha for the EMA, 1.0 is the minimum climb velocity + +DataSaverBigSD* dataSaver; +DataPoint aclX, aclY, aclZ, alt, temp, pres, gyroX, gyroY, gyroZ; + +Telemetry telemetry; +TelemetryData telemData; + +extern float servoAngle; +extern float targetServoAngle; +extern double baseAlt; +extern unsigned long previousTime; +extern bool sd_init; +extern uint32_t startCoastTime; +extern ServoInterface ms24; \ No newline at end of file diff --git a/include/core/ApogeeControl.h b/include/core/ApogeeControl.h new file mode 100644 index 0000000..37d7237 --- /dev/null +++ b/include/core/ApogeeControl.h @@ -0,0 +1,4 @@ +#pragma once + +void processFlightState(); +void controlDeployment(); diff --git a/include/core/CommunicationVerification.h b/include/core/CommunicationVerification.h new file mode 100644 index 0000000..ef46d7d --- /dev/null +++ b/include/core/CommunicationVerification.h @@ -0,0 +1,3 @@ +#pragma once + +void communicateVerification(bool sd_init); diff --git a/include/core/Setup.h b/include/core/Setup.h new file mode 100644 index 0000000..e69de29 diff --git a/include/core/TelemetryProcessing.h b/include/core/TelemetryProcessing.h new file mode 100644 index 0000000..e69de29 diff --git a/src/ApogeeControl.cpp b/src/ApogeeControl.cpp new file mode 100644 index 0000000..b16e0a9 --- /dev/null +++ b/src/ApogeeControl.cpp @@ -0,0 +1,3 @@ +#include "config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" diff --git a/src/CommunicationVerification.cpp b/src/CommunicationVerification.cpp new file mode 100644 index 0000000..b16e0a9 --- /dev/null +++ b/src/CommunicationVerification.cpp @@ -0,0 +1,3 @@ +#include "config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" diff --git a/src/Setup.cpp b/src/Setup.cpp new file mode 100644 index 0000000..b16e0a9 --- /dev/null +++ b/src/Setup.cpp @@ -0,0 +1,3 @@ +#include "config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" diff --git a/src/TelemetryProcessing.cpp b/src/TelemetryProcessing.cpp new file mode 100644 index 0000000..b16e0a9 --- /dev/null +++ b/src/TelemetryProcessing.cpp @@ -0,0 +1,3 @@ +#include "config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" diff --git a/src/main.cpp b/src/main.cpp index 113c25b..9d1735f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,26 +1,28 @@ #include "config.h" - -VerticalVelocityEstimator* verticalVelocityEstimator; -LaunchPredictor *lp; -ApogeeDetector *ad; -ServoInterface ms24; - -// these values when initialized are going to be pointing to null val - they're better definined in setup -BurnoutStateMachine* sm = nullptr; -ApogeePredictor* ap = nullptr; // 0.2 is the alpha for the EMA, 1.0 is the minimum climb velocity - -DataSaverBigSD* dataSaver; -DataPoint aclX, aclY, aclZ, alt, temp, pres, gyroX, gyroY, gyroZ; - -Telemetry telemetry; -TelemetryData telemData; - -float servoAngle; // servo angle global -double baseAlt; -unsigned long previousTime; -bool sd_init = false; -float targetServoAngle; -uint32_t startCoastTime; +#include "common/globals.h" +#include "simulation/Serial_Sim.h" + +// VerticalVelocityEstimator* verticalVelocityEstimator; +// LaunchPredictor *lp; +// ApogeeDetector *ad; +// ServoInterface ms24; + +// // these values when initialized are going to be pointing to null val - they're better definined in setup +// BurnoutStateMachine* sm = nullptr; +// ApogeePredictor* ap = nullptr; // 0.2 is the alpha for the EMA, 1.0 is the minimum climb velocity + +// DataSaverBigSD* dataSaver; +// DataPoint aclX, aclY, aclZ, alt, temp, pres, gyroX, gyroY, gyroZ; + +// Telemetry telemetry; +// TelemetryData telemData; + +// float servoAngle; // servo angle global +// double baseAlt; +// unsigned long previousTime; +// bool sd_init = false; +// float targetServoAngle; +// uint32_t startCoastTime; void communicateVerification(bool sd_init); From 74b59a6ae63a1132089aae4f9b886bfe0be445d4 Mon Sep 17 00:00:00 2001 From: Mikey Schoonmaker Date: Thu, 17 Apr 2025 16:30:06 -0400 Subject: [PATCH 2/5] rip telemetry processing --- include/common/globals.h | 3 +- include/core/TelemetryProcessing.h | 3 ++ src/TelemetryProcessing.cpp | 49 ++++++++++++++++++++++++++++++ 3 files changed, 54 insertions(+), 1 deletion(-) diff --git a/include/common/globals.h b/include/common/globals.h index 5ca5e27..01c7c6a 100644 --- a/include/common/globals.h +++ b/include/common/globals.h @@ -28,4 +28,5 @@ extern double baseAlt; extern unsigned long previousTime; extern bool sd_init; extern uint32_t startCoastTime; -extern ServoInterface ms24; \ No newline at end of file +extern ServoInterface ms24; +extern float currAlt; \ No newline at end of file diff --git a/include/core/TelemetryProcessing.h b/include/core/TelemetryProcessing.h index e69de29..455964c 100644 --- a/include/core/TelemetryProcessing.h +++ b/include/core/TelemetryProcessing.h @@ -0,0 +1,3 @@ +#pragma once + +void updateTelem(); diff --git a/src/TelemetryProcessing.cpp b/src/TelemetryProcessing.cpp index b16e0a9..99ad51e 100644 --- a/src/TelemetryProcessing.cpp +++ b/src/TelemetryProcessing.cpp @@ -1,3 +1,52 @@ #include "config.h" #include "common/globals.h" #include "simulation/Serial_Sim.h" + +#include "core/TelemetryProcessing.h" + +void updateTelem() +{ + #ifdef SIM + SerialSim::getInstance().update(); + delay(10); + #endif + + // init telem & telem sensor recording + telemData = telemetry.getTelemetry(); + + // if you ever change the orientation of the sensors, this WILL probably need to be adjusted + telemData.sensorData["magnetometer"].magnetic.x = telemData.sensorData["magnetometer"].magnetic.y * -1; + telemData.sensorData["magnetometer"].magnetic.y = telemData.sensorData["magnetometer"].magnetic.x; + + currAlt = telemData.sensorData["altitude"].altitude; // will be used later so store + + // update data points + aclX.data = telemData.sensorData["acceleration"].acceleration.x; + aclY.data = telemData.sensorData["acceleration"].acceleration.y; + aclZ.data = telemData.sensorData["acceleration"].acceleration.z; + alt.data = currAlt; + + gyroX.data = telemData.sensorData["gyro"].gyro.x; + gyroY.data = telemData.sensorData["gyro"].gyro.y; + gyroZ.data = telemData.sensorData["gyro"].gyro.z; + temp.data = telemData.sensorData["temperature"].temperature; + pres.data = telemData.sensorData["pressure"].pressure; + + // give everything the same timestamp for eventual byte5 integration + unsigned long currTime = millis(); + aclX.timestamp_ms = aclY.timestamp_ms = aclZ.timestamp_ms = alt.timestamp_ms = currTime; + gyroX.timestamp_ms = gyroY.timestamp_ms = gyroZ.timestamp_ms = currTime; + temp.timestamp_ms = currTime; + pres.timestamp_ms = currTime; + + // save the data points to their respective data names + dataSaver->saveDataPoint(aclX, ACCELEROMETER_X); + dataSaver->saveDataPoint(aclY, ACCELEROMETER_Y); + dataSaver->saveDataPoint(aclZ, ACCELEROMETER_Z); + dataSaver->saveDataPoint(alt, ALTITUDE); + dataSaver->saveDataPoint(temp, TEMPERATURE); + dataSaver->saveDataPoint(pres, PRESSURE); + dataSaver->saveDataPoint(gyroX, GYROSCOPE_X); + dataSaver->saveDataPoint(gyroY, GYROSCOPE_Y); + dataSaver->saveDataPoint(gyroZ, GYROSCOPE_Z); +} \ No newline at end of file From 23dae54d78392bb330796943b343a3a42b98f38a Mon Sep 17 00:00:00 2001 From: Mikey Schoonmaker Date: Thu, 17 Apr 2025 16:34:18 -0400 Subject: [PATCH 3/5] rip communicate verification, -> commsVerification --- include/core/CommunicationVerification.h | 7 ++++++- src/CommunicationVerification.cpp | 24 ++++++++++++++++++++++-- 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/include/core/CommunicationVerification.h b/include/core/CommunicationVerification.h index ef46d7d..d20c4e2 100644 --- a/include/core/CommunicationVerification.h +++ b/include/core/CommunicationVerification.h @@ -1,3 +1,8 @@ #pragma once -void communicateVerification(bool sd_init); +/*** + * initally deploys fins to show that we are in the communicate verification function + * retracts fins before entering loop + * if fins deploy after that point, we have an error + */ +void commsVerification(bool sd_init); diff --git a/src/CommunicationVerification.cpp b/src/CommunicationVerification.cpp index b16e0a9..1e0edbd 100644 --- a/src/CommunicationVerification.cpp +++ b/src/CommunicationVerification.cpp @@ -1,3 +1,23 @@ -#include "config.h" +#include "common/config.h" #include "common/globals.h" -#include "simulation/Serial_Sim.h" + + +/*** + * initally deploys fins to show that we are in the communicate verification function + * retracts fins before entering loop + * if fins deploy after that point, we have an error + */ +void commsVerification(bool sd_init) { + ms24.setAngle(MAX_DEPLOYMENT_ANGLE); + delay(COMMUNICATION_VERIFICATION_DELAY); + ms24.setAngle(MIN_DEPLOYMENT_ANGLE); + delay(COMMUNICATION_VERIFICATION_DELAY); + + SensorsActivated sensorsActivated = telemetry.getSensorsActivated(); + std::vector verifiables = {sensorsActivated.mag, sensorsActivated.bmp, sensorsActivated.imu, sd_init}; + for (bool verifiable : verifiables) { + ms24.setAngle(verifiable ? MIN_DEPLOYMENT_ANGLE : MAX_DEPLOYMENT_ANGLE); + delay(COMMUNICATION_VERIFICATION_DELAY); + } + delay(COMMUNICATION_VERIFICATION_DELAY); +} From 01a8462ddb526ca5bf779ab70259c5af20259efa Mon Sep 17 00:00:00 2001 From: Mikey Schoonmaker Date: Thu, 17 Apr 2025 16:49:48 -0400 Subject: [PATCH 4/5] rip setup, refactor main around new architecture --- include/common/globals.h | 1 - include/core/ApogeeControl.h | 4 - include/core/CommunicationVerification.h | 3 + include/core/Setup.h | 7 + include/core/TelemetryProcessing.h | 4 + src/ApogeeControl.cpp | 3 - src/CommunicationVerification.cpp | 4 +- src/Setup.cpp | 36 ++- src/TelemetryProcessing.cpp | 4 - src/main.cpp | 278 +++++++---------------- 10 files changed, 128 insertions(+), 216 deletions(-) delete mode 100644 include/core/ApogeeControl.h delete mode 100644 src/ApogeeControl.cpp diff --git a/include/common/globals.h b/include/common/globals.h index 01c7c6a..b19b94b 100644 --- a/include/common/globals.h +++ b/include/common/globals.h @@ -6,7 +6,6 @@ #include "data_handling/DataPoint.h" #include "data_handling/DataSaverBigSD.h" - VerticalVelocityEstimator* verticalVelocityEstimator; LaunchPredictor *lp; ApogeeDetector *ad; diff --git a/include/core/ApogeeControl.h b/include/core/ApogeeControl.h deleted file mode 100644 index 37d7237..0000000 --- a/include/core/ApogeeControl.h +++ /dev/null @@ -1,4 +0,0 @@ -#pragma once - -void processFlightState(); -void controlDeployment(); diff --git a/include/core/CommunicationVerification.h b/include/core/CommunicationVerification.h index d20c4e2..7688d98 100644 --- a/include/core/CommunicationVerification.h +++ b/include/core/CommunicationVerification.h @@ -1,5 +1,8 @@ #pragma once +#include "common/config.h" +#include "common/globals.h" + /*** * initally deploys fins to show that we are in the communicate verification function * retracts fins before entering loop diff --git a/include/core/Setup.h b/include/core/Setup.h index e69de29..3a59513 100644 --- a/include/core/Setup.h +++ b/include/core/Setup.h @@ -0,0 +1,7 @@ +#pragma once + +#include "common/config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" + +void setup(); diff --git a/include/core/TelemetryProcessing.h b/include/core/TelemetryProcessing.h index 455964c..afb3bc4 100644 --- a/include/core/TelemetryProcessing.h +++ b/include/core/TelemetryProcessing.h @@ -1,3 +1,7 @@ #pragma once +#include "common/config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" + void updateTelem(); diff --git a/src/ApogeeControl.cpp b/src/ApogeeControl.cpp deleted file mode 100644 index b16e0a9..0000000 --- a/src/ApogeeControl.cpp +++ /dev/null @@ -1,3 +0,0 @@ -#include "config.h" -#include "common/globals.h" -#include "simulation/Serial_Sim.h" diff --git a/src/CommunicationVerification.cpp b/src/CommunicationVerification.cpp index 1e0edbd..8298077 100644 --- a/src/CommunicationVerification.cpp +++ b/src/CommunicationVerification.cpp @@ -1,6 +1,4 @@ -#include "common/config.h" -#include "common/globals.h" - +#include "core/CommunicationVerification.h" /*** * initally deploys fins to show that we are in the communicate verification function diff --git a/src/Setup.cpp b/src/Setup.cpp index b16e0a9..67b382c 100644 --- a/src/Setup.cpp +++ b/src/Setup.cpp @@ -1,3 +1,33 @@ -#include "config.h" -#include "common/globals.h" -#include "simulation/Serial_Sim.h" +#include "core/Setup.h" + +void setup() { + Serial.begin(BAUD_RATE); + delay(SETUP_DELAY); + + ms24.setup(SERVO_PIN, SERVO_RANGE, SERVO_LOWER_PULSE, SERVO_UPPER_PULSE); + telemetry.setupSensors(); + + // be wary, as we're putting data onto the heap with these new statements. may need to be reconfigured if we have to save space in the future + dataSaver = new DataSaverBigSD(SD_CHIP_SELECT); + verticalVelocityEstimator = new VerticalVelocityEstimator(); + ad = new ApogeeDetector(1.0f); + lp = new LaunchPredictor(ACCEL_THRESHOLD_MS2, LAUNCH_WINDOW_SIZE_MS, LAUNCH_WINDOW_INTERVAL_MS); + sm = new BurnoutStateMachine(dataSaver, lp, ad, verticalVelocityEstimator); + ap = new ApogeePredictor(*verticalVelocityEstimator, EMA_ALPHA, MINIMUM_CLIMB_VELOCITY); + + + sd_init = dataSaver->begin(); // super super important for comms verification + + + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); + previousTime = millis(); + targetServoAngle = MIN_DEPLOYMENT_ANGLE; + startCoastTime = 0; + + #ifdef SIM + SerialSim::getInstance().begin(&Serial, sm); + #endif + + delay(SETUP_DELAY); // wait for setup to finish +} diff --git a/src/TelemetryProcessing.cpp b/src/TelemetryProcessing.cpp index 99ad51e..c05e00c 100644 --- a/src/TelemetryProcessing.cpp +++ b/src/TelemetryProcessing.cpp @@ -1,7 +1,3 @@ -#include "config.h" -#include "common/globals.h" -#include "simulation/Serial_Sim.h" - #include "core/TelemetryProcessing.h" void updateTelem() diff --git a/src/main.cpp b/src/main.cpp index 9d1735f..9397d84 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,223 +1,105 @@ -#include "config.h" +#include "common/config.h" #include "common/globals.h" #include "simulation/Serial_Sim.h" -// VerticalVelocityEstimator* verticalVelocityEstimator; -// LaunchPredictor *lp; -// ApogeeDetector *ad; -// ServoInterface ms24; +#include "core/Setup.h" +#include "core/CommunicationVerification.h" -// // these values when initialized are going to be pointing to null val - they're better definined in setup -// BurnoutStateMachine* sm = nullptr; -// ApogeePredictor* ap = nullptr; // 0.2 is the alpha for the EMA, 1.0 is the minimum climb velocity - -// DataSaverBigSD* dataSaver; -// DataPoint aclX, aclY, aclZ, alt, temp, pres, gyroX, gyroY, gyroZ; - -// Telemetry telemetry; -// TelemetryData telemData; - -// float servoAngle; // servo angle global -// double baseAlt; -// unsigned long previousTime; -// bool sd_init = false; -// float targetServoAngle; -// uint32_t startCoastTime; - -void communicateVerification(bool sd_init); - -void setup() -{ - Serial.begin(BAUD_RATE); - delay(SETUP_DELAY); - // put your setup code here, to run once: - ms24.setup(SERVO_PIN, SERVO_RANGE, SERVO_LOWER_PULSE, SERVO_UPPER_PULSE); - telemetry.setupSensors(); - - dataSaver = new DataSaverBigSD(SD_CHIP_SELECT); // Ensure SD is initialized correctly - - // init new pointers here (ld, ap, vve, etc.) - verticalVelocityEstimator = new VerticalVelocityEstimator(); - ad = new ApogeeDetector(1.0f); // 1.0f is the apogee threshold in meters - lp = new LaunchPredictor(ACCEL_THRESHOLD_MS2, LAUNCH_WINDOW_SIZE_MS, LAUNCH_WINDOW_INTERVAL_MS); // blanket values ripped from MARTHA +void main() +{ + setup(); + commsVerification(sd_init); // check sensors & sd card - // NOW initialize objects that use the pointers - sm = new BurnoutStateMachine(dataSaver, lp, ad, verticalVelocityEstimator); - ap = new ApogeePredictor(*verticalVelocityEstimator, EMA_ALPHA, MINIMUM_CLIMB_VELOCITY); + while(1) + { - // confirm initialization, setup sd data saver - sd_init = dataSaver->begin(); + #ifdef SIM + SerialSim::getInstance().update(); + delay(10); + #endif - // LED communication/verif - pinMode(LED_BUILTIN, OUTPUT); - digitalWrite(LED_BUILTIN, HIGH); - previousTime = millis(); + unsigned long loopStartTime = millis(); - targetServoAngle = MIN_DEPLOYMENT_ANGLE; // Default to retracted - startCoastTime = 0; - communicateVerification(sd_init); + // update state we're in (Do not update the ap or vve, because the state machine will do that) + // IMPORTANT: Do not update the vve until after launch, so it's vertical axis determination is correct + sm->update(aclX, aclY, aclZ, alt); - #ifdef SIM - SerialSim::getInstance().begin(&Serial, sm); - #endif -} + // get the current time + unsigned long nowTime = millis(); + float dt = nowTime - previousTime; // time since last loop + previousTime = nowTime; -void loop() -{ - #ifdef SIM - SerialSim::getInstance().update(); - delay(10); - #endif - - unsigned long loopStartTime = millis(); - - // init telem & telem sensor recording - telemData = telemetry.getTelemetry(); - - // if you ever change the orientation of the sensors, this WILL probably need to be adjusted - telemData.sensorData["magnetometer"].magnetic.x = telemData.sensorData["magnetometer"].magnetic.y * -1; - telemData.sensorData["magnetometer"].magnetic.y = telemData.sensorData["magnetometer"].magnetic.x; - - float currAlt = telemData.sensorData["altitude"].altitude; // will be used later so store - - // update data points - aclX.data = telemData.sensorData["acceleration"].acceleration.x; - aclY.data = telemData.sensorData["acceleration"].acceleration.y; - aclZ.data = telemData.sensorData["acceleration"].acceleration.z; - alt.data = currAlt; - - gyroX.data = telemData.sensorData["gyro"].gyro.x; - gyroX.timestamp_ms = telemData.timestamp; - gyroY.data = telemData.sensorData["gyro"].gyro.y; - gyroY.timestamp_ms = telemData.timestamp; - gyroZ.data = telemData.sensorData["gyro"].gyro.z; - gyroZ.timestamp_ms = telemData.timestamp; - temp.data = telemData.sensorData["temperature"].temperature; - temp.timestamp_ms = telemData.timestamp; - pres.data = telemData.sensorData["pressure"].pressure; - pres.timestamp_ms = telemData.timestamp; - - unsigned long currTime = millis(); - aclX.timestamp_ms = aclY.timestamp_ms = aclZ.timestamp_ms = alt.timestamp_ms = currTime; // record timestamp for data points - - // save the data points to their respective data names - dataSaver->saveDataPoint(aclX, ACCELEROMETER_X); - dataSaver->saveDataPoint(aclY, ACCELEROMETER_Y); - dataSaver->saveDataPoint(aclZ, ACCELEROMETER_Z); - dataSaver->saveDataPoint(alt, ALTITUDE); - dataSaver->saveDataPoint(temp, TEMPERATURE); - dataSaver->saveDataPoint(pres, PRESSURE); - dataSaver->saveDataPoint(gyroX, GYROSCOPE_X); - dataSaver->saveDataPoint(gyroY, GYROSCOPE_Y); - dataSaver->saveDataPoint(gyroZ, GYROSCOPE_Z); - - // update state we're in (Do not update the ap or vve, because the state machine will do that) - // IMPORTANT: Do not update the vve until after launch, so it's vertical axis determination is correct - sm->update(aclX, aclY, aclZ, alt); - - // get the current time - unsigned long nowTime = millis(); - float dt = nowTime - previousTime; // time since last loop - previousTime = nowTime; - - if (dt > 0){ - // Save the recriprocal of the time step (ms) to get HZ - float hz = 1000.0f / dt; - // Save this as a data point - dataSaver->saveDataPoint(DataPoint(nowTime, hz), AVERAGE_CYCLE_RATE); - } + if (dt > 0){ + // Save the recriprocal of the time step (ms) to get HZ + float hz = 1000.0f / dt; + // Save this as a data point + dataSaver->saveDataPoint(DataPoint(nowTime, hz), AVERAGE_CYCLE_RATE); + } - // update apogee predictor - ap->update(); // update the apogee predictor with the current data points - float predApogee = ap->getPredictedApogeeAltitude_m(); - dataSaver->saveDataPoint(DataPoint(millis(), predApogee), EST_APOGEE); // save the predicted apogee to the data saver - // Save time to apogee - dataSaver->saveDataPoint(DataPoint(millis(), ap->getTimeToApogee_s()), TIME_TO_APOGEE); // save the time to apogee to the data saver + // update apogee predictor + ap->update(); // update the apogee predictor with the current data points + float predApogee = ap->getPredictedApogeeAltitude_m(); + dataSaver->saveDataPoint(DataPoint(millis(), predApogee), EST_APOGEE); // save the predicted apogee to the data saver + // Save time to apogee + dataSaver->saveDataPoint(DataPoint(millis(), ap->getTimeToApogee_s()), TIME_TO_APOGEE); // save the time to apogee to the data saver - if (sm->getState() == STATE_COAST_ASCENT) // we actually want to deploy - { - if(startCoastTime == 0) - { - startCoastTime = millis(); - } - if(ap->getTimeToApogee_s() < FIN_RETRACTION_THRESHOLD_S || (millis() - startCoastTime) < FIN_EJECTION_DELAY_MS) // test fin full out to full in time - { - targetServoAngle = MIN_DEPLOYMENT_ANGLE; // re-declare in case SCA -> SD - } - - #ifdef TEST_FIN_DEPLOYMENT // for the 04/13/2025 flight to just test if the fins will deploy - else // if we're in SCA, deploy to the maximum possible angle (-10/+10) + if (sm->getState() == STATE_COAST_ASCENT) // we actually want to deploy { - targetServoAngle = MAX_DEPLOYMENT_ANGLE; - } - #endif + if(startCoastTime == 0) + { + startCoastTime = millis(); + } - // currently very rudimentary, logic, should be replacing with something a bit more refined - #ifndef TEST_FIN_DEPLOYMENT - else { - if(ap->getPredictedApogeeAltitude_m() > TARGET_APOGEE + OVERSHOOT_THRESHOLD) // if we're going to overshoot, deploy the fins. + threshold so we don't make a sinusoid nightmare + if(ap->getTimeToApogee_s() < FIN_RETRACTION_THRESHOLD_S || (millis() - startCoastTime) < FIN_EJECTION_DELAY_MS) // test fin full out to full in time { - targetServoAngle = MAX_DEPLOYMENT_ANGLE; - - /** - * - * I'm considering making deployment logic a function of the overshoot -> - * - * if overshooting, targetServoAngle = amt_overshooting_m * proportional gain - * targetServoAngle = constrain(targetAngle, MIN_DEPLOY, MAX_DEPLOY) - * ms24.setAngle(targetServoAngle) - * - * gives us a little more control over aggression of deployment as we launch this more & learn in the future, and has a bit - * more finesse behind it than the current "if overshooting, max deploy" - * - */ + targetServoAngle = MIN_DEPLOYMENT_ANGLE; // re-declare in case SCA -> SD } - else + + #ifdef TEST_FIN_DEPLOYMENT // for the 04/13/2025 flight to just test if the fins will deploy + else // if we're in SCA, deploy to the maximum possible angle (-10/+10) { - targetServoAngle = MIN_DEPLOYMENT_ANGLE; + targetServoAngle = MAX_DEPLOYMENT_ANGLE; } - } - #endif - } - else - { // If we're not in SCA, stay 0 so we don't break the fins - targetServoAngle = MIN_DEPLOYMENT_ANGLE; - } + #endif + + + // currently very rudimentary, logic, should be replacing with something a bit more refined + #ifndef TEST_FIN_DEPLOYMENT + else { + if(ap->getPredictedApogeeAltitude_m() > TARGET_APOGEE + OVERSHOOT_THRESHOLD) // if we're going to overshoot, deploy the fins. + threshold so we don't make a sinusoid nightmare + { + targetServoAngle = MAX_DEPLOYMENT_ANGLE; + + /** + * + * I'm considering making deployment logic a function of the overshoot -> + * + * if overshooting, targetServoAngle = amt_overshooting_m * proportional gain + * targetServoAngle = constrain(targetAngle, MIN_DEPLOY, MAX_DEPLOY) + * ms24.setAngle(targetServoAngle) + * + * gives us a little more control over aggression of deployment as we launch this more & learn in the future, and has a bit + * more finesse behind it than the current "if overshooting, max deploy" + * + */ + } + else + { + targetServoAngle = MIN_DEPLOYMENT_ANGLE; + } + } + #endif - // set angle and log at the end of each iteration - dataSaver->saveDataPoint(DataPoint(millis(), targetServoAngle), FIN_DEPLOYMENT_AMOUNT); // save the servo angle to the data saver - ms24.setAngle(targetServoAngle); -} -/*** - * initally deploys fins to show that we are in the communicate verification function - * retracts fins before entering loop - * if fins deploy after that point, we have an error - */ -void communicateVerification(bool sd_init) -{ - // moving fins to visually show we're in comms check - ms24.setAngle(MAX_DEPLOYMENT_ANGLE); // different from full deploy to visually confirm we're undergoing comms verification - delay(COMMUNICATION_VERIFICATION_DELAY); - ms24.setAngle(MIN_DEPLOYMENT_ANGLE); - delay(COMMUNICATION_VERIFICATION_DELAY); - - // init loop to check sensors & sd ptr - SensorsActivated sensorsActivated = telemetry.getSensorsActivated(); - std::vector verifiables = {sensorsActivated.mag, sensorsActivated.bmp, sensorsActivated.imu, sd_init}; - for (bool verifiable : verifiables) - { - if (verifiable) - { - ms24.setAngle(MIN_DEPLOYMENT_ANGLE); // in is good if everything is working - delay(COMMUNICATION_VERIFICATION_DELAY); } - else - { - ms24.setAngle(MAX_DEPLOYMENT_ANGLE); // out is bad if something goes wrong - delay(COMMUNICATION_VERIFICATION_DELAY); + else + { // If we're not in SCA, stay 0 so we don't break the fins + targetServoAngle = MIN_DEPLOYMENT_ANGLE; } + + // set angle and log at the end of each iteration + dataSaver->saveDataPoint(DataPoint(millis(), targetServoAngle), FIN_DEPLOYMENT_AMOUNT); // save the servo angle to the data saver + ms24.setAngle(targetServoAngle); } - delay(COMMUNICATION_VERIFICATION_DELAY); // delay before returning to main } From 7f1d110b83d01529fc3da1aa9209f0573e63bd2b Mon Sep 17 00:00:00 2001 From: Mikey Schoonmaker Date: Thu, 17 Apr 2025 16:52:18 -0400 Subject: [PATCH 5/5] put updateTelem in main --- src/main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main.cpp b/src/main.cpp index 9397d84..3a66b15 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,6 +4,7 @@ #include "core/Setup.h" #include "core/CommunicationVerification.h" +#include "core/TelemetryProcessing.h" void main() { @@ -20,6 +21,8 @@ void main() unsigned long loopStartTime = millis(); + updateTelem(); // update the telemetry data + // update state we're in (Do not update the ap or vve, because the state machine will do that) // IMPORTANT: Do not update the vve until after launch, so it's vertical axis determination is correct sm->update(aclX, aclY, aclZ, alt);