From 8d00999be87ecf530f075c69d025c05bdfefbdaf Mon Sep 17 00:00:00 2001
From: Matteo Pignataro <matteo.pignataro@tiscali.it>
Date: Tue, 13 Sep 2022 18:45:29 +0200
Subject: [PATCH] [Payload] Fixed some issues and refined some settings
---
src/boards/Payload/CanHandler/CanHandler.cpp | 1 +
src/boards/Payload/Configs/CanHandlerConfig.h | 2 +-
src/boards/Payload/Configs/WingConfig.h | 15 ++++++++++++---
src/boards/Payload/Radio/Radio.cpp | 6 +-----
src/boards/Payload/Sensors/Sensors.cpp | 2 +-
.../FlightModeManager/FlightModeManager.cpp | 1 -
.../StateMachines/NASController/NASController.cpp | 3 ++-
src/boards/Payload/Wing/AltitudeTrigger.cpp | 13 +++++++++++--
src/boards/Payload/Wing/AltitudeTrigger.h | 4 ++++
9 files changed, 33 insertions(+), 14 deletions(-)
diff --git a/src/boards/Payload/CanHandler/CanHandler.cpp b/src/boards/Payload/CanHandler/CanHandler.cpp
index 1d3d165b1..feac32eb5 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 c5b384434..5e1917ff7 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 38ba5ea43..c9c0d1b7b 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 cba577c5d..7ab7a6a12 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 fb39a0105..3bb500f63 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 433cde9dc..89cbe1020 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 2ea33a82c..bd7b98a2f 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 c9a65d106..9900ea3f8 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 7e2ae9b30..bd14a3f3a 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
--
GitLab