constant updates, imu/temp logic changes

src/MainControlLoop.cpp

@@ -17,7 +17,7 @@ MainControlLoop::MainControlLoop()
       burnwire_control_task(),
       camera_control_task(),
       rockblock_control_task(),
-      eeprom_control_task(), // TODO review
+      eeprom_control_task(),
       mission_manager(),
       clock_manager()
 {

src/MissionMode.cpp

@@ -522,7 +522,7 @@ void exit_armed_phase(MissionMode *mode)
 
 void exit_insun_phase()
 {
-    if ((sfr::temperature::temp_c_average->is_valid() && sfr::temperature::in_sun) ||
+    if (((sfr::temperature::temp_c_average->is_valid() && sfr::temperature::in_sun) && (sfr::current::solar_current_average->is_valid() && sfr::current::in_sun)) ||
         (!sfr::temperature::temp_c_average->is_valid() && sfr::current::solar_current_average->is_valid() && sfr::current::in_sun) ||
         (!sfr::temperature::temp_c_average->is_valid() && !sfr::current::solar_current_average->is_valid())) {
         sfr::mission::current_mode = sfr::mission::bootSensors;

src/Monitors/IMUMonitor.cpp

@@ -205,9 +205,9 @@ void IMUMonitor::imu_offset()
     sfr::imu::mag_z_value->set_value(mag_z - mag_zoffset);
 
     // make gyro aligh with mag coor
-    sfr::imu::gyro_x_value->set_value(-(gyro_x - (-0.02297)));
-    sfr::imu::gyro_y_value->set_value(gyro_y - (0.03015));
-    sfr::imu::gyro_z_value->set_value(gyro_z - (-0.01396));
+    sfr::imu::gyro_x_value->set_value(-(gyro_x - (-0.001216)));
+    sfr::imu::gyro_y_value->set_value(gyro_y - (0.116));
+    sfr::imu::gyro_z_value->set_value(gyro_z - (0.0133));
 }
 
 // generate a normal random variable using Box-Muller transform
@@ -354,43 +354,20 @@ void IMUMonitor::capture_imu_values()
         gyro_z = 0;
     }
 
-    //  Pass sensor data / output of plant into ekf
-    ekfObj.Z(0) = mag_x;
-    ekfObj.Z(1) = mag_y;
-    ekfObj.Z(2) = mag_z;
-    ekfObj.Z(3) = gyro_x;
-    ekfObj.Z(4) = gyro_y;
-    ekfObj.Z(5) = gyro_z;
-
-    ekfObj.step();
-
-#ifdef IMU_TESTING
-    Serial.print(ekfObj.state(0));
-    Serial.print(", ");
-    Serial.print(ekfObj.state(1));
-    Serial.print(", ");
-    Serial.print(ekfObj.state(2));
-    Serial.print(", ");
-    Serial.print(ekfObj.state(3));
-    Serial.print(", ");
-    Serial.print(ekfObj.state(4));
-    Serial.print(", ");
-    Serial.println(ekfObj.state(5));
-#endif
-    // update the EKFed values
-    sfr::imu::mag_x_value->set_value(ekfObj.state(0));
-    sfr::imu::mag_y_value->set_value(ekfObj.state(1));
-    sfr::imu::mag_z_value->set_value(ekfObj.state(2));
-    sfr::imu::gyro_x_value->set_value(ekfObj.state(3));
-    sfr::imu::gyro_y_value->set_value(ekfObj.state(4));
-    sfr::imu::gyro_z_value->set_value(ekfObj.state(5));
+    // add reading to buffer
+    sfr::imu::mag_x_value->set_value(mag_x);
+    sfr::imu::mag_y_value->set_value(mag_y);
+    sfr::imu::mag_z_value->set_value(mag_z);
+    sfr::imu::gyro_x_value->set_value(gyro_x);
+    sfr::imu::gyro_y_value->set_value(gyro_y);
+    sfr::imu::gyro_z_value->set_value(gyro_z);
 
     // Add offset readings to buffer
-    sfr::imu::mag_x_average->set_value(ekfObj.state(0));
-    sfr::imu::mag_y_average->set_value(ekfObj.state(1));
-    sfr::imu::mag_z_average->set_value(ekfObj.state(2));
+    sfr::imu::mag_x_average->set_value(mag_x);
+    sfr::imu::mag_y_average->set_value(mag_y);
+    sfr::imu::mag_z_average->set_value(mag_z);
     // used outside of ACS Control Task to determine exit conditions for Detumble Spin
-    sfr::imu::gyro_x_average->set_value(ekfObj.state(3));
-    sfr::imu::gyro_y_average->set_value(ekfObj.state(4));
-    sfr::imu::gyro_z_average->set_value(ekfObj.state(5));
+    sfr::imu::gyro_x_average->set_value(gyro_x);
+    sfr::imu::gyro_y_average->set_value(gyro_y);
+    sfr::imu::gyro_z_average->set_value(gyro_z);
 }

src/Monitors/TemperatureMonitor.cpp

@@ -11,7 +11,7 @@ void TemperatureMonitor::execute()
     sfr::temperature::temp_c_value->set_value(val);
 
     if (sfr::temperature::temp_c_average->get_value(&val)) {
-        sfr::temperature::in_sun = val >= constants::temperature::in_sun_val;
+        sfr::temperature::in_sun = val >= sfr::temperature::in_sun_val;
     } else {
         sfr::temperature::in_sun = false;
     }

src/Pins.cpp

@@ -45,8 +45,8 @@ void Pins::setInitialPinStates()
     Pins::setPinState(constants::camera::power_on_pin, LOW);
     Pins::setPinState(constants::camera::rx, LOW);
     Pins::setPinState(constants::camera::tx, LOW);
-    Pins::setPinState(constants::acs::STBXYpin, HIGH);
-    Pins::setPinState(constants::acs::STBZpin, HIGH);
+    Pins::setPinState(constants::acs::STBXYpin, LOW);
+    Pins::setPinState(constants::acs::STBZpin, LOW);
     Pins::setPinState(constants::burnwire::first_pin, LOW);
     Pins::setPinState(constants::burnwire::second_pin, LOW);
     Pins::setPinState(constants::rockblock::sleep_pin, LOW);

src/constants.hpp

@@ -89,13 +89,12 @@ namespace constants {
     } // namespace rockblock
     namespace temperature {
         constexpr int pin = 39;
-        constexpr int in_sun_val = 30;
         constexpr int min_temp_c = -100;
         constexpr int max_temp_c = 200;
     } // namespace temperature
     namespace current {
         constexpr int pin = 22;
-        constexpr float in_sun_val = 70; // mA
+        constexpr float in_sun_val = 50; // mA
         constexpr int load = 30;         // load resister value (kOhm)
         constexpr float shunt = 0.1;     // shunt resistor value (Ohm)
     } // namespace current
@@ -219,9 +218,9 @@ namespace constants {
         constexpr float temp_z_c = -1.7573124306246634684924856628641;
 
         // Hard Iron Offsets
-        constexpr float hardiron_x = 21.463;
-        constexpr float hardiron_y = 25.745;
-        constexpr float hardiron_z = -11.539;
+        constexpr float hardiron_x = 31.5;
+        constexpr float hardiron_y = 14.5;
+        constexpr float hardiron_z = -6.0;
 
         // Starshot
         constexpr float step_size_input = 0.10;
@@ -261,8 +260,8 @@ namespace constants {
         constexpr float min_mag = -150;
         constexpr float max_mag = 150;
 
-        constexpr float min_gyro = -5;
-        constexpr float max_gyro = 5;
+        constexpr float min_gyro = -10;
+        constexpr float max_gyro = 10;
 
         constexpr int CSAG = 21;
         constexpr int CSM = 20;
@@ -293,7 +292,7 @@ namespace constants {
         static constexpr unsigned int dynamic_data_start = 10;
         static constexpr unsigned int sfr_data_start = 460;
         static constexpr unsigned int sfr_store_size = 5;
-        static constexpr unsigned int sfr_num_fields = 93;
+        static constexpr unsigned int sfr_num_fields = 94;
         static constexpr unsigned int sfr_data_full_offset = sfr_num_fields * sfr_store_size + 4;
         static constexpr unsigned int write_age_limit = 95000; // Must be less than 100000
 

src/sfr.cpp

@@ -4,11 +4,11 @@ namespace sfr {
     namespace stabilization {
         // OP Codes 1100
         // TODO actual default value
-        SFRField<uint32_t> max_time = SFRField<uint32_t>(2 * constants::time::one_hour, 0x1100);
+        SFRField<uint32_t> max_time = SFRField<uint32_t>(5 * constants::time::one_second, 0x1100);
     } // namespace stabilization
     namespace boot {
         // OP Codes 1200
-        SFRField<uint32_t> max_time = SFRField<uint32_t>(2 * constants::time::one_hour, 0x1200);
+        SFRField<uint32_t> max_time = SFRField<uint32_t>(1 * constants::time::one_hour, 0x1200);
     } // namespace boot
     namespace detumble {
         // OP Codes 1500
@@ -111,12 +111,12 @@ namespace sfr {
         // OP Codes 1900
         SFRField<uint16_t> attempts = SFRField<uint16_t>(0, 0x1900);
         SFRField<uint16_t> mode = SFRField<uint16_t>((uint16_t)burnwire_mode_type::standby, 0x1901);
-        SFRField<uint16_t> attempts_limit = SFRField<uint16_t>(10, 0x1902);
+        SFRField<uint16_t> attempts_limit = SFRField<uint16_t>(11, 0x1902);
         SFRField<uint16_t> mandatory_attempts_limit = SFRField<uint16_t>(2, 0x1903);
         SFRField<uint32_t> start_time = SFRField<uint32_t>(0, 0x1904);
         SFRField<uint32_t> burn_time = SFRField<uint32_t>(600, 0, 5 * constants::time::one_second, 0x1905);
         SFRField<uint32_t> armed_time = SFRField<uint32_t>(48 * constants::time::one_hour, 0, 12 * constants::time::one_hour, 0x1906);
-        SFRField<uint32_t> delay_time = SFRField<uint32_t>(constants::time::one_second, 0x1907);
+        SFRField<uint32_t> delay_time = SFRField<uint32_t>(5 * constants::time::one_second, 0x1907);
     } // namespace burnwire
     namespace camera {
         // OP Codes 2000
@@ -203,6 +203,7 @@ namespace sfr {
     namespace temperature {
         // OP Codes 2300
         SFRField<bool> in_sun = SFRField<bool>(false, 0x2300);
+        SFRField<uint8_t> in_sun_val = SFRField<uint8_t>(23, 0x2301);
 
         SensorReading *temp_c_average = new SensorReading(fault_groups::power_faults::temp_c_average, 1500, constants::temperature::min_temp_c, constants::temperature::max_temp_c);
         SensorReading *temp_c_value = new SensorReading(fault_groups::power_faults::temp_c_value, 1, constants::temperature::min_temp_c, constants::temperature::max_temp_c);
@@ -264,8 +265,8 @@ namespace sfr {
             {constants::camera::rx, LOW},
             {constants::camera::tx, LOW},
             {constants::button::button_pin, HIGH},
-            {constants::acs::STBXYpin, HIGH},
-            {constants::acs::STBZpin, HIGH},
+            {constants::acs::STBXYpin, LOW},
+            {constants::acs::STBZpin, LOW},
             {constants::burnwire::first_pin, LOW},
             {constants::burnwire::second_pin, LOW},
             {constants::rockblock::sleep_pin, LOW}};

src/sfr.hpp

@@ -224,6 +224,7 @@ namespace sfr {
     namespace temperature {
         // OP Codes 2300
         extern SFRField<bool> in_sun;
+        extern SFRField<uint8_t> in_sun_val;
 
         extern SensorReading *temp_c_average;
         extern SensorReading *temp_c_value;