MIDAS Mini LUNA Cleanup

MIDAS/lib/TCAL9538/TCAL9538.cpp

@@ -32,10 +32,6 @@ bool TCAL9538Init(int reset_pin){
         TwoWire& wire = tcal_get_wire_by_id(i);
         uint8_t addr = addrs[i];
 
-        // remove this when done
-        Serial.print("Testing ");
-        Serial.println(addr);
-
         wire.beginTransmission(addr);
         wire.write(REG_OUTPUT);
         if(wire.endTransmission() != 0){

MIDAS/src/data_logging.cpp

@@ -24,7 +24,7 @@ ASSOCIATE(FSMState, ID_FSM)
 ASSOCIATE(PyroState, ID_PYRO)
 ASSOCIATE(CameraData, ID_CAMERADATA)
 ASSOCIATE(AngularKalmanData, ID_ANGULARKALMAN)
-ASSOCIATE(IMU_SFLP, ID_SFLPHW)
+ASSOCIATE(IMU_SFLP, ID_SFLP)
 
 
 /**
@@ -83,15 +83,11 @@ void log_data(LogSink& sink, RocketData& data) {
     log_from_sensor_data(sink, data.voltage);
     log_from_sensor_data(sink, data.gps);
     log_from_sensor_data(sink, data.magnetometer);
-    //log_from_sensor_data(sink, data.orientation);
     log_from_sensor_data(sink, data.fsm_state);
     log_from_sensor_data(sink, data.kalman);
     log_from_sensor_data(sink, data.angular_kalman_data);
     log_from_sensor_data(sink, data.pyro);
-
-    //log_from_sensor_data(sink, data.quaternions);
     log_from_sensor_data(sink, data.cam_data);
-
 }
 
 #ifndef SILSIM

MIDAS/src/hardware/Barometer.cpp

@@ -28,7 +28,7 @@ Barometer BarometerSensor::read() {
     */
     uint32_t pressure = MS.getPressure(); // Pascals
     float temperature = MS.getTemperature(); // Celcius
-    float altitude = MS.getAltitude();
+    float altitude = MS.getAltitudeExtendedModel();
 
     return Barometer(temperature, pressure, altitude);
 }

MIDAS/src/hardware/IMU.cpp

@@ -36,7 +36,6 @@ IMU IMUSensor::read(){
 
     //Angular rate
     if(status.gda){
-        //Serial.println("oh we are angular");
         LSM6DSV.get_angular_velocity_from_fs2000_to_dps(&reading.angular_velocity.vx, 
                                                     &reading.angular_velocity.vy, 
                                                     &reading.angular_velocity.vz);

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.mq_tilt < MAXIMUM_TILT_ANGLE;
 }
 
 

MIDAS/src/hardware/pins.h

@@ -1,9 +1,9 @@
 #pragma once
 
 // SPI sensor bus
-#define SPI_MISO 3//
-#define SPI_MOSI 2//
-#define SPI_SCK 1//
+#define SPI_MISO 3
+#define SPI_MOSI 2
+#define SPI_SCK 1
 
 // barometer chip select
 #define MS5611_CS 44
@@ -17,21 +17,21 @@
 #define IMU_IRQ_PIN 15
 
 // i2c bus pins
-#define I2C_SDA 21//
-#define I2C_SCL 26//
+#define I2C_SDA 21
+#define I2C_SCL 26
 
 // Buzzer pins
 #define BUZZER_PIN 12
 #define BUZZER_CHANNEL 1
 
 // GPIO Expander pins
 #define EXP_RESET 13
-#define EXP_INT 48//
+#define EXP_INT 48
 
 // GPS pins
-#define SAM_RESET 5//
+#define SAM_RESET 5
 
-// pyro pins //hmmm
+// pyro pins
 #define PYRO_GLOBAL_ARM_PIN GpioAddress(0, 3)
 #define PYROA_FIRE_PIN GpioAddress(0, 0)
 #define PYROB_FIRE_PIN GpioAddress(0, 1)
@@ -46,30 +46,29 @@
 #define SENSE_D 5
 
 // Voltage pins (on the ADC)
-#define PYRO_SENSE 2 //hmmm
+#define PYRO_SENSE 2
 #define VCAP_SENSE 6
 #define VBAT_SENSE 7
 
 // E22 (radio)
-#define E22_CS 37//
-#define E22_DI01 41//
-#define E22_DI03 40//
-#define E22_BUSY 38//
-#define E22_RXEN 39//
-#define E22_RESET 6//
-
-// LEDs hmmmm
+#define E22_CS 37
+#define E22_DI01 41
+#define E22_DI03 40
+#define E22_BUSY 38
+#define E22_RXEN 39
+#define E22_RESET 6
+
+// LEDs
 #define LED_BLUE   8 
 #define LED_GREEN  9 
 #define LED_ORANGE 10 
 #define LED_RED    11
 
 // FLASH memory pins
-
-#define FLASH_CMD 47//
+#define FLASH_CMD 47
 #define FLASH_CLK 16
-#define FLASH_DAT0 33//
-#define FLASH_DAT1 34//
+#define FLASH_DAT0 33
+#define FLASH_DAT1 34
 #define FLASH_DAT2 18
 #define FLASH_DAT3 17
 

MIDAS/src/log_format.h

@@ -18,7 +18,7 @@ enum ReadingDiscriminant {
     ID_PYRO = 10,
     ID_CAMERADATA = 11,
     ID_ANGULARKALMAN = 12,
-    ID_SFLPHW = 13,
+    ID_SFLP = 13,
 };
 
 
@@ -39,12 +39,11 @@ struct LoggedReading {
     uint32_t timestamp_ms;
     union {
         IMU imu;
-        IMU_SFLP hw_filt;
+        IMU_SFLP sflp;
         Barometer barometer;
         Voltage voltage;
         GPS gps;
         Magnetometer magnetometer;
-        //Orientation orientation;
         KalmanData kalman;
         AngularKalmanData angular_kalman;
         FSMState fsm;

MIDAS/src/rocket_state.h

@@ -184,8 +184,7 @@ struct CommandFlags {
  */
 struct RocketData {
 public:
-    //SensorData<LSM> lsm;
-
+
     SensorData<KalmanData> kalman;
     SensorData<AngularKalmanData> angular_kalman_data;
     BufferedSensorData<IMU, 16> imu;

MIDAS/src/sensor_data.h

@@ -142,18 +142,9 @@ enum class OrientationReadingType {
 /**
  * @struct SFLP
  * 
- * @brief Data from the Sensor Fusion Low Power module
+ * @brief Data from the LSM6DSV320X Sensor Fusion Low Power module
  * 
  */
-
-/**
-*
-* @brief Implementing obtaining SFLP data from FIFO
-*
-*
-*/
-
-// Raw IMU data from the LS6DSV320X this is hw filtered
 struct IMU_SFLP {
     Quaternion quaternion;
     Acceleration gravity;
@@ -273,10 +264,8 @@ struct KalmanData {
 struct AngularKalmanData {
     Quaternion quaternion;
     float gyrobias[3];
-    double comp_tilt = 0.0;
     double mq_tilt = 0.0;
     bool has_data = false;
-    OrientationReadingType reading_type = OrientationReadingType::FULL_READING;
 
     float yaw = 0;
     float pitch = 0;

MIDAS/src/systems.cpp

@@ -61,12 +61,7 @@ DECLARE_THREAD(barometer, RocketSystems *arg)
             arg->rocket_data.barometer.update(reading);
             prev_reading = reading; // Only update prev_reading with accepted readings
         }
-        // Serial.print("Barometer ");
-        // Serial.print(reading.altitude);
-        // Serial.print(" ");
-        // Serial.print(reading.pressure);
-        // Serial.print(" ");
-        // Serial.println(reading.temperature);
+
         THREAD_SLEEP(6);
     }
 }
@@ -263,7 +258,7 @@ DECLARE_THREAD(buzzer, RocketSystems *arg)
 DECLARE_THREAD(angularkalman, RocketSystems *arg)
 { //
     mqekf.initialize(arg);
-    // Serial.println("Initialized ekf :(");
+    // Serial.println("Initialized mqekf :(");
     TickType_t last = xTaskGetTickCount();
 
     while (true)
@@ -301,7 +296,7 @@ DECLARE_THREAD(angularkalman, RocketSystems *arg)
         arg->rocket_data.angular_kalman_data.update(current_state);
 
         last = xTaskGetTickCount();
-        // Serial.println("Angular Kalman");
+
         THREAD_SLEEP(50);
     }
 }
@@ -347,12 +342,8 @@ 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();
-
         last = xTaskGetTickCount();
-        // Serial.println("Kalman");
+
         THREAD_SLEEP(50);
     }
 }

MIDAS/src/telemetry.cpp

@@ -109,20 +109,19 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) {
 
     //-------------------------------------------------------------------
 
-    // Tilt & FSM State --> comp_tilt vs mq_tilt
+    // Tilt & FSM State
     static_assert(FSMState::FSM_STATE_COUNT < 16);
-    uint16_t tilt_norm = (angular_kalman.comp_tilt / M_PI) * 0x0fff; // Encodes tilt value 0-1 into range 0x0000 - 0x0fff
+    uint16_t tilt_norm = (angular_kalman.mq_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);
 
     //-------------------------------------------------------------------
 
-    //Serial.println(packet.tilt_fsm, 2);
     // Battery voltage
     packet.batt_volt = inv_convert_range<uint8_t>(voltage.v_Bat, MAX_TELEM_VOLTAGE_V);
 
     // Roll rate
-    float roll_rate_hz = std::clamp(std::abs(imu.angular_velocity.vx) / (2.0f*static_cast<float>(PI)), 0.0f, MAX_ROLL_RATE_HZ);
+    float roll_rate_hz = std::clamp(std::abs(imu.angular_velocity.vx) / 360.0f, 0.0f, MAX_ROLL_RATE_HZ); // divide by 360 to convert from dps to Hz
     packet.roll_rate = roll_rate_hz / MAX_ROLL_RATE_HZ * 0xFF;
 
     // KF data

MIDAS/src/telemetry_packet.h

@@ -6,7 +6,7 @@
 #define MAX_TELEM_VOLTAGE_V 6.0f
 #define MAX_TELEM_CONT_I 0.2f
 #define MAX_KF_VPOSITION_M 100000.0f   // Max vertical position of KF data (m)
-#define MAX_KF_LPOSITION_M 100000.0f   // Max lateral position of KF data (m)
+#define MAX_KF_LPOSITION_M 50000.0f   // Max lateral position of KF data (m)
 #define MAX_ROLL_RATE_HZ 10.0f
 #define MAX_ABS_ACCEL_RANGE_G 64
 #define MAX_KF_XVELOCITY_MS 2000.0f
@@ -43,7 +43,7 @@ struct TelemetryPacket {
     uint16_t kf_py; // 16 bit meters
     uint16_t kf_pz; // 16 bit meters
 
-    uint32_t pyro; // 7 bit continuity x 4 channels, 4 bit unused
+    uint32_t pyro; // 8 bit continuity x 4 channels
 
     uint8_t roll_rate;
     uint8_t camera_state;

eMMC/.gitignore

@@ -3,3 +3,4 @@
 .vscode/c_cpp_properties.json
 .vscode/launch.json
 .vscode/ipch
+*.launch