Merge pull request #24 from CURocketEngineering/feb-8

Feb 8

README.md

@@ -60,6 +60,10 @@ Unit tests are managed by the [Native](https://github.com/CURocketEngineering/Na
 
 The following rocket flights have successfully utilized this avionics system (most recent first):
 
+- **[MARTHA 1.3 Feb 8th 2025 Test Vehicle Launch Attempt](https://github.com/CURocketEngineering/MARTHA-1.3/releases/tag/1.0.0)**
+ MARTHA was mounted to the Active Aero test vehicle, but it never launched. Data was collected for 14.18 hours before the 9V battery died. The data was successfully dumped, and the last hour of data before shutdown was recorded. A consistent 100Hz loop speed was achieved. During a rough assembly, the LaunchPredictor never had a false positive, even though it detected large acceleration values.
+
+
 - **[MARTHA 1.1 Nov 9th L1 Cert Launch](https://github.com/CURocketEngineering/MARTHA-1.1/releases/tag/v1.1.0):**  
   Launch detection and SD card data logging performed as expected. Note: No altitude data was recorded due to issues with sensor drivers or hardware damage.
 

hal/serial_mock.h

@@ -9,6 +9,7 @@
 #include <string>
 #include <algorithm>
 #include <vector>
+#include <stdint.h>
 
 class MockSerial {
 public:

include/data_handling/DataNames.h

@@ -23,6 +23,7 @@
 // Metadata
 #define TIMESTAMP 14
 #define STATE_CHANGE 15
+#define FLIGHT_ID 16
 
 
 

include/data_handling/DataSaverSPI.h

@@ -57,6 +57,8 @@ class DataSaverSPI : public IDataSaver {
      */
     virtual int saveDataPoint(DataPoint dp, uint8_t name) override;
 
+    int saveTimestamp(uint32_t timestamp_ms, uint8_t name);
+
     virtual bool begin() override;
 
     /**
@@ -104,7 +106,7 @@ class DataSaverSPI : public IDataSaver {
      * @brief Dumps all data from flash to Serial
      * 
      */
-    void dumpData(Stream &serial);
+    void dumpData(Stream &serial, bool ignoreEmptyPages);
 
     /**
      * @brief Resets all internal state values (buffer, lastDataPoint, nextWriteAddress, lastTimestamp_ms)

include/state_estimation/LaunchPredictor.h

@@ -76,6 +76,7 @@ class LaunchPredictor
     float getThreshold() {return accelerationThresholdSq_ms2;}
     // Gives the window interval in ms
     uint16_t getWindowInterval() {return windowInterval_ms;}
+    uint16_t getAcceptableTimeDifference() {return acceptableTimeDifference_ms;}
 
 
 private:
@@ -87,7 +88,7 @@ class LaunchPredictor
     uint16_t min_window_size_ms; // If the calculated time range is less than this, don't try to detect launch
     uint16_t max_window_size_ms; // If the calculated time range is greater than this, don't try to detect launch
 
-    float twentyPercentWindowInterval_ms;
+    float acceptableTimeDifference_ms;
     // The window holding the acceleration magnitude squared b/c sqrt is expensive
     CircularArray<DataPoint> AclMagSqWindow_ms2;
     bool launched;

src/data_handling/DataSaverSPI.cpp

@@ -20,13 +20,10 @@ DataSaverSPI::DataSaverSPI(uint16_t timestampInterval_ms, Adafruit_SPIFlash *fla
 int DataSaverSPI::saveDataPoint(DataPoint dp, uint8_t name) {
     if (rebootedInPostLaunchMode || isChipFullDueToPostLaunchProtection) return 1; // Do not save if we rebooted in post-launch mode
 
-    // Write timestamp if enough time has passed since the last one
+    // Write a timestamp automatically if enough time has passed since the last one
     uint32_t timestamp = dp.timestamp_ms;
     if (timestamp - lastTimestamp_ms > timestampInterval_ms) {
-        TimestampRecord_t tr = {TIMESTAMP, timestamp};
-        if (!addRecordToBuffer(&tr) == 0) return -1;
-
-        lastTimestamp_ms = timestamp;  // Everything after this timestamp until the next timestamp will use this timestamp when reconstructed
+        if (saveTimestamp(timestamp, name) < 0) return -1;
     }
 
     Record_t record = {name, dp.data};
@@ -36,6 +33,16 @@ int DataSaverSPI::saveDataPoint(DataPoint dp, uint8_t name) {
     return 0;
 }
 
+int DataSaverSPI::saveTimestamp(uint32_t timestamp_ms, uint8_t name){
+    if (rebootedInPostLaunchMode || isChipFullDueToPostLaunchProtection) return 1; // Do not save if we rebooted in post-launch mode
+
+    TimestampRecord_t tr = {TIMESTAMP, timestamp_ms};
+    if (!addRecordToBuffer(&tr) == 0) return -1;
+
+    lastTimestamp_ms = timestamp_ms; 
+    return 0;
+}
+
 int DataSaverSPI::addDataToBuffer(const uint8_t* data, size_t length) {
     if (bufferIndex + length > BUFFER_SIZE) {
         // Flush the buffer
@@ -64,13 +71,13 @@ int DataSaverSPI::flushBuffer() {
         return -1; // Indicate no write due to post-launch protection
     }
 
-    // If we just entered a new sector, erase it
     if (nextWriteAddress % SFLASH_SECTOR_SIZE == 0) {
         if (!flash->eraseSector(nextWriteAddress / SFLASH_SECTOR_SIZE)) {
             return -1;
         }
     }
 
+    // Write 1 page of data
     if (!flash->writeBuffer(nextWriteAddress, buffer, BUFFER_SIZE)) {
         return -1;
     }
@@ -111,31 +118,90 @@ void DataSaverSPI::clearPostLaunchMode() {
     clearInternalState();
 }
 
-void DataSaverSPI::dumpData(Stream &serial) {
+void DataSaverSPI::dumpData(Stream &serial, bool ignoreEmptyPages) {
     uint32_t readAddress = DATA_START_ADDRESS;
     // For each page write 51 sets of 5 bytes to serial with a newline
     uint8_t buffer[SFLASH_PAGE_SIZE];
     size_t recordSize = sizeof(Record_t);
     size_t numRecordsPerPage = SFLASH_PAGE_SIZE / recordSize;
 
+    // If not in post-launch mode, erase the next sector after nextWriteAddress
+    // This ensures that we don't accidentally dump old data from previous flights
+    // If ignoreEmptyPages is true, then we don't need to erase the next sector
+    if (!postLaunchMode && !ignoreEmptyPages) {
+        flash->eraseSector(nextWriteAddress / SFLASH_SECTOR_SIZE + 1);
+    }
+
     // To ensure it's lined-up let's set a '\n' , '\r' and a 's' at the start
     serial.write('a');
     serial.write('b');
     serial.write('c');
+    serial.write('d');
+    serial.write('e');
+    serial.write('f');
+
+    bool done = false;
+    bool timedOut = false;
+    bool stoppedFromEmptyPage = false;
+    bool badRead = false;
 
-    while (readAddress < flash->size()) { // just flash 1 page
+    while (readAddress < flash->size()) { 
         if (!readFromFlash(readAddress, buffer, SFLASH_PAGE_SIZE)) {
+            badRead = true;
             return;
         }
+
+        // If the first name of this page is 255 then break
+        if (buffer[0] == 255) {
+            if (ignoreEmptyPages) {
+                continue;
+            }
+            done = true;
+            stoppedFromEmptyPage = true;
+            break;
+        }
+
+        // At the start of each page, write some alignment characters
+        char startLine[3] = {'l', 's', 'h'};
+        serial.write(reinterpret_cast<uint8_t*>(startLine), 3);
+
+
+
         for (size_t i = 0; i < numRecordsPerPage; i++) {
+
             serial.write(buffer + i * recordSize, recordSize);
             // serial.write('\n');
         }
-        readAddress += SFLASH_PAGE_SIZE;
-    }
 
-    // Write "done"
-    serial.write("done\n");
+        // Wait for a 'n' character to be received before continuing (10 second timeout)
+        uint32_t timeout = millis() + 10000;
+        while (serial.read() != 'n') {
+            if (millis() > timeout) {
+                timedOut = true;
+                return;
+            }
+        }
+
+    }
+    for (int i = 0; i < 255; i++){
+        char doneLine[3] = {'E', 'O', 'F'};
+        serial.write(reinterpret_cast<uint8_t*>(doneLine), 3);
+        if (done){
+            serial.write('D');
+        }
+        if (timedOut){
+            serial.write('T');
+        }
+        if (stoppedFromEmptyPage){
+            serial.write('P');
+        }
+        if (badRead){
+            serial.write('B');
+        }
+        if (readAddress >= flash->size()){
+            serial.write('F');
+        }
+    }
 }
 
 void DataSaverSPI::clearInternalState() {

src/state_estimation/LaunchPredictor.cpp

@@ -19,13 +19,13 @@ LaunchPredictor::LaunchPredictor(float accelerationThreshold_ms2,
 
     launched = false;
     launchedTime_ms = 0;
-    twentyPercentWindowInterval_ms = windowInterval_ms * 0.2;  // Defines the radius of acceptable time differences between entries in the window
+    acceptableTimeDifference_ms = windowInterval_ms * 0.5;  // Defines the radius of acceptable time differences between entries in the window
     median_acceleration_squared = 0;
 
     // The minimum window size should occur when all data comes in at the smallest possible intervals
     // The maximum window size should occur when all data comes in at the largest possible intervals
-    this->min_window_size_ms = (windowInterval_ms - twentyPercentWindowInterval_ms) * AclMagSqWindow_ms2.getMaxSize();
-    this->max_window_size_ms = (windowInterval_ms + twentyPercentWindowInterval_ms) * AclMagSqWindow_ms2.getMaxSize();
+    this->min_window_size_ms = (windowInterval_ms - acceptableTimeDifference_ms) * (AclMagSqWindow_ms2.getMaxSize() - 1);
+    this->max_window_size_ms = (windowInterval_ms + acceptableTimeDifference_ms) * (AclMagSqWindow_ms2.getMaxSize() - 1);
 }
 
 int LaunchPredictor::update(DataPoint xac, DataPoint yac, DataPoint zac)
@@ -68,7 +68,7 @@ int LaunchPredictor::update(DataPoint xac, DataPoint yac, DataPoint zac)
     if (!AclMagSqWindow_ms2.isFull())
     {
         #ifdef DEBUG
-        Serial.println("LaunchPredictor: Populating initial window");
+        // Serial.println("LaunchPredictor: Populating initial window");
         #endif
         AclMagSqWindow_ms2.push(DataPoint(time_ms, aclMagSq));
 
@@ -79,7 +79,7 @@ int LaunchPredictor::update(DataPoint xac, DataPoint yac, DataPoint zac)
     uint32_t time_diff = time_ms - AclMagSqWindow_ms2.getFromHead(0).timestamp_ms;
 
     // Check that the data didn't come in too fast
-    if (time_diff < windowInterval_ms - twentyPercentWindowInterval_ms){
+    if (time_diff < windowInterval_ms - acceptableTimeDifference_ms){
         #ifdef DEBUG
         Serial.println("LaunchPredictor: DATA TOO EARLY");
         Serial.printf("Time diff: %d\n", time_diff);
@@ -90,7 +90,7 @@ int LaunchPredictor::update(DataPoint xac, DataPoint yac, DataPoint zac)
         return LP_DATA_TOO_FAST;
     }
 
-    if (time_diff > windowInterval_ms + twentyPercentWindowInterval_ms)
+    if (time_diff > windowInterval_ms + acceptableTimeDifference_ms)
     {
         #ifdef DEBUG
         Serial.println("LaunchPredictor: DATA TOO LATE");
@@ -120,7 +120,8 @@ int LaunchPredictor::update(DataPoint xac, DataPoint yac, DataPoint zac)
     {
         #ifdef DEBUG
         Serial.println("LaunchPredictor: Time range too small, waiting...");
-        Serial.printf("Time range: %d\n", time_range);
+        Serial.printf("Time range:     %d\n", time_range);
+        Serial.printf("Min Time Range: %d\n", min_window_size_ms);
         Serial.printf("Incoming time: %d\n", time_ms);
         Serial.printf("Head time: %d\n", AclMagSqWindow_ms2.getFromHead(0).timestamp_ms);
         Serial.printf("Tail time: %d\n", AclMagSqWindow_ms2.getFromHead(AclMagSqWindow_ms2.getMaxSize() - 1).timestamp_ms);
@@ -134,7 +135,8 @@ int LaunchPredictor::update(DataPoint xac, DataPoint yac, DataPoint zac)
     {
         #ifdef DEBUG
         Serial.println("LaunchPredictor: Time range too large, waiting...");
-        Serial.printf("Time range: %d\n", time_range);
+        Serial.printf("Time range:     %d\n", time_range);
+        Serial.printf("Max Time Range: %d\n", max_window_size_ms);
         Serial.printf("Incoming time: %d\n", time_ms);
         Serial.printf("Head time: %d\n", AclMagSqWindow_ms2.getFromHead(0).timestamp_ms);
         Serial.printf("Tail time: %d\n", AclMagSqWindow_ms2.getFromHead(AclMagSqWindow_ms2.getMaxSize() - 1).timestamp_ms);

src/state_estimation/StateMachine.cpp

@@ -11,10 +11,12 @@ StateMachine::StateMachine(IDataSaver* dataSaver, LaunchPredictor* launchPredict
 
 int StateMachine::update(DataPoint aclX, DataPoint aclY, DataPoint aclZ, DataPoint alt) {
     // Update the state
+    int lpStatus = LP_DEFAULT_FAIL; 
     switch (state) {
         case STATE_ARMED:
             // Serial.println("lp update");
-            launchPredictor->update(aclX, aclY, aclZ);
+            lpStatus = launchPredictor->update(aclX, aclY, aclZ);
+            // Serial.println(lpStatus);
             if (launchPredictor->isLaunched()) {
                 // Change state to ascent
                 state = STATE_ASCENT;