From 2a57e6c20ddf68bf926c6514825ef2d14bf8929a 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:05:50 +0200
Subject: [PATCH] [Payload][Radio] Implement CALIBRATION_TM system TM
---
src/boards/Payload/Radio/MessageHandler.cpp | 33 ++++++++++++++++++
src/boards/Payload/Sensors/SensorData.h | 29 ++++++++++++----
src/boards/Payload/Sensors/Sensors.cpp | 34 +++++++++++++++----
src/boards/Payload/Sensors/Sensors.h | 7 ++++
src/scripts/logdecoder/Payload/logdecoder.cpp | 2 +-
5 files changed, 90 insertions(+), 15 deletions(-)
diff --git a/src/boards/Payload/Radio/MessageHandler.cpp b/src/boards/Payload/Radio/MessageHandler.cpp
index 89eff4225..d56b14445 100644
--- a/src/boards/Payload/Radio/MessageHandler.cpp
+++ b/src/boards/Payload/Radio/MessageHandler.cpp
@@ -815,6 +815,39 @@ bool Radio::MavlinkBackend::enqueueSystemTm(SystemTMList tmId)
return true;
}
+ case MAV_CALIBRATION_ID:
+ {
+ mavlink_message_t msg;
+ mavlink_calibration_tm_t tm;
+
+ auto cal = parent.getModule<Sensors>()->getCalibrationData();
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.gyro_bias_x = cal.gyroBiasX;
+ tm.gyro_bias_y = cal.gyroBiasY;
+ tm.gyro_bias_z = cal.gyroBiasZ;
+ tm.mag_bias_x = cal.magBiasX;
+ tm.mag_bias_y = cal.magBiasY;
+ tm.mag_bias_z = cal.magBiasZ;
+ tm.mag_scale_x = cal.magScaleX;
+ tm.mag_scale_y = cal.magScaleY;
+ tm.mag_scale_z = cal.magScaleZ;
+ tm.static_press_1_bias = cal.staticPressBias;
+ tm.static_press_1_scale = cal.staticPressScale;
+ tm.static_press_2_bias = 0.0f;
+ tm.static_press_2_scale = 0.0f;
+ tm.dpl_bay_press_bias = 0.0f;
+ tm.dpl_bay_press_scale = 0.0f;
+ tm.dynamic_press_bias = cal.dynamicPressBias;
+ tm.dynamic_press_scale = cal.dynamicPressScale;
+
+ mavlink_msg_calibration_tm_encode(config::Mavlink::SYSTEM_ID,
+ config::Mavlink::COMPONENT_ID,
+ &msg, &tm);
+ enqueueMessage(msg);
+ return true;
+ }
+
default:
return false;
}
diff --git a/src/boards/Payload/Sensors/SensorData.h b/src/boards/Payload/Sensors/SensorData.h
index d198b315d..91cfe924a 100644
--- a/src/boards/Payload/Sensors/SensorData.h
+++ b/src/boards/Payload/Sensors/SensorData.h
@@ -50,22 +50,37 @@ struct DynamicPressureData : public Boardcore::PressureData
}
};
-struct SensorsCalibrationParameter
+struct SensorCalibrationData
{
uint64_t timestamp = 0;
- float referencePressure = 0;
- float offsetStatic = 0;
- float offsetDynamic = 0;
+ float gyroBiasX = 0.0f;
+ float gyroBiasY = 0.0f;
+ float gyroBiasZ = 0.0f;
+ float magBiasX = 0.0f;
+ float magBiasY = 0.0f;
+ float magBiasZ = 0.0f;
+ float magScaleX = 0.0f;
+ float magScaleY = 0.0f;
+ float magScaleZ = 0.0f;
+ float staticPressBias = 0.0f;
+ float staticPressScale = 0.0f;
+ float dynamicPressBias = 0.0f;
+ float dynamicPressScale = 0.0f;
static std::string header()
{
- return "timestamp,referencePressure,offsetStatic,offsetDynamic\n";
+ return "timestamp,gyroBiasX,gyroBiasY,gyroBiasZ,magBiasX,magBiasY,"
+ "magBiasZ,magScaleX,magScaleY,magScaleZ,staticPressBias,"
+ "staticPressScale,dynamicPressBias,dynamicPressScale\n";
}
void print(std::ostream& os) const
{
- os << timestamp << "," << referencePressure << "," << offsetStatic
- << "," << offsetDynamic << "\n";
+ os << timestamp << "," << gyroBiasX << "," << gyroBiasY << ","
+ << gyroBiasZ << "," << magBiasX << "," << magBiasY << "," << magBiasZ
+ << "," << magScaleX << "," << magScaleY << "," << magScaleZ << ","
+ << staticPressBias << "," << staticPressScale << ","
+ << dynamicPressBias << "," << dynamicPressScale << "\n";
}
};
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index a60bdb73d..49c4ab50c 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -178,17 +178,37 @@ void Sensors::calibrate()
}
dynamicPressure->updateOffset(dynamicPressureMean);
- miosix::Lock<miosix::FastMutex> lock{gyroCalibrationMutex};
- gyroCalibration = gyroCalibrator.computeResult();
+ {
+ miosix::Lock<miosix::FastMutex> lock{gyroCalibrationMutex};
+ gyroCalibration = gyroCalibrator.computeResult();
+ }
// Log the offsets
- auto cal = SensorsCalibrationParameter{
+ auto data = getCalibrationData();
+ Logger::getInstance().log(data);
+}
+
+SensorCalibrationData Sensors::getCalibrationData()
+{
+ miosix::Lock<miosix::FastMutex> magLock{magCalibrationMutex};
+ miosix::Lock<miosix::FastMutex> gyroLock{gyroCalibrationMutex};
+
+ return SensorCalibrationData{
.timestamp = TimestampTimer::getTimestamp(),
- .referencePressure = reference,
- .offsetStatic = staticPressure->getOffset(),
- .offsetDynamic = dynamicPressure->getOffset(),
+ .gyroBiasX = gyroCalibration.getb().x(),
+ .gyroBiasY = gyroCalibration.getb().y(),
+ .gyroBiasZ = gyroCalibration.getb().z(),
+ .magBiasX = magCalibration.getb().x(),
+ .magBiasY = magCalibration.getb().y(),
+ .magBiasZ = magCalibration.getb().z(),
+ .magScaleX = magCalibration.getA().x(),
+ .magScaleY = magCalibration.getA().y(),
+ .magScaleZ = magCalibration.getA().z(),
+ .staticPressBias = staticPressure->getOffset(),
+ .staticPressScale = 1.0f,
+ .dynamicPressBias = dynamicPressure->getOffset(),
+ .dynamicPressScale = 1.0f,
};
- Logger::getInstance().log(cal);
}
void Sensors::resetMagCalibrator()
diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h
index 03e302365..d69a847eb 100644
--- a/src/boards/Payload/Sensors/Sensors.h
+++ b/src/boards/Payload/Sensors/Sensors.h
@@ -67,6 +67,13 @@ public:
*/
void calibrate();
+ /**
+ * @brief Returns the current sensors calibration parameters.
+ *
+ * @internal Ensure mutexes are unlocked before calling this function.
+ */
+ SensorCalibrationData getCalibrationData();
+
/**
* @brief Resets the magnetometer calibration.
*/
diff --git a/src/scripts/logdecoder/Payload/logdecoder.cpp b/src/scripts/logdecoder/Payload/logdecoder.cpp
index 1919157dd..2be72b49c 100644
--- a/src/scripts/logdecoder/Payload/logdecoder.cpp
+++ b/src/scripts/logdecoder/Payload/logdecoder.cpp
@@ -66,7 +66,7 @@ void registerTypes(Deserializer& ds)
ds.registerType<WingControllerStatus>();
ds.registerType<StaticPressureData>();
ds.registerType<DynamicPressureData>();
- ds.registerType<SensorsCalibrationParameter>();
+ ds.registerType<SensorCalibrationData>();
ds.registerType<PinChangeData>();
ds.registerType<WingControllerAlgorithmData>();
ds.registerType<WingAlgorithmData>();
--
GitLab