From 371f443fe380b4d1645c86996371b61c1335940b Mon Sep 17 00:00:00 2001
From: Federico Mandelli <federicomandelli@skywarder.eu>
Date: Wed, 2 Aug 2023 12:04:48 +0200
Subject: [PATCH] [Bugfixes] Fixed bugs in FMM and NASController
---
src/boards/Parafoil/Configs/NASConfig.h | 2 +-
.../FlightModeManager/FlightModeManager.cpp | 2 +-
.../StateMachines/NASController/NASController.cpp | 14 +++++---------
3 files changed, 7 insertions(+), 11 deletions(-)
diff --git a/src/boards/Parafoil/Configs/NASConfig.h b/src/boards/Parafoil/Configs/NASConfig.h
index 9c9f381ae..408ff859a 100644
--- a/src/boards/Parafoil/Configs/NASConfig.h
+++ b/src/boards/Parafoil/Configs/NASConfig.h
@@ -37,7 +37,7 @@ constexpr uint32_t UPDATE_PERIOD = 20; // [ms]
constexpr int CALIBRATION_SAMPLES_COUNT = 20;
constexpr int CALIBRATION_SLEEP_TIME = 100; // [ms]
-constexpr float ACCELERATION_THRESHOLD = 0.5; // [m/s^2]
+constexpr float ACCELERATION_THRESHOLD = 2; // [m/s^2]
static const Boardcore::NASConfig config = {
UPDATE_PERIOD / 1000.0, // T
diff --git a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
index b372abadc..6d9c8609f 100644
--- a/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
+++ b/src/boards/Parafoil/StateMachines/FlightModeManager/FlightModeManager.cpp
@@ -274,7 +274,7 @@ State FlightModeManager::state_disarmed(const Event& event)
case EV_ENTRY:
{
logStatus(FlightModeManagerState::DISARMED);
- EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FMM);
+ EventBroker::getInstance().post(FLIGHT_DISARMED, TOPIC_FLIGHT);
return transition(
&FlightModeManager::state_flying); // the parafoil is always
diff --git a/src/boards/Parafoil/StateMachines/NASController/NASController.cpp b/src/boards/Parafoil/StateMachines/NASController/NASController.cpp
index eeddbe5fc..17573d6c5 100644
--- a/src/boards/Parafoil/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Parafoil/StateMachines/NASController/NASController.cpp
@@ -74,23 +74,19 @@ void NASController::update()
nas.correctGPS(gpsData);
nas.correctBaro(pressureData.pressure);
- // Correct with accelerometer if the acceleration is in specs
- Vector3f acceleration = static_cast<AccelerometerData>(imuData);
- if (accelerationValid &&
- (acceleration.norm() > (9.8 + ACCELERATION_THRESHOLD) ||
- acceleration.norm() < (9.8 - ACCELERATION_THRESHOLD)))
- accelerationValid = false;
if (accelerationValid)
nas.correctAcc(imuData);
- if (!accelerationValid &&
- (acceleration.norm() < (9.8 + ACCELERATION_THRESHOLD) ||
- acceleration.norm() > (9.8 - ACCELERATION_THRESHOLD)))
+ // Correct with accelerometer if the acceleration is in specs
+ Vector3f acceleration = static_cast<AccelerometerData>(imuData);
+ if (acceleration.norm() < (9.8 + (ACCELERATION_THRESHOLD) / 2) &&
+ acceleration.norm() > (9.8 - (ACCELERATION_THRESHOLD) / 2))
{
accSampleAfterSpike++;
}
else
{
+ accelerationValid = false;
accSampleAfterSpike = 0;
}
if (accSampleAfterSpike > 50)
--
GitLab