diff --git a/src/boards/Parafoil/Configs/NASConfig.h b/src/boards/Parafoil/Configs/NASConfig.h index 9c9f381aeb603e357f829a6358e3456d69359154..408ff859a898dbbb4e835f012575e5f12ce28ceb 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 b372abadc3820a2e6426ac0b72fde157e148ffae..6d9c8609f82e5df14b6827892bd0cd62c7b42991 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 eeddbe5fc2db89cfaef937520386fb5ec23aadd0..17573d6c55fa67de1a3143fbabd372e1ee57c784 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)