From 7c29dfcae3c36913b2edff91f492b9aeaedcdff9 Mon Sep 17 00:00:00 2001
From: Davide Basso <davide.basso@skywarder.eu>
Date: Sat, 29 Mar 2025 14:16:29 +0100
Subject: [PATCH] [Parafoil] Enable mag correction, flip x axis
---
CMakeLists.txt | 10 +++++-----
src/Parafoil/Configs/SensorsConfig.h | 2 +-
.../FlightModeManager/FlightModeManager.cpp | 3 +--
.../StateMachines/NASController/NASController.cpp | 3 +--
4 files changed, 8 insertions(+), 10 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4aa347cdf..11d68fa23 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 5cf4c208f..8a5937244 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 562f73a0f..24eb3dffe 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 d3f7dce6c..bc8de17fa 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)
--
GitLab