Merge pull request #188 from ISSUIUC/feature/magnetometer-calib

Magnetometer calibration

Author: MuhammadAli8209

MIDAS/extra_script.py

@@ -25,7 +25,7 @@
 
 file_eeprom = f"""\
 // autogenerated on build by applying crc32 on esp_eeprom_format.h
-#define EEPROM_CHECKSUM (0x{checksum:08x})
+#define EEPROM_CHECKSUM (0x{checksum_eeprom:08x})
 """
 
 with Path("src/log_checksum.h").open("w") as checksum_file:

MIDAS/src/buzzer.h

@@ -45,6 +45,7 @@ struct BuzzerController {
 #define LAND_TONE_LENGTH 11
 
 #define C_XL_LENGTH 3
+#define C_MG_LENGTH 5
 
 extern Sound free_bird[FREE_BIRD_LENGTH];
 extern Sound warn_tone[WARN_TONE_LENGTH];
@@ -53,4 +54,9 @@ extern Sound land_tone[LAND_TONE_LENGTH];
 extern Sound xl_calib_rdy[C_XL_LENGTH];
 extern Sound xl_calib_next_axis[C_XL_LENGTH];
 extern Sound xl_calib_done[C_XL_LENGTH];
-extern Sound xl_calib_abort[C_XL_LENGTH];
+extern Sound xl_calib_abort[C_XL_LENGTH];
+
+extern Sound mg_calib_rdy[C_MG_LENGTH];
+extern Sound mg_calib_done[C_MG_LENGTH];
+extern Sound mg_calib_inp[C_MG_LENGTH];
+extern Sound mg_calib_bad[C_MG_LENGTH];

MIDAS/src/esp_eeprom.cpp

@@ -1,6 +1,7 @@
 #include <esp_eeprom.h>
 
 constexpr size_t EEPROM_SIZE = sizeof(MIDASEEPROM);
+constexpr size_t EEPROM_MAX_SIZE = 512; // https://randomnerdtutorials.com/esp32-flash-memory/
 
 bool EEPROMController::read() {
     MIDASEEPROM _read;
@@ -38,7 +39,7 @@ bool EEPROMController::commit() {
 }
 
 ErrorCode EEPROMController::init() {
-
+    static_assert(EEPROM_SIZE <= EEPROM_MAX_SIZE);
     EEPROM.begin((size_t)EEPROM_SIZE);
 
     if (!read()) {

MIDAS/src/esp_eeprom_format.h

@@ -5,5 +5,7 @@
 struct MIDASEEPROM {
     uint32_t checksum;
 
-    Acceleration lsm6dsv320x_hg_xl_bias;
+    Acceleration lsm6dsv320x_hg_xl_bias = {0.0f, 0.0f, 0.0f};
+    Magnetometer mmc5983ma_softiron_bias = {1.0f, 1.0f, 1.0f};
+    Magnetometer mmc5983ma_hardiron_bias = {0.0f, 0.0f, 0.0f};
 };

MIDAS/src/hardware/Magnetometer.cpp

@@ -5,6 +5,18 @@
 
 SFE_MMC5983MA MMC5983;           // global static instance of the sensor
 
+#define MGC_TONE_PITCH_H Sound{3000, 50}
+#define MGC_TONE_PITCH_L Sound{2200, 50}
+#define MGC_TONE_PITCH_LONG Sound{3000, 250}
+#define MGC_TONE_PITCH_LONG_L Sound{2000, 250}
+#define MGC_TONE_WAIT Sound{0, 50}
+#define MGC_TONE_NOOP Sound{0, 1}
+
+Sound mg_calib_rdy[C_MG_LENGTH] = {MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H};
+Sound mg_calib_done[C_MG_LENGTH] = {MGC_TONE_PITCH_LONG, MGC_TONE_WAIT, MGC_TONE_PITCH_H, MGC_TONE_WAIT, MGC_TONE_PITCH_H};
+Sound mg_calib_inp[C_MG_LENGTH] = {MGC_TONE_PITCH_H, MGC_TONE_NOOP, MGC_TONE_NOOP, MGC_TONE_NOOP,MGC_TONE_NOOP};
+Sound mg_calib_bad[C_MG_LENGTH] = {MGC_TONE_PITCH_LONG_L, MGC_TONE_WAIT, MGC_TONE_PITCH_LONG_L, MGC_TONE_NOOP, MGC_TONE_NOOP};
+
 ErrorCode MagnetometerSensor::init() {
     // Checks if sensor is connected
     if (!MMC5983.begin(MMC5983_CS)) {
@@ -28,6 +40,110 @@ Magnetometer MagnetometerSensor::read() {
     Y = ((double)cy - sf)/sf;
     Z = ((double)cz - sf)/sf;
 
-    Magnetometer reading{X, Y, Z};
+    // 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};
     return reading;
+}
+
+void MagnetometerSensor::begin_calibration(BuzzerController& buzzer) {
+    Serial.println("[MAG] Calibration begin");
+    buzzer.play_tune(mg_calib_rdy, C_MG_LENGTH);
+    in_calibration_mode = true;
+    _calib_begin_timestamp = millis();
+    _calib_beeping = 1;
+    _calib_max_axis = {-INFINITY, -INFINITY, -INFINITY};
+    _calib_min_axis = {INFINITY, INFINITY, INFINITY};
+    _calib_magnitude_sum = 0.0;
+    _calib_num_datapoints = 0;
+}
+
+void MagnetometerSensor::calib_reading(Magnetometer& reading, EEPROMController& eeprom, BuzzerController& buzzer) {
+    // X reading
+    if(reading.mx > _calib_max_axis.mx) { _calib_max_axis.mx = reading.mx; };
+    if(reading.mx < _calib_min_axis.mx) { _calib_min_axis.mx = reading.mx; };
+    // Y
+    if(reading.my > _calib_max_axis.my) { _calib_max_axis.my = reading.my; };
+    if(reading.my < _calib_min_axis.my) { _calib_min_axis.my = reading.my; };
+    // Z
+    if(reading.mz > _calib_max_axis.mz) { _calib_max_axis.mz = reading.mz; };
+    if(reading.mz < _calib_min_axis.mz) { _calib_min_axis.mz = reading.mz; };
+
+    // Magnitude
+    double mag = sqrtf(reading.mx*reading.mx + reading.my*reading.my + reading.mz*reading.mz);
+    _calib_magnitude_sum += mag;
+    _calib_num_datapoints++;
+
+    // Beeping
+    if(get_time_since_calibration_start() > 15000 * _calib_beeping && _calib_beeping < 4) {
+        _calib_beeping++;
+        buzzer.play_tune(mg_calib_inp, C_MG_LENGTH);
+    }
+
+    if(get_time_since_calibration_start() > _calib_time) {
+        Serial.println("[MAG] Calibration done.");
+        commit_calibration(eeprom, buzzer);
+    }
+}
+
+bool MagnetometerSensor::calibration_valid(const Magnetometer& b, const Magnetometer& s) {
+    constexpr float kSensorEpsilon = 1e-6f;
+    constexpr float kSensorMaxShear = 5.0f; // probably should tweak this
+
+    if(s.mx < kSensorEpsilon || s.my < kSensorEpsilon || s.mz < kSensorEpsilon) {
+        // Scale data is too small, so we probably didn't get enough data
+        return false;
+    }
+
+    float s_min = fminf(s.mx, fminf(s.my, s.mz)); // Get minimum value of s
+    float s_max = fmaxf(s.mx, fmaxf(s.my, s.mz)); // Get minimum value of s
+
+    if(s_max > kSensorMaxShear * s_min) {
+        // this is iffy but could indicate strong sensor reference ellipsoid shear
+        // return false;
+    }
+
+    return true;
+}
+
+void MagnetometerSensor::restore_calibration(EEPROMController& eeprom) {
+    in_calibration_mode = false;
+
+    // Read back the calibration data from EEPROM:
+    calibration_bias_softiron = eeprom.data.mmc5983ma_softiron_bias;
+    calibration_bias_hardiron = eeprom.data.mmc5983ma_hardiron_bias;
+}
+
+void MagnetometerSensor::commit_calibration(EEPROMController& eeprom, BuzzerController& buzzer) {
+    in_calibration_mode = false;
+
+    Magnetometer b, s;
+    double magnitude_tgt = _calib_magnitude_sum / _calib_num_datapoints;
+
+    // b: Origin offset
+    b.mx = (_calib_min_axis.mx + _calib_max_axis.mx) / 2;
+    b.my = (_calib_min_axis.my + _calib_max_axis.my) / 2;
+    b.mz = (_calib_min_axis.mz + _calib_max_axis.mz) / 2;
+
+    // s: Scale offset
+    s.mx = (_calib_max_axis.mx - _calib_min_axis.mx) / (2*magnitude_tgt);
+    s.my = (_calib_max_axis.my - _calib_min_axis.my) / (2*magnitude_tgt);
+    s.mz = (_calib_max_axis.mz - _calib_min_axis.mz) / (2*magnitude_tgt);
+
+    Serial.print("MAG: ");
+    Serial.println(magnitude_tgt);
+
+    if(!calibration_valid(b, s)) {
+        Serial.println("[MAG] Calibration is bad.");
+        buzzer.play_tune(mg_calib_bad, C_MG_LENGTH);
+        return; // The calibration is garbage... this probably shouldn't fail silently but idc rn.
+    }
+
+    calibration_bias_hardiron = b;
+    calibration_bias_softiron = s;
+    buzzer.play_tune(mg_calib_done, C_MG_LENGTH);
+
+    eeprom.data.mmc5983ma_softiron_bias = calibration_bias_softiron;
+    eeprom.data.mmc5983ma_hardiron_bias = calibration_bias_hardiron;
+    eeprom.commit();
 }

MIDAS/src/hardware/sensors.h

@@ -54,6 +54,33 @@ struct IMUSensor {
 struct MagnetometerSensor {
     ErrorCode init();
     Magnetometer read();
+
+    // Calibration functions
+    bool in_calibration_mode = false;
+    void begin_calibration(BuzzerController& buzzer);
+    void calib_reading(Magnetometer& reading, EEPROMController& eeprom, BuzzerController& buzzer);
+    void restore_calibration(EEPROMController& eeprom);
+
+    Magnetometer calibration_bias_hardiron = {0.0, 0.0, 0.0}; // hard iron offset -- "recenters" data on origin (0,0,0).
+    Magnetometer calibration_bias_softiron = {1.0, 1.0, 1.0}; // soft iron offset -- scales per-axis data (Should be 3x3, but we'll try 1x3 for now.)
+
+    unsigned long get_time_since_calibration_start() { return millis() - _calib_begin_timestamp; }
+
+    private:
+    void commit_calibration(EEPROMController& eeprom, BuzzerController& buzzer); // Calculate and commit the calibration to memory
+    bool calibration_valid(const Magnetometer& b, const Magnetometer& s); // Calculate and sanity check calibration data
+    /* Maximum value per-axis during calibration */
+    Magnetometer _calib_max_axis; 
+    /* Minimum value per-axis during calibration */
+    Magnetometer _calib_min_axis;
+    unsigned long _calib_begin_timestamp;
+    double _calib_magnitude_sum = 0.0;
+    int _calib_num_datapoints = 0;
+    int _calib_beeping = 0;
+    /* Magnetometer calibration isn't based on a per-axis calibration, but on getting as many datapoints as possible.
+    For now let's try 60 sec */
+    const unsigned long _calib_time = 60000;
+
 };
 
 /**

MIDAS/src/systems.cpp

@@ -108,11 +108,28 @@ DECLARE_THREAD(imuthread, RocketSystems *arg)
 }
 
 DECLARE_THREAD(magnetometer, RocketSystems* arg) {
+    arg->sensors.magnetometer.restore_calibration(arg->eeprom);
+    int i = 0;
     while (true) {
         // xSemaphoreTake(spi_mutex, portMAX_DELAY);
         Magnetometer reading = arg->sensors.magnetometer.read();
         // xSemaphoreGive(spi_mutex);
+
+        // Sensor calibration
+        if(arg->sensors.magnetometer.in_calibration_mode) {
+            arg->sensors.magnetometer.calib_reading(reading, arg->eeprom, arg->buzzer);
+            // Mag calibration handles its own calibration timing.
+        }
+
+        // Sensor biases
+        Magnetometer b = arg->sensors.magnetometer.calibration_bias_hardiron; // "Hard iron" / origin offset.
+        Magnetometer s = arg->sensors.magnetometer.calibration_bias_softiron; // "Soft iron" / scale offset.
+        reading.mx = (reading.mx - b.mx) / s.mx;
+        reading.my = (reading.my - b.my) / s.my;
+        reading.mz = (reading.mz - b.mz) / s.mz;
+
         arg->rocket_data.magnetometer.update(reading);
+
         THREAD_SLEEP(50);
     }
 }
@@ -330,9 +347,9 @@ DECLARE_THREAD(kalman, RocketSystems *arg)
 
         arg->rocket_data.kalman.update(current_state);
 
-        Serial.print("MQ tilt: ");
-        Serial.println(current_angular_kalman.mq_tilt);
-        Serial.println();
+        // Serial.print("MQ tilt: ");
+        // Serial.println(current_angular_kalman.mq_tilt);
+        // Serial.println();
 
         last = xTaskGetTickCount();
         // Serial.println("Kalman");
@@ -398,6 +415,9 @@ void handle_tlm_command(TelemetryCommand &command, RocketSystems *arg, FSMState
     case CommandType::CALIB_ACCEL:
         arg->sensors.imu.begin_calibration(arg->buzzer);
         break;
+    case CommandType::CALIB_MAG:
+        arg->sensors.magnetometer.begin_calibration(arg->buzzer);
+        break;
     default:
         break; // how
     }

MIDAS/src/telemetry_packet.h

@@ -50,7 +50,7 @@ struct TelemetryPacket {
 
 
 // Commands transmitted from ground station to rocket
-enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_ON, CAM_OFF, TOGGLE_CAM_VMUX, CALIB_ACCEL };
+enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_ON, CAM_OFF, TOGGLE_CAM_VMUX, CALIB_ACCEL, CALIB_MAG };
 
 /**
  * @struct TelemetryCommand

ground/src/feather_duo/Command.h

@@ -2,7 +2,7 @@
 #include<stdint.h>
 #include<array>
 
-enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_ON, CAM_OFF, TOGGLE_CAM_VMUX, CALIB_ACCEL, EMPTY };
+enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_ON, CAM_OFF, TOGGLE_CAM_VMUX, CALIB_ACCEL, CALIB_MAG, EMPTY };
 // Commands transmitted from ground station to rocket
 struct TelemetryCommand {
     CommandType command;

ground/src/feather_duo/main.cpp

@@ -146,6 +146,8 @@ void handle_serial(const String& key) {
         command.command = CommandType::TOGGLE_CAM_VMUX;
     } else if (cmd_name == "CXL") {
         command.command = CommandType::CALIB_ACCEL;
+    } else if (cmd_name == "CMG") {
+        command.command = CommandType::CALIB_MAG;
     } else {
         Serial.println(json_command_bad);
         return;