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..b19b94b --- /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; +extern float currAlt; \ No newline at end of file diff --git a/include/core/CommunicationVerification.h b/include/core/CommunicationVerification.h new file mode 100644 index 0000000..7688d98 --- /dev/null +++ b/include/core/CommunicationVerification.h @@ -0,0 +1,11 @@ +#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 + * if fins deploy after that point, we have an error + */ +void commsVerification(bool sd_init); diff --git a/include/core/Setup.h b/include/core/Setup.h new file mode 100644 index 0000000..3a59513 --- /dev/null +++ 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 new file mode 100644 index 0000000..afb3bc4 --- /dev/null +++ b/include/core/TelemetryProcessing.h @@ -0,0 +1,7 @@ +#pragma once + +#include "common/config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" + +void updateTelem(); diff --git a/src/CommunicationVerification.cpp b/src/CommunicationVerification.cpp new file mode 100644 index 0000000..8298077 --- /dev/null +++ b/src/CommunicationVerification.cpp @@ -0,0 +1,21 @@ +#include "core/CommunicationVerification.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); +} diff --git a/src/Setup.cpp b/src/Setup.cpp new file mode 100644 index 0000000..67b382c --- /dev/null +++ b/src/Setup.cpp @@ -0,0 +1,33 @@ +#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 new file mode 100644 index 0000000..c05e00c --- /dev/null +++ b/src/TelemetryProcessing.cpp @@ -0,0 +1,48 @@ +#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 diff --git a/src/main.cpp b/src/main.cpp index 113c25b..3a66b15 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,221 +1,108 @@ -#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; - -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 - - // NOW initialize objects that use the pointers - sm = new BurnoutStateMachine(dataSaver, lp, ad, verticalVelocityEstimator); - ap = new ApogeePredictor(*verticalVelocityEstimator, EMA_ALPHA, MINIMUM_CLIMB_VELOCITY); - - // confirm initialization, setup sd data saver - sd_init = dataSaver->begin(); - - // LED communication/verif - pinMode(LED_BUILTIN, OUTPUT); - digitalWrite(LED_BUILTIN, HIGH); - previousTime = millis(); - - targetServoAngle = MIN_DEPLOYMENT_ANGLE; // Default to retracted - startCoastTime = 0; - communicateVerification(sd_init); - - #ifdef SIM - SerialSim::getInstance().begin(&Serial, sm); - #endif -} +#include "common/config.h" +#include "common/globals.h" +#include "simulation/Serial_Sim.h" -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); - } +#include "core/Setup.h" +#include "core/CommunicationVerification.h" +#include "core/TelemetryProcessing.h" - // 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 +void main() +{ + setup(); + commsVerification(sd_init); // check sensors & sd card - if (sm->getState() == STATE_COAST_ASCENT) // we actually want to deploy + while(1) { - 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 SIM + SerialSim::getInstance().update(); + delay(10); + #endif + + 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); + + // 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); } - - #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) + + // 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 { - 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 }