diff --git a/MIDAS/src/data_logging.cpp b/MIDAS/src/data_logging.cpp index 7f7f4157..1350a5b8 100644 --- a/MIDAS/src/data_logging.cpp +++ b/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); diff --git a/MIDAS/src/gnc/constants.h b/MIDAS/src/gnc/constants.h index 5a334bba..1ea7d18d 100644 --- a/MIDAS/src/gnc/constants.h +++ b/MIDAS/src/gnc/constants.h @@ -2,21 +2,9 @@ // constants const float pi = 3.14159268; -const float a = 343.0; // (m/s) speed of sound -const float rho = 1.225; // average air density -const float r = 0.0396; // (m) -const float height_full = 3.0259; // (m) height of rocket Full Stage -const float height_sustainer = 1.5021; // (m) height of rocket Sustainer -const float mass_sustainer_dry = 3.61; // (kg) Sustainer Dry Mass -const float mass_sustainer_wet = 4.68; // (kg) Sustainer Wet Mass -const float mass_booster_dry = 3.76; // (kg) Booster dry mass -const float mass_booster_wet = 5.91; // (kg) Booster wet mass -const float mass_full = mass_sustainer_wet+mass_booster_wet; // (kg) Total mass wet -const float mass_first_burnout = mass_booster_dry+mass_sustainer_wet;// (kg) Total mass after first stage burnout -const float mass_second_burnout = mass_booster_dry+mass_sustainer_dry;// (kg) Total mass after first stage burnout const float gravity_ms2 = 9.81; // (m/s^2) accel due to gravity const float accel_noise_density_x = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI. Assuming Acceleration noise density (high-g) in high-performance mode const float accel_noise_density_y = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI const float accel_noise_density_z = 1000; // ug/sqrt(hz) from the accelerometer on MIDAS MINI -const float gyro_RMS_noise = 0.1; // Need to get this data from the datasheet -const float mag_RMS_noise = 0; //mG \ No newline at end of file +const float gyro_RMS_noise =3.8; //mdps/√Hz +const float mag_noise = 0.4; //mG \ No newline at end of file diff --git a/MIDAS/src/gnc/ekf.cpp b/MIDAS/src/gnc/ekf.cpp index ef4e69c4..fd457dbd 100644 --- a/MIDAS/src/gnc/ekf.cpp +++ b/MIDAS/src/gnc/ekf.cpp @@ -57,8 +57,8 @@ void EKF::initialize(RocketSystems *args) P_k.block<2, 2>(4, 4) = Eigen::Matrix2f::Identity() * 1e-2f; // z block (pos,vel) // set Measurement Noise Matrix - R(0, 0) = 1.9; // barometer noise - R(1, 1) = 4.0f; // GPS altitude noise (lower trust) + R(0, 0) = 0.1; // barometer noise + R(1, 1) = 1.5f; // GPS altitude noise (lower trust) R(2, 2) = 3.0f; // GPS east noise R(3, 3) = 3.0f; // GPS north noise } @@ -77,11 +77,11 @@ void EKF::priori(float dt, AngularKalmanData &orientation, FSMState fsm, Acceler setB(dt); // Compute control input at current time step Eigen::Matrix sensor_accel_global_g = Eigen::Matrix::Zero(); - sensor_accel_global_g(0, 0) = acceleration.ax + 0.045f; - sensor_accel_global_g(1, 0) = acceleration.ay - 0.065f; - sensor_accel_global_g(2, 0) = acceleration.az - 0.06f; - euler_t angles_rad = orientation.getEuler(); - BodyToGlobal(angles_rad, sensor_accel_global_g); + sensor_accel_global_g(0, 0) = acceleration.ax ; + sensor_accel_global_g(1, 0) = acceleration.ay ; + sensor_accel_global_g(2, 0) = acceleration.az ; + // euler_t angles_rad = orientation.getEuler(); + // BodyToGlobal(angles_rad, sensor_accel_global_g); // Do not apply acceleration if on pad float g_ms2 = (fsm > FSMState::STATE_IDLE) ? gravity_ms2 : 0.0f; u_control(0, 0) = sensor_accel_global_g(0, 0) * g_ms2; @@ -192,20 +192,12 @@ void EKF::update(Barometer barometer, Acceleration acceleration, AngularKalmanDa landed_state_duration = s_dt; // Reset timer } - // Only reset velocities if we've been landed for at least 0.5 seconds - if (landed_state_duration >= 0.5f) - { - x_k(3, 0) = 0.0f; // velocity y (vy) = 0 when landed - x_k(5, 0) = 0.0f; // velocity z (vz) = 0 when landed - } - } - else - { - // Negate velocity z if it's negative (so negative becomes positive) - if (x_k(5, 0) < 0) // velocity z (vz) - { - x_k(5, 0) = -x_k(5, 0); // Negate negative velocity z to make it positive - } + // // Only reset velocities if we've been landed for at least 0.5 seconds + // if (landed_state_duration >= 0.5f) + // { + // x_k(3, 0) = 0.0f; // velocity y (vy) = 0 when landed + // x_k(5, 0) = 0.0f; // velocity z (vz) = 0 when landed + // } } // Update output state structure diff --git a/MIDAS/src/gnc/ekf.h b/MIDAS/src/gnc/ekf.h index e991f7bc..52b5fe95 100644 --- a/MIDAS/src/gnc/ekf.h +++ b/MIDAS/src/gnc/ekf.h @@ -47,7 +47,6 @@ class EKF : public KalmanFilter float Cn = 0; float Wind_alpha = 0.85f; float Cp = 0; - float curr_mass_kg = mass_full; //(kg) Sustainer + Booster, but value changes over time. std::vector starting_gps; // latitude, longitude, altitude std::vector starting_ecef; // x, y, z diff --git a/MIDAS/src/gnc/mqekf.cpp b/MIDAS/src/gnc/mqekf.cpp index adf3e627..d6a5d052 100644 --- a/MIDAS/src/gnc/mqekf.cpp +++ b/MIDAS/src/gnc/mqekf.cpp @@ -1,5 +1,6 @@ #include "mqekf.h" - +#include "constants.h" +#include /* mag y -> -x @@ -13,11 +14,11 @@ void QuaternionMEKF::initialize(RocketSystems *args) { - float Pq0 = 1e-6; + float Pq0 = 1e-4; float Pb0 = 1e-1; - sigma_a = {accel_noise_density_x * sqrt(100.0f) * 1e-6 * 9.81, accel_noise_density_y * sqrt(100.0f) * 1e-6 * 9.81, accel_noise_density_z * sqrt(100.0f) * 1e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet - sigma_g = {0.1 * pi / 180, 0.1 * pi / 180, 0.1 * pi / 180}; // 0.1 deg/s - sigma_m = {0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3), 0.4e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 + sigma_a = {accel_noise_density_x * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_y * sqrt(20.0f) * 1.0e-6 * 9.81, accel_noise_density_z * sqrt(20.0f) * 1.0e-6 * 9.81}; // ug/sqrt(Hz) *sqrt(hz). values are from datasheet + sigma_g = {gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f), gyro_RMS_noise * pow(1.0f, -3.0f) * pi / 180.0f * pow(20.0f, 0.5f)}; + sigma_m = {mag_noise * 1.0e-4 / sqrt(3), mag_noise * 1.0e-4 / sqrt(3), mag_noise * 1.0e-4 / sqrt(3)}; // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 // 0.4 mG -> T, it is 0.4 total so we divide by sqrt3 Q = initialize_Q(sigma_g); Eigen::Matrix sigmas; sigmas << sigma_a, sigma_m; @@ -74,7 +75,7 @@ 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) // { @@ -82,9 +83,13 @@ void QuaternionMEKF::tick(float dt, Magnetometer &magnetometer, Velocity &angula // 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 curr_quat = quaternion(); // w,x,y,z state.quaternion.w = curr_quat(0, 0); @@ -113,7 +118,7 @@ void QuaternionMEKF::time_update(Velocity const &gyro, float Ts) gyr(0, 0) = gyro.vx * (pi / 180.0f); gyr(1, 0) = gyro.vy * (pi / 180.0f); gyr(2, 0) = gyro.vz * (pi / 180.0f); - + set_transition_matrix(gyr - x.tail(3), Ts); Eigen::Vector4f q; // necessary to reorder to w,x,y,z @@ -285,7 +290,7 @@ Eigen::Matrix QuaternionMEKF::initialize_Q(Eigen::Matrix Q = Eigen::Matrix::Zero(); Q.block<3, 3>(0, 0) = sigma_g.array().square().matrix().asDiagonal(); - Q.block<3, 3>(3, 3) = 1e-12 * Eigen::Matrix3f::Identity(); + Q.block<3, 3>(3, 3) = 1e-2 * Eigen::Matrix3f::Identity(); return Q; } @@ -301,9 +306,9 @@ void QuaternionMEKF::initialize_from_acc_mag(Acceleration const &acc_struct, Mag Eigen::Matrix const acc_normalized = acc / anorm; Eigen::Matrix const mag_normalized = mag.normalized(); - Eigen::Matrix const Rz = -acc_normalized; - Eigen::Matrix const Ry = Rz.cross(mag_normalized).normalized(); - Eigen::Matrix const Rx = Ry.cross(Rz).normalized(); + Eigen::Matrix const Rx = acc_normalized; + Eigen::Matrix const Rz = Rx.cross(mag_normalized).normalized(); + Eigen::Matrix const Ry = Rz.cross(Rx).normalized(); // Construct the rotation matrix Eigen::Matrix const R = (Eigen::Matrix() << Rx, Ry, Rz).finished(); @@ -353,17 +358,12 @@ 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 - ref = Eigen::Quaternionf(1, 0, 0, 0); + ref = Eigen::Quaternionf(0, 0, 0, 1); Eigen::Quaternion rotated = qref * ref * qref.conjugate(); - Eigen::Matrix reference_vector = {0, 0, -1}; + Eigen::Matrix reference_vector = {0, 0, 1}; Eigen::Matrix rotated_vector = {rotated.x(), rotated.y(), rotated.z()}; float dot = rotated_vector.dot(reference_vector); @@ -376,14 +376,34 @@ void QuaternionMEKF::calculate_tilt() tilt = acos(dot / (cur_mag * ref_mag)); } - const float gain = 0.2; + // const float gain = 0.2; // Arthur's Comp Filter - float filtered_tilt = gain * tilt + (1 - gain) * prev_tilt; - prev_tilt = filtered_tilt; - state.mq_tilt = filtered_tilt; + // 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 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: "); - // Serial.println(filtered_tilt * (180/3.14f)); + state.sflp_tilt = tilt; } -QuaternionMEKF mqekf; \ No newline at end of file +QuaternionMEKF mqekf; diff --git a/MIDAS/src/gnc/mqekf.h b/MIDAS/src/gnc/mqekf.h index 6faabe7d..1b5f3bbf 100644 --- a/MIDAS/src/gnc/mqekf.h +++ b/MIDAS/src/gnc/mqekf.h @@ -23,7 +23,7 @@ class QuaternionMEKF Eigen::Ref const> const &vhat, Eigen::Ref 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 quaternion(); Eigen::Matrix covariance(); Eigen::Matrix gyroscope_bias(); @@ -32,7 +32,8 @@ class QuaternionMEKF AngularKalmanData getState(); Eigen::Matrix quatToEuler(const Eigen::Matrix &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> &gyr, float Ts); Eigen::Matrix skew_symmetric_matrix(const Eigen::Ref> &vec) const; Eigen::Matrix magnetometer_measurement_func() const; diff --git a/MIDAS/src/hardware/Magnetometer.cpp b/MIDAS/src/hardware/Magnetometer.cpp index 46966dd1..f9a29d6a 100644 --- a/MIDAS/src/hardware/Magnetometer.cpp +++ b/MIDAS/src/hardware/Magnetometer.cpp @@ -42,7 +42,7 @@ Magnetometer MagnetometerSensor::read() { // We multiply by 8, which is the full scale of the mag. // https://github.com/sparkfun/SparkFun_MMC5983MA_Magnetometer_Arduino_Library/blob/main/examples/Example4-SPI_Simple_measurement/Example4-SPI_Simple_measurement.ino - Magnetometer reading{X*8, Y*8, Z*8}; + Magnetometer reading{Y*8, -X*8, -Z*8}; return reading; } diff --git a/MIDAS/src/hardware/Pyro.cpp b/MIDAS/src/hardware/Pyro.cpp index 82df176d..3aadf7cb 100644 --- a/MIDAS/src/hardware/Pyro.cpp +++ b/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.mq_tilt < MAXIMUM_TILT_ANGLE; + return angular_kalman_data.sflp_tilt < MAXIMUM_TILT_ANGLE; } diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index be5a24d4..e6b65d22 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -188,7 +188,7 @@ struct RocketData { SensorData kalman; SensorData angular_kalman_data; BufferedSensorData imu; - SensorData hw_filtered; + SensorData sflp; BufferedSensorData barometer; SensorData pyro; SensorData fsm_state; diff --git a/MIDAS/src/sensor_data.h b/MIDAS/src/sensor_data.h index 340b453e..e4d74fbe 100644 --- a/MIDAS/src/sensor_data.h +++ b/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) { @@ -264,6 +264,7 @@ struct KalmanData { struct AngularKalmanData { Quaternion quaternion; float gyrobias[3]; + double sflp_tilt = 0.0; double mq_tilt = 0.0; bool has_data = false; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index bd806a6f..41f1544e 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -75,7 +75,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. @@ -96,7 +96,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); } @@ -271,13 +271,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 = { @@ -289,8 +289,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); diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry.cpp index bb55ba4d..b44a74ce 100644 --- a/MIDAS/src/telemetry.cpp +++ b/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 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);