[UNFINISHED] Meta Logging

MIDAS/src/data_logging.cpp

@@ -101,6 +101,8 @@ void log_data(LogSink& sink, RocketData& data) {
 
 }
 
+
+
 #ifndef SILSIM
 #define MAX_FILES 999
 

MIDAS/src/data_logging.h

@@ -11,6 +11,9 @@
 //#include "hardware/Emmc.h"
 #endif
 
+
+
+
 /**
  * @class LogSink
  * 
@@ -22,6 +25,7 @@ class LogSink {
 
     virtual ErrorCode init() = 0;
     virtual void write(const uint8_t* data, size_t size) = 0;
+    virtual void write_meta(const uint8_t* data, size_t size) = 0;
 };
 
 void log_begin(LogSink& sink);
@@ -37,6 +41,8 @@ class MultipleLogSink : public LogSink {
     };
 
     void write(const uint8_t* data, size_t size) override {};
+
+    void write_meta(const uint8_t* data, size_t size) override {};
 };
 
 template<typename Sink, typename... Sinks>
@@ -57,6 +63,11 @@ class MultipleLogSink<Sink, Sinks...> : public LogSink {
         sinks.write(data, size);
     };
 
+    void write_meta(const uint8_t* data, size_t size) override {
+        sink.write(data, size);
+        sinks.write(data, size);
+    };
+
 private:
     Sink sink;
     MultipleLogSink<Sinks...> sinks;

MIDAS/src/data_logging_meta.cpp

@@ -0,0 +1,32 @@
+#include "data_logging_meta.h"
+#include "log_format.h"
+#include "log_checksum.h"
+
+/*
+
+Queue<MetaLogging::MetaLogEntry> _q;
+
+bool MetaLogging::get_queued(MetaLogEntry* out) { return _q.receive(out); }
+
+void log_event(MetaDataCode event_type, uint32_t timestamp) {
+    MetaLogging::MetaLogEntry entry{event_type, 0, 0};
+    entry.size = sizeof(uint32_t);
+    memcpy(entry.data, &timestamp, entry.size);
+    _q.send(entry);
+}
+
+template <typename T>
+void log_data(MetaDataCode data_type, const T& data) {
+
+    // double check...
+    static_assert(sizeof(T) <= META_LOGGING_MAX_SIZE, "Datatype for log_data too large");
+
+    MetaLogEntry entry{data_type, 0, 0};
+    entry.size = sizeof(T);
+    memcpy(entry.data, &data, entry.size);
+    _q.send(entry);
+
+
+}
+
+*/

MIDAS/src/data_logging_meta.h

@@ -0,0 +1,42 @@
+#pragma once
+//#include "rocket_state.h"
+//#include "errors.h"
+
+/*
+
+#define META_LOGGING_MAX_SIZE 64
+
+enum MetaDataCode {
+    // Launch events
+    EVENT_TLAUNCH,
+    EVENT_TBURNOUT,
+    EVENT_TIGNITION,
+    EVENT_TAPOGEE,
+    EVENT_TMAIN,
+
+    // Non-events
+    DATA_LAUNCHSITE_BARO,
+    DATA_LAUNCHSITE_GPS,
+    DATA_LAUNCH_INITIAL_TILT,
+    DATA_TILT_AT_BURNOUT,
+    DATA_TILT_AT_IGNITION
+};
+
+struct MetaLogging {
+
+    public:
+    struct MetaLogEntry {
+        MetaDataCode log_type;
+        char data[META_LOGGING_MAX_SIZE];
+        size_t size;
+    };
+
+    bool get_queued(MetaLogEntry* out);
+
+    void log_event(MetaDataCode event_type, uint32_t timestamp);
+
+    template <typename T>
+    void log_data(MetaDataCode data_type, const T& data);
+};
+
+*/

MIDAS/src/esp_eeprom_checksum.h

@@ -0,0 +1,2 @@
+// autogenerated on build by applying crc32 on esp_eeprom_format.h
+#define EEPROM_CHECKSUM (0xcfa22d7d)

MIDAS/src/finite-state-machines/fsm.cpp

@@ -84,7 +84,11 @@ StateEstimate::StateEstimate(RocketData& state) {
  * 
  * @return New FSM State
 */
-FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) {
+FSMState FSM::tick_fsm(RocketData& sys) {
+    FSMState state = sys.fsm_state.getRecentUnsync();
+    StateEstimate state_estimate(sys);
+    CommandFlags& commands = sys.command_flags;
+
     //get current time
     double current_time = pdTICKS_TO_MS(xTaskGetTickCount());
 
@@ -338,7 +342,11 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
  * 
  * @return New FSM State
 */
-FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) {
+FSMState FSM::tick_fsm(RocketData& sys) {
+    FSMState state = sys.fsm_state.getRecentUnsync();
+    StateEstimate state_estimate(sys);
+    CommandFlags& commands = sys.command_flags;
+
     double current_time = pdTICKS_TO_MS(xTaskGetTickCount());
 
     switch (state) {

MIDAS/src/finite-state-machines/fsm.h

@@ -32,7 +32,11 @@ class FSM {
 public:
     FSM() = default;
 
-    FSMState tick_fsm(FSMState& curr_state, StateEstimate state_estimate, CommandFlags& commands);
+    //  FSMState tick_fsm(FSMState& curr_state, StateEstimate state_estimate, CommandFlags& commands);
+
+    // Constructor for the metadata
+    // Created so that FSMState, StateEstimate, and CommandFlags objects can supercede the mutex lock
+    FSMState tick_fsm(RocketData& sys);
 
 private:
     double launch_time;

MIDAS/src/hardware/SDLog.cpp

@@ -22,11 +22,19 @@ ErrorCode SDSink::init() {
     }
 
     char file_name[16] = "data";
-    char ext[] = ".launch";
-    sdFileNamer(file_name, ext, SD_MMC);
+    char meta_name[24];
+    char dfext[] = ".launch";
+    sdFileNamer(file_name, dfext, SD_MMC);
+
+    strcpy(meta_name, file_name);
+    char* extpos = strrchr(meta_name, '.');
+    if (extpos) {
+        strcpy(extpos, ".meta");
+    }
 
     file = SD_MMC.open(file_name, FILE_WRITE, true);
-    if (!file) {
+    meta = SD_MMC.open(meta_name, FILE_WRITE, true);
+    if (!file || !meta) {
         failed = true;
         return ErrorCode::SDCouldNotOpenFile;
     }
@@ -52,4 +60,16 @@ void SDSink::write(const uint8_t* data, size_t size) {
             unflushed_bytes = 0;
         }
     }
+
+    return;
 }
+
+void SDSink::write_meta(const uint8_t* data, size_t size) {
+    if (failed) { return; }
+
+    file.write(data, size);
+    file.write('\n');
+    file.flush(); // Meta writes are infrequent, so flushing is OK.
+
+    return;
+}

MIDAS/src/hardware/SDLog.h

@@ -19,7 +19,9 @@ class SDSink : public LogSink {
 
     ErrorCode init() override;
     void write(const uint8_t* data, size_t size) override;
+    void write_meta(const uint8_t* data, size_t size) override;
 private:
     File file;
+    File meta;
     size_t unflushed_bytes = 0;
 };

MIDAS/src/hardware/main.cpp

@@ -14,7 +14,7 @@
  */
 
 // #ifdef IS_SUSTAINER
-// MultipleLogSink<EMMCSink> sinks;
+//MultipleLogSink<EMMCSink> sinks;
 MultipleLogSink<SDSink> sinks;
 // #else
 // MultipleLogSink<> sinks;

MIDAS/src/rocket_state.h

@@ -153,6 +153,56 @@ class Latency {
     }
 };
 
+#define META_LOGGING_MAX_SIZE 64
+
+enum MetaDataCode {
+    // Launch events
+    EVENT_TLAUNCH,
+    EVENT_TBURNOUT,
+    EVENT_TIGNITION,
+    EVENT_TAPOGEE,
+    EVENT_TMAIN,
+
+    // Non-events
+    DATA_LAUNCHSITE_BARO,
+    DATA_LAUNCHSITE_GPS,
+    DATA_LAUNCH_INITIAL_TILT,
+    DATA_TILT_AT_BURNOUT,
+    DATA_TILT_AT_IGNITION
+};
+
+struct MetaLogging {
+    public:
+        struct MetaLogEntry {
+            MetaDataCode log_type;
+            size_t size;
+            char data[META_LOGGING_MAX_SIZE];
+        };
+
+        Queue<MetaLogEntry> _q;
+
+        bool get_queued(MetaLogEntry* out) { return _q.receive(out); }
+
+        void log_event(MetaDataCode event_type, uint32_t timestamp) {
+            MetaLogEntry entry{event_type, 0, 0};
+            entry.size = sizeof(uint32_t);
+            memcpy(entry.data, &timestamp, entry.size);
+            _q.send(entry);
+        }
+
+        template <typename T>
+        void log_data(MetaDataCode data_type, const T& data) {
+
+            // double check...
+            static_assert(sizeof(T) <= META_LOGGING_MAX_SIZE, "Datatype for log_data too large");
+
+            MetaLogEntry entry{data_type, 0, 0};
+            entry.size = sizeof(T);
+            memcpy(entry.data, &data, entry.size);
+            _q.send(entry);
+        }
+};
+
 /**
  * @struct CommandFlags
  * 
@@ -197,7 +247,7 @@ struct RocketData {
     SensorData<Orientation> orientation;
     SensorData<Voltage> voltage;
     SensorData<CameraData> cam_data;
-    
+
     CommandFlags command_flags;
     Latency log_latency;
 };

MIDAS/src/systems.cpp

@@ -21,15 +21,31 @@
  */
 DECLARE_THREAD(logger, RocketSystems* arg) {
     log_begin(arg->log_sink);
+    int meta_delay_ctr = 0; //  Will cause meta logs to write slower
     while (true) {
         log_data(arg->log_sink, arg->rocket_data);
 
         arg->rocket_data.log_latency.tick();
+        meta_delay_ctr++;
+
+        MetaLogging::MetaLogEntry entry;
+
+        if (meta_delay_ctr >= 100) {
+            if(arg->meta_logging.get_queued(&entry)) {
+                uint8_t buf[72];
+                size_t total_size = sizeof(MetaDataCode) + entry.size;
+                memcpy(buf, &entry.log_type, sizeof(MetaDataCode));
+                memcpy(buf + sizeof(MetaDataCode), &entry.data, entry.size);
+                arg->log_sink.write_meta(buf, total_size);
+            }
+        }
 
         THREAD_SLEEP(1);
     }
 }
 
+
+
 DECLARE_THREAD(barometer, RocketSystems* arg) {
     // Reject single rogue barometer readings that are very different from the immediately prior reading
     // Will only reject a certain number of readings in a row
@@ -146,6 +162,35 @@ DECLARE_THREAD(voltage, RocketSystems* arg) {
     }
 }
 
+void fsm_transitioned_to(FSMState& new_state, FSMState& old_state, RocketSystems* sys, double current_time) {
+    // Do something, NO delays allowed!
+    Orientation cur_orientation = sys->rocket_data.orientation.getRecentUnsync();
+    switch (new_state) {
+        case FSMState::STATE_FIRST_BOOST:
+            sys->meta_logging.log_event(MetaDataCode::EVENT_TLAUNCH, current_time);
+            sys->meta_logging.log_data(MetaDataCode::DATA_LAUNCHSITE_BARO, sys->rocket_data.barometer.getRecentUnsync());
+            sys->meta_logging.log_data(MetaDataCode::DATA_LAUNCHSITE_GPS, sys->rocket_data.gps.getRecentUnsync());
+            sys->meta_logging.log_data(MetaDataCode::DATA_LAUNCH_INITIAL_TILT, cur_orientation.tilt);
+            break;
+        case FSMState::STATE_BURNOUT:
+            sys->meta_logging.log_event(MetaDataCode::EVENT_TBURNOUT, current_time);
+            sys->meta_logging.log_data(MetaDataCode::DATA_TILT_AT_BURNOUT, cur_orientation.tilt);
+            break;
+        case FSMState::STATE_SECOND_BOOST:
+            sys->meta_logging.log_event(MetaDataCode::EVENT_TIGNITION, current_time);
+            sys->meta_logging.log_data(MetaDataCode::DATA_TILT_AT_IGNITION, cur_orientation.tilt);
+            break;
+        case FSMState::STATE_DROGUE_DEPLOY:
+            sys->meta_logging.log_event(MetaDataCode::EVENT_TAPOGEE, current_time);
+            break;
+        case FSMState::STATE_MAIN_DEPLOY:
+            sys->meta_logging.log_event(MetaDataCode::EVENT_TMAIN, current_time);
+            break;
+        default:
+            break;
+    }
+}
+
 // This thread has a bit of extra logic since it needs to play a tune exactly once the sustainer ignites
 DECLARE_THREAD(fsm, RocketSystems* arg) {
     FSM fsm{};
@@ -157,9 +202,10 @@ DECLARE_THREAD(fsm, RocketSystems* arg) {
         CommandFlags& telemetry_commands = arg->rocket_data.command_flags;
         double current_time = pdTICKS_TO_MS(xTaskGetTickCount());
 
-        FSMState next_state = fsm.tick_fsm(current_state, state_estimate, telemetry_commands);
+        FSMState next_state = fsm.tick_fsm(arg->rocket_data);
 
         arg->rocket_data.fsm_state.update(next_state);
+        if(current_state != next_state) {fsm_transitioned_to(next_state, current_state, arg, current_time);}
 
         if (current_state == FSMState::STATE_SAFE) {
             if((current_time - last_time_led_flash) > 250) {

MIDAS/src/systems.h

@@ -48,6 +48,7 @@ struct RocketSystems {
     Sensors sensors;
     RocketData rocket_data;
     LogSink& log_sink;
+    MetaLogging meta_logging;
     BuzzerController buzzer;
     LEDController led;
     Telemetry tlm;