From 5d78ffebf6f72500d7536d70397bbb6decae7ae8 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Wed, 28 Feb 2024 15:57:03 +0100
Subject: [PATCH] [RIGv2][Radio][Sensors] Added sensor info request TM
---
src/boards/RIGv2/Radio/Radio.cpp | 22 ++++++++++++++++++++--
src/boards/RIGv2/Sensors/Sensors.cpp | 18 +++++++++++++++---
src/boards/RIGv2/Sensors/Sensors.h | 4 ++++
3 files changed, 39 insertions(+), 5 deletions(-)
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index 7bb15b073..2041adfea 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -181,7 +181,26 @@ void Radio::handleMessage(const mavlink_message_t& msg)
uint8_t tmId = mavlink_msg_system_tm_request_tc_get_tm_id(&msg);
mavlink_message_t tm;
- if (packSystemTm(tmId, tm))
+ if (tmId == MAV_SENSORS_STATE_ID)
+ {
+ sendAck(msg);
+
+ auto sensors = modules.get<Sensors>()->getSensorInfos();
+ for (auto sensor : sensors)
+ {
+ mavlink_sensor_state_tm_t tm2;
+
+ strcpy(tm2.sensor_name, sensor.id.c_str());
+ tm2.state = (sensor.isInitialized ? 1 : 0) |
+ (sensor.isEnabled ? 2 : 0);
+
+ mavlink_msg_sensor_state_tm_encode(
+ Config::Radio::MAV_SYSTEM_ID,
+ Config::Radio::MAV_COMPONENT_ID, &tm, &tm2);
+ enqueuePacket(tm);
+ }
+ }
+ else if (packSystemTm(tmId, tm))
{
sendAck(msg);
enqueuePacket(tm);
@@ -274,7 +293,6 @@ void Radio::handleMessage(const mavlink_message_t& msg)
void Radio::handleCommand(const mavlink_message_t& msg)
{
- ModuleManager& modules = ModuleManager::getInstance();
uint8_t cmd = mavlink_msg_command_tc_get_command_id(&msg);
switch (cmd)
{
diff --git a/src/boards/RIGv2/Sensors/Sensors.cpp b/src/boards/RIGv2/Sensors/Sensors.cpp
index 9805efde8..85d607cbf 100644
--- a/src/boards/RIGv2/Sensors/Sensors.cpp
+++ b/src/boards/RIGv2/Sensors/Sensors.cpp
@@ -162,19 +162,22 @@ LoadCellData Sensors::getTankWeight()
return {sample.timestamp, load};
}
-CurrentData Sensors::getUmbilicalCurrent() {
+CurrentData Sensors::getUmbilicalCurrent()
+{
auto sample = getInternalADCLastSample();
return {sample.timestamp, sample.voltage[11]};
}
-CurrentData Sensors::getServoCurrent() {
+CurrentData Sensors::getServoCurrent()
+{
auto sample = getInternalADCLastSample();
return {sample.timestamp, sample.voltage[9]};
}
-VoltageData Sensors::getBatteryVoltage() {
+VoltageData Sensors::getBatteryVoltage()
+{
auto sample = getInternalADCLastSample();
return {sample.timestamp, sample.voltage[14]};
@@ -200,6 +203,15 @@ void Sensors::calibrate()
tankLcOffset = tankStats.getStats().mean;
}
+std::vector<SensorInfo> Sensors::getSensorInfos()
+{
+ return {
+ manager->getSensorInfo(internalAdc.get()),
+ manager->getSensorInfo(adc1.get()),
+ manager->getSensorInfo(tc1.get()),
+ };
+}
+
void Sensors::internalAdcInit(Boardcore::SensorManager::SensorMap_t &map)
{
internalAdc = std::make_unique<InternalADC>(ADC1);
diff --git a/src/boards/RIGv2/Sensors/Sensors.h b/src/boards/RIGv2/Sensors/Sensors.h
index 43770888a..5dfa2c79e 100644
--- a/src/boards/RIGv2/Sensors/Sensors.h
+++ b/src/boards/RIGv2/Sensors/Sensors.h
@@ -29,7 +29,9 @@
#include <sensors/SensorManager.h>
#include <atomic>
+#include <functional>
#include <memory>
+#include <vector>
#include <utils/ModuleManager/ModuleManager.hpp>
namespace RIGv2
@@ -64,6 +66,8 @@ public:
void calibrate();
+ std::vector<Boardcore::SensorInfo> getSensorInfos();
+
private:
void internalAdcInit(Boardcore::SensorManager::SensorMap_t &map);
void internalAdcCallback();
--
GitLab