Process-profiling

MIDAS/src/data_logging.cpp

@@ -25,6 +25,7 @@ ASSOCIATE(Orientation, ID_ORIENTATION)
 ASSOCIATE(FSMState, ID_FSM)
 ASSOCIATE(KalmanData, ID_KALMAN)
 ASSOCIATE(PyroState, ID_PYRO)
+ASSOCIATE(ProcessTime, ID_PROCESSTIME);
 
 /**
  * @brief writes a reading, with its ID, timestamp, and data to a specific sink
@@ -88,6 +89,7 @@ void log_data(LogSink& sink, RocketData& data) {
     log_from_sensor_data(sink, data.fsm_state);
     log_from_sensor_data(sink, data.kalman);
     log_from_sensor_data(sink, data.pyro);
+    log_from_sensor_data(sink, data.processTime);
 }
 
 #ifndef SILSIM

MIDAS/src/log_format.h

@@ -20,6 +20,7 @@ enum ReadingDiscriminant {
     ID_FSM = 10,
     ID_KALMAN = 11,
     ID_PYRO = 12,
+    ID_PROCESSTIME = 13,
 };
 
 
@@ -51,5 +52,6 @@ struct LoggedReading {
         KalmanData kalman;
         FSMState fsm;
         PyroState pyro;
+        ProcessTime processtime;
     } data;
 };

MIDAS/src/rocket_state.h

@@ -176,6 +176,7 @@ struct RocketData {
     SensorData<Magnetometer> magnetometer;
     SensorData<Orientation> orientation;
     SensorData<Voltage> voltage;
+    SensorData<ProcessTime> processTime;
 
     Latency log_latency;
 };

MIDAS/src/sensor_data.h

@@ -229,3 +229,28 @@ struct PyroState {
     bool is_global_armed = false;
     PyroChannel channels[4];
 };
+
+enum class ProcessName {
+    TELEMETRY = 1,
+    ORIENTATION = 2,
+    KALMAN = 3,
+    BUZZER = 4,
+    FSM = 5,
+    I2C = 6,
+    MAGNETOMETER = 7,
+    ACCELEROMETERS = 8,
+    BAROMETER = 9,
+    LOGGER = 10
+};
+
+/**
+ * @struct ProcessTime
+ * 
+ * @brief The process time of the processes in the thread
+*/
+struct ProcessTime
+{
+    ProcessName name;
+    float dt;
+};
+

MIDAS/src/systems.cpp

@@ -19,24 +19,43 @@
 DECLARE_THREAD(logger, RocketSystems* arg) {
     log_begin(arg->log_sink);
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         log_data(arg->log_sink, arg->rocket_data);
 
         arg->rocket_data.log_latency.tick();
 
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::LOGGER;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(1);
     }
 }
 
 DECLARE_THREAD(barometer, RocketSystems* arg) {
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         Barometer reading = arg->sensors.barometer.read();
         arg->rocket_data.barometer.update(reading);
+
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::BAROMETER;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(6);
     }
 }
 
 DECLARE_THREAD(accelerometers, RocketSystems* arg) {
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
 #ifdef IS_SUSTAINER
         LowGData lowg = arg->sensors.low_g.read();
         arg->rocket_data.low_g.update(lowg);
@@ -45,25 +64,49 @@ DECLARE_THREAD(accelerometers, RocketSystems* arg) {
         arg->rocket_data.low_g_lsm.update(lowglsm);
         HighGData highg = arg->sensors.high_g.read();
         arg->rocket_data.high_g.update(highg);
+
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::ACCELEROMETERS;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(2);
     }
 }
 
 DECLARE_THREAD(orientation, RocketSystems* arg) {
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         Orientation reading = arg->sensors.orientation.read();
         if (reading.has_data) {
             arg->rocket_data.orientation.update(reading);
         }
 
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::ORIENTATION;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(100);
     }
 }
 
 DECLARE_THREAD(magnetometer, RocketSystems* arg) {
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         Magnetometer reading = arg->sensors.magnetometer.read();
         arg->rocket_data.magnetometer.update(reading);
+
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::MAGNETOMETER;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(50);  //data rate is 155hz so 7 is closest
     }
 }
@@ -73,6 +116,8 @@ DECLARE_THREAD(i2c, RocketSystems* arg) {
     int i = 0;
 
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         if (i % 10 == 0) {
             GPS reading = arg->sensors.gps.read();
             arg->rocket_data.gps.update(reading);
@@ -91,6 +136,12 @@ DECLARE_THREAD(i2c, RocketSystems* arg) {
         arg->led.update();
         i += 1;
 
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::I2C;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(10);
     }
 }
@@ -100,6 +151,8 @@ DECLARE_THREAD(fsm, RocketSystems* arg) {
     FSM fsm{};
     bool already_played_freebird = false;
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync();
         StateEstimate state_estimate(arg->rocket_data);
 
@@ -112,14 +165,28 @@ DECLARE_THREAD(fsm, RocketSystems* arg) {
             already_played_freebird = true;
         }
 
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::FSM;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(50);
     }
 }
 
 DECLARE_THREAD(buzzer, RocketSystems* arg) {
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         arg->buzzer.tick();
 
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt = dt;
+        new_processTime.name = ProcessName::BUZZER;
+        arg->rocket_data.processTime.update(new_processTime);
+
         THREAD_SLEEP(10);
     }
 }
@@ -129,6 +196,7 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
     TickType_t last = xTaskGetTickCount();
 
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
         // add the tick update function
         Barometer current_barom_buf = arg->rocket_data.barometer.getRecentUnsync();
         LowGData current_accelerometer = arg->rocket_data.low_g.getRecentUnsync();
@@ -143,17 +211,32 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
         KalmanData current_state = yessir.getState();
 
         arg->rocket_data.kalman.update(current_state);
-
         last = xTaskGetTickCount();
 
+        dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt;
+        new_processTime.name = ProcessName::KALMAN;
+        arg->rocket_data.processTime.update(new_processTime);
+
+
         THREAD_SLEEP(50);
     }
 }
 
 DECLARE_THREAD(telemetry, RocketSystems* arg) {
     while (true) {
+        TickType_t startTime = xTaskGetTickCount();
+
         arg->tlm.transmit(arg->rocket_data, arg->led);
 
+        float dt = pdTICKS_TO_MS(xTaskGetTickCount() - startTime) / 1000.0f;
+        ProcessTime new_processTime;
+        new_processTime.dt;
+        new_processTime.name = ProcessName::TELEMETRY;
+        arg->rocket_data.processTime.update(new_processTime);
+
+
         THREAD_SLEEP(1);
     }
 }