diff --git a/CMakeLists.txt b/CMakeLists.txt index 4aa347cdf592935741311b6b54d4d52cb7edd373..11d68fa236ff1d7089ebc53b684a179763e35b1b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,8 +134,8 @@ sbs_target(parafoil-guided-jesolo stm32f429zi_death_stack_v2) add_executable(parafoil-guided-dynamic src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER}) target_include_directories(parafoil-guided-dynamic PRIVATE ${OBSW_INCLUDE_DIRS}) target_compile_definitions(parafoil-guided-dynamic PRIVATE ALGORITHM_CLOSED_LOOP DYNAMIC_TARGET - DYNAMIC_TARGET_LATITUDE_OFFSET=50 - DYNAMIC_TARGET_LONGITUDE_OFFSET=50 + DYNAMIC_TARGET_LATITUDE_OFFSET=35 + DYNAMIC_TARGET_LONGITUDE_OFFSET=-156 ) sbs_target(parafoil-guided-dynamic stm32f429zi_death_stack_v2) @@ -151,10 +151,10 @@ sbs_target(parafoil-t-approach-milano stm32f429zi_death_stack_v2) add_executable(parafoil-t-approach-dynamic src/Parafoil/parafoil-entry.cpp ${PARAFOIL_COMPUTER}) target_include_directories(parafoil-t-approach-dynamic PRIVATE ${OBSW_INCLUDE_DIRS}) -target_compile_definitions(parafoil-t-approach-dynamic PRIVATE + target_compile_definitions(parafoil-t-approach-dynamic PRIVATE ALGORITHM_EARLY_MANEUVER DYNAMIC_TARGET - DYNAMIC_TARGET_LATITUDE_OFFSET=50 - DYNAMIC_TARGET_LONGITUDE_OFFSET=50 + DYNAMIC_TARGET_LATITUDE_OFFSET=35 + DYNAMIC_TARGET_LONGITUDE_OFFSET=-156 ) sbs_target(parafoil-t-approach-dynamic stm32f429zi_death_stack_v2) diff --git a/src/Parafoil/Configs/SensorsConfig.h b/src/Parafoil/Configs/SensorsConfig.h index 5cf4c208fc3a917fa222c58199ad00c1ffde3ff0..8a59372446b1a660a0d22f33872963d84d7422e5 100644 --- a/src/Parafoil/Configs/SensorsConfig.h +++ b/src/Parafoil/Configs/SensorsConfig.h @@ -55,7 +55,7 @@ constexpr auto ACC_GYRO_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_200; constexpr auto MAG_ODR = Boardcore::BMX160Config::OutputDataRate::HZ_100; static const Boardcore::AxisOrthoOrientation AXIS_ORIENTATION = { - Boardcore::Direction::POSITIVE_Z, Boardcore::Direction::POSITIVE_Y}; + Boardcore::Direction::NEGATIVE_Z, Boardcore::Direction::POSITIVE_Y}; constexpr auto TEMP_DIVIDER = 1000; constexpr auto FIFO_WATERMARK = 40; diff --git a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp index 562f73a0f1fffee3b2ade80f0191723c08419a1e..24eb3dffe58f19a8e48b197ac992c19621cd5fb5 100644 --- a/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -233,8 +233,7 @@ State FlightModeManager::PreFlightSensorCalibration(const Event& event) { case EV_ENTRY: { - updateState( - FlightModeManagerState::PRE_FLIGHT_ALGORITHM_CALIBRATION); + updateState(FlightModeManagerState::PRE_FLIGHT_SENSOR_CALIBRATION); // Wait for sensors to stabilize before calibration // The first few LPS28DFW samples contain garbage data diff --git a/src/Parafoil/StateMachines/NASController/NASController.cpp b/src/Parafoil/StateMachines/NASController/NASController.cpp index d3f7dce6c60ee85b01a6cad360f8e0d689edfb77..bc8de17faa7c785fbad86aa606a8d8652df0e99f 100644 --- a/src/Parafoil/StateMachines/NASController/NASController.cpp +++ b/src/Parafoil/StateMachines/NASController/NASController.cpp @@ -282,8 +282,7 @@ void NASController::update() // Perform initial NAS prediction nas.predictGyro(imu); nas.predictAcc(imu); - - // NOTE: Magnetometer correction has been disabled + nas.correctMag(imu); if (lastGpsTimestamp < gps.gpsTimestamp && gps.fix == 3 && accLength < Config::NAS::DISABLE_GPS_ACCELERATION_THRESHOLD)