From 97195aecece50a53832dd691a49e86896ba82f6d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Niccol=C3=B2=20Betto?= <niccolo.betto@skywarder.eu>
Date: Mon, 9 Sep 2024 23:04:37 +0200
Subject: [PATCH] [Payload][Radio] Implement SET_CALIBRATION_PRESSURE telemetry
message
---
src/boards/Payload/Radio/MessageHandler.cpp | 33 +++++++++++++++++++++
src/boards/Payload/Sensors/Sensors.cpp | 25 ++++++++++++++--
src/boards/Payload/Sensors/Sensors.h | 7 +++++
3 files changed, 63 insertions(+), 2 deletions(-)
diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index b58c86516..89eff4225 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -270,6 +270,39 @@ void Radio::MavlinkBackend::handleMessage(const mavlink_message_t& msg)
}
}
+ case MAVLINK_MSG_ID_SET_CALIBRATION_PRESSURE_TC:
+ {
+ if (parent.getModule<FlightModeManager>()->getState() !=
+ FlightModeManagerState::ON_GROUND_DISARMED)
+ {
+ return enqueueNack(msg);
+ }
+
+ float press =
+ mavlink_msg_set_calibration_pressure_tc_get_pressure(&msg);
+
+ if (press == 0)
+ {
+ parent.getModule<Sensors>()->resetBaroCalibrationReference();
+ EventBroker::getInstance().post(TMTC_CALIBRATE, TOPIC_TMTC);
+ enqueueAck(msg);
+ }
+ else
+ {
+ parent.getModule<Sensors>()->setBaroCalibrationReference(press);
+ EventBroker::getInstance().post(TMTC_CALIBRATE, TOPIC_TMTC);
+
+ if (press < 50000)
+ {
+ enqueueWack(msg);
+ }
+ else
+ {
+ enqueueAck(msg);
+ }
+ }
+ }
+
case MAVLINK_MSG_ID_RAW_EVENT_TC:
{
uint8_t topicId = mavlink_msg_raw_event_tc_get_topic_id(&msg);
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index ed768b7e5..a60bdb73d 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -161,8 +161,15 @@ void Sensors::calibrate()
dynamicPressureSum / config::Calibration::SAMPLE_COUNT;
float lps28dfwAvg = lps28dfwSum / config::Calibration::SAMPLE_COUNT;
- // Calibrate the analog static pressure sensor against the LPS28DFW
- float reference = lps28dfwAvg;
+ // Calibrate all analog pressure sensors against the LPS28DFW or the
+ // telemetry reference
+ float reference = 0;
+ {
+ miosix::Lock<miosix::FastMutex> lock{baroCalibrationMutex};
+ reference = useBaroCalibrationReference ? baroCalibrationReference
+ : lps28dfwAvg;
+ }
+
if (reference > config::Calibration::ATMOS_THRESHOLD)
{
// Apply the offset only if reference is valid
@@ -224,6 +231,20 @@ bool Sensors::saveMagCalibration()
}
}
+void Sensors::setBaroCalibrationReference(float reference)
+{
+ miosix::Lock<miosix::FastMutex> lock{baroCalibrationMutex};
+ baroCalibrationReference = reference;
+ useBaroCalibrationReference = true;
+}
+
+void Sensors::resetBaroCalibrationReference()
+{
+ miosix::Lock<miosix::FastMutex> lock{baroCalibrationMutex};
+ baroCalibrationReference = 0.0f;
+ useBaroCalibrationReference = false;
+}
+
LPS22DFData Sensors::getLPS22DFLastSample()
{
return lps22df ? lps22df->getLastSample() : LPS22DFData{};
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index 591547258..03e302365 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -90,6 +90,9 @@ public:
*/
bool saveMagCalibration();
+ void setBaroCalibrationReference(float reference);
+ void resetBaroCalibrationReference();
+
Boardcore::LPS22DFData getLPS22DFLastSample();
Boardcore::LPS28DFWData getLPS28DFWLastSample();
Boardcore::H3LIS331DLData getH3LIS331DLLastSample();
@@ -185,6 +188,10 @@ private:
bool sensorManagerInit();
+ miosix::FastMutex baroCalibrationMutex;
+ float baroCalibrationReference = 0;
+ bool useBaroCalibrationReference = false;
+
miosix::FastMutex magCalibrationMutex;
Boardcore::SoftAndHardIronCalibration magCalibrator;
Boardcore::SixParametersCorrector magCalibration;
--
GitLab