log initial state, only log during flight not while flight is disarmed

Author: jacobh460

lib/controller/controller_and_estimator.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "matlab_funcs.h"
+
 struct Controller_Output {
   float thrust_N;
   float roll_rad_sec_squared;
@@ -36,6 +38,9 @@ struct Controller_Input {
 };
 
 namespace ControllerAndEstimator {
+extern Matrix9_9 Flight_P;
+extern Vector19 x_est;
+
 void init_controller_and_estimator_constants();
 Controller_Output get_controller_output(Controller_Input ci, float dT);
 }; // namespace ControllerAndEstimator

lib/trajectory_following/TrajectoryFollower.cpp

@@ -73,6 +73,8 @@ void follow_trajectory() {
         lastloop = timer;
         counter = 0;
         GPS::set_current_position_as_origin();
+
+        TrajectoryLogger::log_controller_state();
       }
 
       if (timer - lasttelemetry > TELEMETRY_INTERVAL_US) {
@@ -141,7 +143,10 @@ void follow_trajectory() {
       Prop::set_throttle_roll(thrust_perc, diffy_perc);
       GimbalServos::setGimbalAngle(-co.gimbal_yaw_deg, co.gimbal_pitch_deg);
 
-      TrajectoryLogger::log_trajectory_flash(timer, i, ci, co);
+      if (flight_armed) // we only want to log flight data, not pre-flight
+      {
+        TrajectoryLogger::log_trajectory_flash(timer, i, ci, co);
+      }
 
       if (send_telemetry) {
         // TODO - send telemetry here

lib/trajectory_following/TrajectoryLogger.cpp

@@ -30,20 +30,20 @@ CString<400> telemCSV;
 //   }
 // }
 
-struct __packed MeasurementFlags {
+struct __packed EntryFlags {
   bool new_imu_packet : 1;
   bool new_gps_packet : 1;
+  bool controller_state : 1;
 };
 
-static_assert(sizeof(MeasurementFlags) == 1, "sizeof(MeasurementFlags) error");
+static_assert(sizeof(EntryFlags) == 1, "sizeof(MeasurementFlags) error");
 
 struct __packed EntryBase {
-  MeasurementFlags flags;
   float time;
   uint8_t phase;
 };
 
-static_assert(sizeof(EntryBase) == 6, "sizeof(EntryBase) error");
+static_assert(sizeof(EntryBase) == 5, "sizeof(EntryBase) error");
 
 struct __packed SensorEntry {
 
@@ -88,14 +88,30 @@ struct __packed GpsEntry {
 
 static_assert(sizeof(GpsEntry) == 18 * 4, "sizeof(GpsEntry) error");
 
+void log_controller_state() {
+  EntryFlags flags{};
+
+  flags.controller_state = 1;
+
+  Logging::write((uint8_t *)&flags, sizeof(flags));
+
+  // log x_est and flight_P
+  Logging::write((uint8_t *)(ControllerAndEstimator::x_est.data()), sizeof(ControllerAndEstimator::x_est(0)) * (ControllerAndEstimator::x_est.size()));
+
+  Logging::write((uint8_t *)(ControllerAndEstimator::Flight_P.data()), sizeof(ControllerAndEstimator::Flight_P(0)) * (ControllerAndEstimator::Flight_P.size()));
+  
+  return;
+}
+
 void log_trajectory_flash(float time, int phase, Controller_Input ci, Controller_Output co) {
 
-  MeasurementFlags flags{};
+  EntryFlags flags{};
   flags.new_gps_packet = ci.new_gps_packet;
   flags.new_imu_packet = ci.new_imu_packet;
 
+  Logging::write((uint8_t *)&flags, sizeof(flags));
+
   EntryBase entryBase{};
-  entryBase.flags = flags;
   entryBase.time = time;
   entryBase.phase = phase;
 

lib/trajectory_following/TrajectoryLogger.h

@@ -10,4 +10,6 @@ namespace TrajectoryLogger {
 void log_trajectory_flash(float time, int phase, const Controller_Input ci, const Controller_Output co);
 
 void log_calib_flash();
+
+void log_controller_state();
 }; // namespace TrajectoryLogger