integrated sflp with mqekf

Author: tjmcmanamen38

MIDAS/src/data_logging.cpp

@@ -78,7 +78,7 @@ void log_begin(LogSink& sink) {
 */
 void log_data(LogSink& sink, RocketData& data) {
     log_from_sensor_data(sink, data.imu);
-    log_from_sensor_data(sink, data.hw_filtered);
+    log_from_sensor_data(sink, data.sflp);
     log_from_sensor_data(sink, data.barometer);
     log_from_sensor_data(sink, data.voltage);
     log_from_sensor_data(sink, data.gps);

MIDAS/src/gnc/mqekf.cpp

@@ -75,17 +75,20 @@ QuaternionMEKF::QuaternionMEKF()
     state = AngularKalmanData();
 }
 
-void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state)
+void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state, Velocity &gyro_bias_sflp)
 {
     if (FSM_state >= FSMState::STATE_IDLE) //
     {
 
         // setQ(dt, sd);
         // priori(dt, orientation, FSM_state, acceleration);
         // update(barometer, acceleration, orientation, FSM_state, gps);
-
+        x(3) = gyro_bias_sflp.vx * pi/180;
+        x(4) = gyro_bias_sflp.vy* pi/180;
+        x(5) = gyro_bias_sflp.vz* pi/180;
         time_update(angular_velocity, dt);
         measurement_update(acceleration, magnetometer);
+        
         calculate_tilt();
         Eigen::Matrix<float, 4, 1> curr_quat = quaternion(); // w,x,y,z
 
@@ -355,11 +358,6 @@ AngularKalmanData QuaternionMEKF::getState()
 void QuaternionMEKF::calculate_tilt()
 {
 
-    const float alpha = 0.98; // Higher values dampen out current measurements --> reduce peaks
-
-    // The guess & check method!
-    // Quat --> euler --> rotation matrix --> reference&cur vector --> dot product for angle!
-
     Eigen::Quaternion<float>
         ref = Eigen::Quaternionf(0, 0, 0, 1);
 
@@ -379,12 +377,33 @@ void QuaternionMEKF::calculate_tilt()
     }
 
     // const float gain = 0.2;
-    // // Arthur's Comp Filter
+    // Arthur's Comp Filter
     // float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt;
     // prev_tilt = filtered_tilt;
     state.mq_tilt = tilt;
+}
+
+
+void QuaternionMEKF::calculate_tilt(IMU_SFLP imu_sflp)
+{
+
+    Eigen::Vector3f ref(1,0,0);
+
+    Eigen::Quaternion<float> sflp_quat = Eigen::Quaternionf(imu_sflp.quaternion.w,imu_sflp.quaternion.x,imu_sflp.quaternion.y,imu_sflp.quaternion.z);
+    sflp_quat.normalize();
+    
+    Eigen::Vector3f rotated_vector = sflp_quat * ref;
+
+    Eigen::Vector3f world_vertical(0, 0, 1);
+    float dot = rotated_vector.dot(world_vertical);
+    float cur_mag = rotated_vector.norm();
+    float tilt = 0;
+    if (cur_mag != 0)
+    {
+        tilt = acos(dot / (cur_mag));
+    }
 
-    // Serial.print("TILT: ");
+    state.sflp_tilt = tilt;
 }
 
 QuaternionMEKF mqekf;

MIDAS/src/gnc/mqekf.h

@@ -23,7 +23,7 @@ class QuaternionMEKF
         Eigen::Ref<Eigen::Matrix<float, 3, 1> const> const &vhat,
         Eigen::Ref<Eigen::Matrix<float, 3, 3> const> const &Rm);
 
-    void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state);
+    void tick(float dt, Magnetometer &magnetometer, Velocity &angular_velocity, Acceleration &acceleration, FSMState FSM_state, Velocity &gyro_bias_sflp);
     Eigen::Matrix<float, 4, 1> quaternion();
     Eigen::Matrix<float, 6, 6> covariance();
     Eigen::Matrix<float, 3, 1> gyroscope_bias();
@@ -32,7 +32,8 @@ class QuaternionMEKF
     AngularKalmanData getState();
     Eigen::Matrix<float, 3, 1> quatToEuler(const Eigen::Matrix<float, 4, 1> &q);
     // void tick(float dt, float sd, );
-    void calculate_tilt();
+    void calculate_tilt(); //using internal quaternion
+    void calculate_tilt(IMU_SFLP imu_sflp); //using SFLP quaternion
     void set_transition_matrix(const Eigen::Ref<const Eigen::Matrix<float, 3, 1>> &gyr, float Ts);
     Eigen::Matrix<float, 3, 3> skew_symmetric_matrix(const Eigen::Ref<const Eigen::Matrix<float, 3, 1>> &vec) const;
     Eigen::Matrix<float, 3, 1> magnetometer_measurement_func() const;

MIDAS/src/hardware/Pyro.cpp

@@ -26,7 +26,7 @@ bool error_is_failure(GpioError error_code) {
  */
 bool can_fire_igniter(AngularKalmanData angular_kalman_data) {
     // With new GNC orientation code we can add a simple check.
-    return angular_kalman_data.comp_tilt < MAXIMUM_TILT_ANGLE; // comp_tilt or mq_tilt?
+    return angular_kalman_data.sflp_tilt < MAXIMUM_TILT_ANGLE; 
 }
 
 

MIDAS/src/rocket_state.h

@@ -189,7 +189,7 @@ struct RocketData {
     SensorData<KalmanData> kalman;
     SensorData<AngularKalmanData> angular_kalman_data;
     BufferedSensorData<IMU, 16> imu;
-    SensorData<IMU_SFLP> hw_filtered;
+    SensorData<IMU_SFLP> sflp;
     BufferedSensorData<Barometer, 16> barometer;
     SensorData<PyroState> pyro;
     SensorData<FSMState> fsm_state;

MIDAS/src/sensor_data.h

@@ -122,7 +122,7 @@ struct Magnetometer {
     double mz;
 };
 
-struct Quaternion {
+struct Quaternion { //long term, remove or rename this struct, it will conflict with libraries where Quaternion is well defined. 
     float w, x, y, z;
 
     static float dot(const Quaternion& q1, const Quaternion& q2) {
@@ -273,7 +273,8 @@ struct KalmanData {
 struct AngularKalmanData {
     Quaternion quaternion;
     float gyrobias[3];
-    double comp_tilt = 0.0;
+    double sflp_tilt = 0.0; 
+    // double comp_tilt = 0.0;
     double mq_tilt = 0.0;
     bool has_data = false;
     OrientationReadingType reading_type = OrientationReadingType::FULL_READING;

MIDAS/src/systems.cpp

@@ -80,7 +80,7 @@ DECLARE_THREAD(imuthread, RocketSystems *arg)
     {
         xSemaphoreTake(spi_mutex, portMAX_DELAY);
         IMU imudata = arg->sensors.imu.read();
-        IMU_SFLP hw_filter = arg->sensors.imu.read_sflp();
+        IMU_SFLP sflp = arg->sensors.imu.read_sflp();
         xSemaphoreGive(spi_mutex);
 
         // Sensor calibration, if it is triggered.
@@ -101,7 +101,7 @@ DECLARE_THREAD(imuthread, RocketSystems *arg)
         imudata.highg_acceleration.az = imudata.highg_acceleration.az + bias.az;
 
         arg->rocket_data.imu.update(imudata);
-        arg->rocket_data.hw_filtered.update(hw_filter);
+        arg->rocket_data.sflp.update(sflp);
 
         THREAD_SLEEP(5);
     }
@@ -276,13 +276,13 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg)
             TickType_t last = xTaskGetTickCount();
             arg->rocket_data.command_flags.should_reset_kf = false;
         }
-
+        IMU_SFLP current_imu_sflp = arg->rocket_data.sflp.getRecent();
         IMU current_imu = arg->rocket_data.imu.getRecent();
         Acceleration current_high_g = current_imu.highg_acceleration;
         Acceleration current_low_g = current_imu.lowg_acceleration;
         Magnetometer current_mag = arg->rocket_data.magnetometer.getRecent();
         Velocity current_angular_velocity = current_imu.angular_velocity; // degrees
-
+        Velocity current_gyro_bias = current_imu_sflp.gyro_bias;
         AngularKalmanData current_angular_kalman = arg->rocket_data.angular_kalman_data.getRecent();
 
         Acceleration current_accelerations = {
@@ -294,8 +294,8 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg)
         float timestamp = pdTICKS_TO_MS(xTaskGetTickCount()) / 1000.0f;
 
         // Check with Divij
-        mqekf.tick(dt, current_mag, current_angular_velocity,current_accelerations, FSM_state);
-
+        mqekf.tick(dt, current_mag, current_angular_velocity, current_accelerations, FSM_state, current_gyro_bias);
+        mqekf.calculate_tilt(current_imu_sflp);    
         AngularKalmanData current_state = mqekf.getState();
 
         arg->rocket_data.angular_kalman_data.update(current_state);

MIDAS/src/telemetry.cpp

@@ -83,7 +83,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) {
 
     TelemetryPacket packet { };
     IMU imu = data.imu.getRecentUnsync();
-    IMU_SFLP hw_filtered_data = data.hw_filtered.getRecentUnsync();
+    IMU_SFLP sflp_data = data.sflp.getRecentUnsync();
     GPS gps = data.gps.getRecentUnsync();
     Voltage voltage = data.voltage.getRecentUnsync();
     Barometer barometer = data.barometer.getRecentUnsync();
@@ -111,7 +111,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) {
 
     // Tilt & FSM State --> comp_tilt vs mq_tilt
     static_assert(FSMState::FSM_STATE_COUNT < 16);
-    uint16_t tilt_norm = (angular_kalman.mq_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff
+    uint16_t tilt_norm = (angular_kalman.sflp_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff
     packet.tilt_fsm |= ((tilt_norm << 4) & 0xfff0);
     packet.tilt_fsm |= ((uint16_t)fsm & 0x000f);