diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp index 1d3d165b1ce1244f31c43bc4bcce9e7cf5fc41d3..feac32eb5a99c30a74a3b785865b55789421f23e 100644 --- a/src/boards/Payload/CanHandler/CanHandler.cpp +++ b/src/boards/Payload/CanHandler/CanHandler.cpp @@ -125,6 +125,7 @@ CanHandler::CanHandler() PITOT_TRANSMISSION_PERIOD); EventBroker::getInstance().subscribe(this, TOPIC_TMTC); + EventBroker::getInstance().subscribe(this, TOPIC_FLIGHT); } void CanHandler::handleCanMessage(const CanMessage &msg) diff --git a/src/boards/Payload/Configs/CanHandlerConfig.h b/src/boards/Payload/Configs/CanHandlerConfig.h index c5b38443467b5f8176303ca31005b23311c716ad..5e1917ff76cb2f155f9e1e467b4eed9231cbc8a7 100644 --- a/src/boards/Payload/Configs/CanHandlerConfig.h +++ b/src/boards/Payload/Configs/CanHandlerConfig.h @@ -34,7 +34,7 @@ namespace Payload namespace CanHandlerConfig { - +// Important to use only TMTC events because the Can sends on that topic static const std::map<Common::CanConfig::EventId, Common::Events> eventToEvent{ {Common::CanConfig::EventId::ARM, Common::TMTC_ARM}, {Common::CanConfig::EventId::DISARM, Common::TMTC_DISARM}, diff --git a/src/boards/Payload/Configs/WingConfig.h b/src/boards/Payload/Configs/WingConfig.h index 38ba5ea432a77c6d94beee54252ceca238433234..c9c0d1b7b734ca9feac1d3d4156fdb9f9555f6f2 100644 --- a/src/boards/Payload/Configs/WingConfig.h +++ b/src/boards/Payload/Configs/WingConfig.h @@ -46,13 +46,22 @@ constexpr float WING_CALIBRATION_PRESSURE = 101325; // [Pa] constexpr float WING_CALIBRATION_TEMPERATURE = 300; // [K] constexpr uint8_t WING_PRESSURE_MEAN_COUNT = 20; +#if defined(EUROC) +constexpr float DEFAULT_TARGET_LAT = 39.389733; +constexpr float DEFAULT_TARGET_LON = -8.288992; +#elif defined(ROCCARASO) +constexpr float DEFAULT_TARGET_LAT = 41.8086605; +constexpr float DEFAULT_TARGET_LON = 14.0543387; +#else // Milan constexpr float DEFAULT_TARGET_LAT = 42; constexpr float DEFAULT_TARGET_LON = 9; +#endif // Wing altitude checker configs -constexpr float WING_ALTITUDE_CHECKER_TASK_ID = 230; -constexpr float WING_ALTITUDE_CHECKER_PERIOD = 100; // [ms] -constexpr float WING_ALTITUDE_REFERENCE = 400; +constexpr float WING_ALTITUDE_CHECKER_TASK_ID = 230; +constexpr float WING_ALTITUDE_CHECKER_PERIOD = 100; // [ms] +constexpr float WING_ALTITUDE_REFERENCE = 450; +constexpr int WING_ALTITUDE_TRIGGER_CONFIDENCE = 5; } // namespace WingConfig diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp index cba577c5dfaca63f65df524e61ed0f461a14eab2..7ab7a6a12ee4fd04af110869680695ae3b6ba605 100644 --- a/src/boards/Payload/Radio/Radio.cpp +++ b/src/boards/Payload/Radio/Radio.cpp @@ -83,11 +83,7 @@ Boardcore::MavlinkStatus Radio::getMavlinkStatus() return mavDriver->getStatus(); } -void Radio::logStatus() -{ - Logger::getInstance().log(mavDriver->getStatus()); - // TODO: Add transceiver status logging -} +void Radio::logStatus() { Logger::getInstance().log(mavDriver->getStatus()); } Radio::Radio() { diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp index fb39a01053c40159a33b223a8d423a561dc6eb0c..3bb500f63f3223365b1bac1ac1240b9f176f470e 100644 --- a/src/boards/Payload/Sensors/Sensors.cpp +++ b/src/boards/Payload/Sensors/Sensors.cpp @@ -166,7 +166,7 @@ void Sensors::calibrate() dplPressure->setOffset(dplPressureMean - ms5803Mean); pitotPressure->setOffset(pitotPressureStats.getStats().mean); - calibrating = true; + calibrating = false; } void Sensors::pitotSetReferenceAltitude(float altitude) diff --git a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp index 433cde9dc7a9639a3dfa4327898674c93eb251a1..89cbe102077686717bf88267492bb09435ea036c 100644 --- a/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp +++ b/src/boards/Payload/StateMachines/FlightModeManager/FlightModeManager.cpp @@ -361,7 +361,6 @@ State FlightModeManager::state_armed(const Event& event) { return transition(&FlightModeManager::state_disarmed); } - // TODO: Reviews the liftoff event case TMTC_FORCE_LAUNCH: { return transition(&FlightModeManager::state_flying); diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp index 2ea33a82c6561bea2f8abf4683d7e2e63aa04f4a..bd7b98a2fc85db97dbfdcc390ebf57daca8075b8 100644 --- a/src/boards/Payload/StateMachines/NASController/NASController.cpp +++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp @@ -54,7 +54,8 @@ bool NASController::start() void NASController::update() { // If the nas is not active i skip the step - if (this->testState(&NASController::state_active)) + if (this->testState(&NASController::state_active) && + Sensors::getInstance().getUbxGpsLastSample().fix != 0) { auto imuData = Sensors::getInstance().getBMX160WithCorrectionLastSample(); diff --git a/src/boards/Payload/Wing/AltitudeTrigger.cpp b/src/boards/Payload/Wing/AltitudeTrigger.cpp index c9a65d1067a9a1c4cadfb52df64001d246ce979e..9900ea3f84d6a7c2b610362224540775ec27e5fb 100644 --- a/src/boards/Payload/Wing/AltitudeTrigger.cpp +++ b/src/boards/Payload/Wing/AltitudeTrigger.cpp @@ -44,8 +44,9 @@ AltitudeTrigger::AltitudeTrigger() bind(&AltitudeTrigger::update, this), WING_ALTITUDE_CHECKER_PERIOD, WING_ALTITUDE_CHECKER_TASK_ID); - // Set also the altitude to the default one - altitude = WING_ALTITUDE_REFERENCE; + // Set the altitude to the default one + altitude = WING_ALTITUDE_REFERENCE; + confidence = 0; } void AltitudeTrigger::setDeploymentAltitude(float alt) @@ -62,8 +63,16 @@ void AltitudeTrigger::update() NASState state = NASController::getInstance().getNasState(); if (-state.d < altitude) + confidence++; + + // When we are sure that the altitude is below the set one we trigger + // the cutters + if (confidence >= WING_ALTITUDE_TRIGGER_CONFIDENCE) + { + confidence = 0; EventBroker::getInstance().post(FLIGHT_WING_ALT_REACHED, TOPIC_FLIGHT); + } } } } // namespace Payload diff --git a/src/boards/Payload/Wing/AltitudeTrigger.h b/src/boards/Payload/Wing/AltitudeTrigger.h index 7e2ae9b30d5f2979ece14b11a42e960b66e864d0..bd14a3f3a67cad40d8a21ff676d71d518ae0aa37 100644 --- a/src/boards/Payload/Wing/AltitudeTrigger.h +++ b/src/boards/Payload/Wing/AltitudeTrigger.h @@ -44,5 +44,9 @@ private: // The altitude could be different from the default one float altitude; + + // Number of times that the algorithm detects to be below the fixed + // altitude + int confidence; }; } // namespace Payload