diff --git a/src/boards/Parafoil/Radio/Radio.cpp b/src/boards/Parafoil/Radio/Radio.cpp
index 9d6bf189ca62987ec285cf58e9da4ef705689edd..f545c66fe7d7f8dd608c02bc45851edc1d1cee79 100644
--- a/src/boards/Parafoil/Radio/Radio.cpp
+++ b/src/boards/Parafoil/Radio/Radio.cpp
@@ -119,7 +119,7 @@ void Radio::sendNack(const mavlink_message_t& msg)
mavlink_message_t nackMsg;
mavlink_msg_nack_tm_pack(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMP_ID, &nackMsg, msg.msgid,
- msg.seq, 0);
+ msg.seq);
enqueueMsg(nackMsg);
}
@@ -237,13 +237,14 @@ void Radio::handleMavlinkMessage(const mavlink_message_t& msg)
for (SensorInfo i : sensorsState)
{
strcpy(tm.sensor_name, i.id.c_str());
+ tm.state = 0;
if (i.isEnabled)
{
- tm.enabled = 1;
+ tm.state += 1;
}
if (i.isInitialized)
{
- tm.initialized = 1;
+ tm.state += 2;
}
mavlink_msg_sensor_state_tm_encode(
RadioConfig::MAV_SYSTEM_ID, RadioConfig::MAV_COMP_ID,
@@ -492,7 +493,7 @@ void Radio::handleCommand(const mavlink_message_t& msg)
{MAV_CMD_FORCE_INIT, TMTC_FORCE_INIT},
{MAV_CMD_FORCE_LAUNCH, TMTC_FORCE_LAUNCH},
{MAV_CMD_FORCE_LANDING, TMTC_FORCE_LANDING},
- // {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
+ {MAV_CMD_FORCE_APOGEE, TMTC_FORCE_APOGEE},
{MAV_CMD_FORCE_EXPULSION, TMTC_FORCE_EXPULSION},
{MAV_CMD_FORCE_DEPLOYMENT, TMTC_FORCE_DEPLOYMENT},
{MAV_CMD_START_LOGGING, TMTC_START_LOGGING},
diff --git a/src/boards/Parafoil/TMRepository/TMRepository.cpp b/src/boards/Parafoil/TMRepository/TMRepository.cpp
index 2a7b4aaa4941f677764e518b19ad9921dbc8ea22..78787bcdbcdb5361c9fca1065eb9a8baad5c5d67 100644
--- a/src/boards/Parafoil/TMRepository/TMRepository.cpp
+++ b/src/boards/Parafoil/TMRepository/TMRepository.cpp
@@ -54,13 +54,13 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
{
mavlink_sys_tm_t tm;
- tm.timestamp = TimestampTimer::getTimestamp();
- tm.logger = Logger::getInstance().isStarted();
- // tm.board_scheduler = modules.get<BoardScheduler>()->isStarted();
- tm.event_broker = EventBroker::getInstance().isRunning();
- tm.radio = modules.get<Radio>()->isStarted();
- tm.sensors = modules.get<Sensors>()->isStarted();
- tm.pin_handler = modules.get<PinHandler>()->isStarted();
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.logger = Logger::getInstance().isStarted();
+ tm.board_scheduler = modules.get<BoardScheduler>()->isStarted();
+ tm.event_broker = EventBroker::getInstance().isRunning();
+ tm.radio = modules.get<Radio>()->isStarted();
+ tm.sensors = modules.get<Sensors>()->isStarted();
+ tm.pin_observer = modules.get<PinHandler>()->isStarted();
mavlink_msg_sys_tm_encode(RadioConfig::MAV_SYSTEM_ID,
RadioConfig::MAV_COMP_ID, &msg, &tm);
@@ -91,34 +91,32 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
break;
}
- // case SystemTMList::MAV_MAVLINK_STATS:
- // {
- // mavlink_mavlink_stats_tm_t tm;
-
- // // Get the mavlink stats
- // MavlinkStatus stats =
- // modules.get<Radio>()->mavDriver->getStatus();
-
- // tm.timestamp = stats.timestamp;
- // tm.n_send_queue = stats.nSendQueue;
- // tm.max_send_queue = stats.maxSendQueue;
- // tm.n_send_errors = stats.nSendErrors;
- // tm.msg_received = stats.mavStats.msg_received;
- // tm.buffer_overrun = stats.mavStats.buffer_overrun;
- // tm.parse_error = stats.mavStats.parse_error;
- // tm.parse_state = stats.mavStats.parse_state;
- // tm.packet_idx = stats.mavStats.packet_idx;
- // tm.current_rx_seq = stats.mavStats.current_rx_seq;
- // tm.current_tx_seq = stats.mavStats.current_tx_seq;
- // tm.packet_rx_success_count =
- // stats.mavStats.packet_rx_success_count; tm.packet_rx_drop_count
- // = stats.mavStats.packet_rx_drop_count;
-
- // mavlink_msg_mavlink_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- // RadioConfig::MAV_COMP_ID,
- // &msg, &tm);
- // break;
- // }
+ case SystemTMList::MAV_MAVLINK_STATS:
+ {
+ mavlink_mavlink_stats_tm_t tm;
+
+ // Get the mavlink stats
+ MavlinkStatus stats = modules.get<Radio>()->mavDriver->getStatus();
+
+ tm.timestamp = stats.timestamp;
+ tm.n_send_queue = stats.nSendQueue;
+ tm.max_send_queue = stats.maxSendQueue;
+ tm.n_send_errors = stats.nSendErrors;
+ tm.msg_received = stats.mavStats.msg_received;
+ tm.buffer_overrun = stats.mavStats.buffer_overrun;
+ tm.parse_error = stats.mavStats.parse_error;
+ tm.parse_state = stats.mavStats.parse_state;
+ tm.packet_idx = stats.mavStats.packet_idx;
+ tm.current_rx_seq = stats.mavStats.current_rx_seq;
+ tm.current_tx_seq = stats.mavStats.current_tx_seq;
+ tm.packet_rx_success_count = stats.mavStats.packet_rx_success_count;
+ tm.packet_rx_drop_count = stats.mavStats.packet_rx_drop_count;
+
+ mavlink_msg_mavlink_stats_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg,
+ &tm);
+ break;
+ }
case SystemTMList::MAV_NAS_ID:
{
mavlink_nas_tm_t tm;
@@ -235,9 +233,8 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
.batVoltage;
// tm.current_consumption =
// modules.get<Sensors>()->getCurrentLastSample().current;
- tm.temperature = lps22df.temperature;
- tm.cutter_presence =
- Logger::getInstance().getStats().lastWriteError;
+ tm.temperature = lps22df.temperature;
+ tm.logger_error = Logger::getInstance().getStats().lastWriteError;
tm.battery_voltage = modules.get<Sensors>()
->getBatteryVoltageLastSample()
@@ -261,26 +258,26 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
&tm);
break;
}
- // case SystemTMList::MAV_FSM_ID:
- // {
- // mavlink_fsm_tm_t tm;
-
- // tm.timestamp = TimestampTimer::getTimestamp();
- // tm.abk_state = 0;
- // tm.ada_state = 0;
- // tm.dpl_state = static_cast<uint8_t>(
- // modules.get<WingController>()->getStatus().state);
- // tm.fmm_state = static_cast<uint8_t>(
- // modules.get<FlightModeManager>()->getStatus().state);
- // tm.nas_state = static_cast<uint8_t>(
- // modules.get<NASController>()->getStatus().state);
- // tm.wes_state = 0;
-
- // mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID,
- // RadioConfig::MAV_COMP_ID, &msg, &tm);
-
- // break;
- // }
+ case SystemTMList::MAV_FSM_ID:
+ {
+ mavlink_fsm_tm_t tm;
+
+ tm.timestamp = TimestampTimer::getTimestamp();
+ tm.abk_state = 0;
+ tm.ada_state = 0;
+ tm.dpl_state = static_cast<uint8_t>(
+ modules.get<WingController>()->getStatus().state);
+ tm.fmm_state = static_cast<uint8_t>(
+ modules.get<FlightModeManager>()->getStatus().state);
+ tm.nas_state = static_cast<uint8_t>(
+ modules.get<NASController>()->getStatus().state);
+ tm.wes_state = 0;
+
+ mavlink_msg_fsm_tm_encode(RadioConfig::MAV_SYSTEM_ID,
+ RadioConfig::MAV_COMP_ID, &msg, &tm);
+
+ break;
+ }
case SystemTMList::MAV_PIN_OBS_ID:
{
mavlink_pin_tm_t tm;
diff --git a/src/boards/common/Mavlink.h b/src/boards/common/Mavlink.h
index 5daa9bb827ced42f7735a3a6de7647ade664c23d..97ea89d8fa0fcfcf1cc13b0aec56956f283a4d4b 100644
--- a/src/boards/common/Mavlink.h
+++ b/src/boards/common/Mavlink.h
@@ -25,5 +25,5 @@
#pragma GCC diagnostic ignored "-Wcast-align"
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
-#include <mavlink_lib/lyra/mavlink.h>
+#include <mavlink_lib/gemini/mavlink.h>
#pragma GCC diagnostic pop
\ No newline at end of file