diff --git a/src/MainControlLoop.cpp b/src/MainControlLoop.cpp index 22d7a370..634e7bf2 100644 --- a/src/MainControlLoop.cpp +++ b/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() { diff --git a/src/MissionMode.cpp b/src/MissionMode.cpp index 161b87d9..6c453fae 100644 --- a/src/MissionMode.cpp +++ b/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; diff --git a/src/Monitors/IMUMonitor.cpp b/src/Monitors/IMUMonitor.cpp index 2555bdde..3f2be27d 100644 --- a/src/Monitors/IMUMonitor.cpp +++ b/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); } diff --git a/src/Monitors/TemperatureMonitor.cpp b/src/Monitors/TemperatureMonitor.cpp index 4698bdb9..a9a31a1e 100644 --- a/src/Monitors/TemperatureMonitor.cpp +++ b/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; } diff --git a/src/Pins.cpp b/src/Pins.cpp index 4d62bbe8..74ea07dc 100644 --- a/src/Pins.cpp +++ b/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); diff --git a/src/constants.hpp b/src/constants.hpp index 6954e21c..2b9cbd6a 100644 --- a/src/constants.hpp +++ b/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 diff --git a/src/sfr.cpp b/src/sfr.cpp index 378a011e..dd05b575 100644 --- a/src/sfr.cpp +++ b/src/sfr.cpp @@ -4,11 +4,11 @@ namespace sfr { namespace stabilization { // OP Codes 1100 // TODO actual default value - SFRField max_time = SFRField(2 * constants::time::one_hour, 0x1100); + SFRField max_time = SFRField(5 * constants::time::one_second, 0x1100); } // namespace stabilization namespace boot { // OP Codes 1200 - SFRField max_time = SFRField(2 * constants::time::one_hour, 0x1200); + SFRField max_time = SFRField(1 * constants::time::one_hour, 0x1200); } // namespace boot namespace detumble { // OP Codes 1500 @@ -111,12 +111,12 @@ namespace sfr { // OP Codes 1900 SFRField attempts = SFRField(0, 0x1900); SFRField mode = SFRField((uint16_t)burnwire_mode_type::standby, 0x1901); - SFRField attempts_limit = SFRField(10, 0x1902); + SFRField attempts_limit = SFRField(11, 0x1902); SFRField mandatory_attempts_limit = SFRField(2, 0x1903); SFRField start_time = SFRField(0, 0x1904); SFRField burn_time = SFRField(600, 0, 5 * constants::time::one_second, 0x1905); SFRField armed_time = SFRField(48 * constants::time::one_hour, 0, 12 * constants::time::one_hour, 0x1906); - SFRField delay_time = SFRField(constants::time::one_second, 0x1907); + SFRField delay_time = SFRField(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 in_sun = SFRField(false, 0x2300); + SFRField in_sun_val = SFRField(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}}; diff --git a/src/sfr.hpp b/src/sfr.hpp index 0919a129..5be4f62b 100644 --- a/src/sfr.hpp +++ b/src/sfr.hpp @@ -224,6 +224,7 @@ namespace sfr { namespace temperature { // OP Codes 2300 extern SFRField in_sun; + extern SFRField in_sun_val; extern SensorReading *temp_c_average; extern SensorReading *temp_c_value;