diff --git a/src/boards/Groundstation/Base/Radio/Radio.h b/src/boards/Groundstation/Base/Radio/Radio.h
index e09d207c9166a97403167b204dc25681b059cc8e..d927021c3fbaa1fa715a1b5858cd553d4d5c53a8 100644
--- a/src/boards/Groundstation/Base/Radio/Radio.h
+++ b/src/boards/Groundstation/Base/Radio/Radio.h
@@ -30,12 +30,16 @@ namespace GroundstationBase
class RadioMain : public Groundstation::RadioBase, public Boardcore::Module
{
public:
+ RadioMain(): Groundstation::RadioBase(Groundstation::MAV_PING_MSG_ID_ROCKET) {};
+
[[nodiscard]] bool start();
};
class RadioPayload : public Groundstation::RadioBase, public Boardcore::Module
{
public:
+ RadioPayload(): Groundstation::RadioBase(Groundstation::MAV_PING_MSG_ID_PAYLOAD) {};
+
[[nodiscard]] bool start();
};
diff --git a/src/boards/Groundstation/Common/Config/RadioConfig.h b/src/boards/Groundstation/Common/Config/RadioConfig.h
index c32abe0d514e7145a4f19e319d123f450ba7b25d..193c9c85fe2661e98ff8ddd2403d6713329056e8 100644
--- a/src/boards/Groundstation/Common/Config/RadioConfig.h
+++ b/src/boards/Groundstation/Common/Config/RadioConfig.h
@@ -22,6 +22,8 @@
#pragma once
+#include <common/Mavlink.h>
+
#include <cstddef>
#include <cstdint>
@@ -33,15 +35,12 @@
namespace Groundstation
{
-constexpr size_t MAV_OUT_QUEUE_SIZE = 10;
+// Mavlink driver parameters
+constexpr uint16_t MAV_SLEEP_AFTER_SEND = 10;
constexpr size_t MAV_PENDING_OUT_QUEUE_SIZE = 10;
-constexpr uint16_t MAV_SLEEP_AFTER_SEND = 5;
-constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10;
-
-/// @brief Every how many ms force the flush of the send queue.
-constexpr unsigned int AUTOMATIC_FLUSH_PERIOD = 250;
-/// @brief After how many ms stop waiting for the other side to send commands.
-constexpr long long AUTOMATIC_FLUSH_DELAY = 2000;
+constexpr uint8_t MAV_PING_MSG_ID_ROCKET = MAVLINK_MSG_ID_ROCKET_FLIGHT_TM;
+constexpr uint8_t MAV_PING_MSG_ID_PAYLOAD = MAVLINK_MSG_ID_PAYLOAD_FLIGHT_TM;
+constexpr long long AUTOMATIC_FLUSH_DELAY = 2000;
/// @brief Period of the radio status telemetry.
constexpr unsigned int RADIO_STATUS_PERIOD = 250;
diff --git a/src/boards/Groundstation/Common/Ports/EthernetBase.cpp b/src/boards/Groundstation/Common/Ports/EthernetBase.cpp
index f6c627b234ded251bbd0d6f44bc914e269c69063..c18fd69c2a3464f437875bc62c8afa96462b06d8 100644
--- a/src/boards/Groundstation/Common/Ports/EthernetBase.cpp
+++ b/src/boards/Groundstation/Common/Ports/EthernetBase.cpp
@@ -58,9 +58,9 @@ void EthernetBase::handleINTn()
void EthernetBase::sendMsg(const mavlink_message_t& msg)
{
- if (mav_driver && mav_driver->isStarted())
+ if (mavDriver && mavDriver->isStarted())
{
- mav_driver->enqueueMsg(msg);
+ mavDriver->enqueueMsg(msg);
}
}
@@ -95,9 +95,9 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
auto mav_handler = [this](EthernetMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); };
- mav_driver = std::make_unique<EthernetMavDriver>(this, mav_handler, 0, 10);
+ mavDriver = std::make_unique<EthernetMavDriver>(this, mav_handler, 0, 0);
- if (!mav_driver->start())
+ if (!mavDriver->start())
{
return false;
}
diff --git a/src/boards/Groundstation/Common/Ports/EthernetBase.h b/src/boards/Groundstation/Common/Ports/EthernetBase.h
index cd377b5744d86893fbaf592e9bf652cf8440fd34..985e528efcf7b3f812a9cb86156236af5bc02ccf 100644
--- a/src/boards/Groundstation/Common/Ports/EthernetBase.h
+++ b/src/boards/Groundstation/Common/Ports/EthernetBase.h
@@ -25,7 +25,7 @@
#include <ActiveObject.h>
#include <common/Mavlink.h>
#include <drivers/WIZ5500/WIZ5500.h>
-#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/MavlinkDriver/MavlinkDriverV0.h>
#include <memory>
@@ -36,7 +36,7 @@ Boardcore::WizIp genNewRandomIp();
Boardcore::WizMac genNewRandomMac();
using EthernetMavDriver =
- Boardcore::MavlinkDriver<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
+ Boardcore::MavlinkDriverV0<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
class EthernetBase : public Boardcore::Transceiver
{
@@ -64,7 +64,7 @@ private:
bool started = false;
std::unique_ptr<Boardcore::Wiz5500> wiz5500;
- std::unique_ptr<EthernetMavDriver> mav_driver;
+ std::unique_ptr<EthernetMavDriver> mavDriver;
};
} // namespace Groundstation
\ No newline at end of file
diff --git a/src/boards/Groundstation/Common/Ports/Serial.cpp b/src/boards/Groundstation/Common/Ports/Serial.cpp
index 45f4b7712016451e394c9ce4284e5bcb8c6fb4ce..8fbbf677b0376b54bb1d32763f49f8c0a1cb254c 100644
--- a/src/boards/Groundstation/Common/Ports/Serial.cpp
+++ b/src/boards/Groundstation/Common/Ports/Serial.cpp
@@ -34,9 +34,9 @@ bool Serial::start()
auto mav_handler = [this](SerialMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); };
- mav_driver = std::make_unique<SerialMavDriver>(this, mav_handler, 0, 10);
+ mavDriver = std::make_unique<SerialMavDriver>(this, mav_handler, 0, 0);
- if (!mav_driver->start())
+ if (!mavDriver->start())
{
return false;
}
@@ -46,9 +46,9 @@ bool Serial::start()
void Serial::sendMsg(const mavlink_message_t& msg)
{
- if (mav_driver && mav_driver->isStarted())
+ if (mavDriver && mavDriver->isStarted())
{
- mav_driver->enqueueMsg(msg);
+ mavDriver->enqueueMsg(msg);
}
}
diff --git a/src/boards/Groundstation/Common/Ports/Serial.h b/src/boards/Groundstation/Common/Ports/Serial.h
index ea39b639428dd17b623c6e0899ac2cd6c5e7e630..3c4417ddf52fd9dcd870e13e12f60579cd8e180d 100644
--- a/src/boards/Groundstation/Common/Ports/Serial.h
+++ b/src/boards/Groundstation/Common/Ports/Serial.h
@@ -24,7 +24,7 @@
#include <ActiveObject.h>
#include <common/Mavlink.h>
-#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/MavlinkDriver/MavlinkDriverV0.h>
#include <memory>
#include <utils/ModuleManager/ModuleManager.hpp>
@@ -33,7 +33,7 @@ namespace Groundstation
{
using SerialMavDriver =
- Boardcore::MavlinkDriver<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
+ Boardcore::MavlinkDriverV0<1024, 10, MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
/**
* @brief Class responsible for UART communication.
@@ -64,7 +64,7 @@ private:
bool send(uint8_t* pkt, size_t len) override;
miosix::FastMutex mutex;
- std::unique_ptr<SerialMavDriver> mav_driver;
+ std::unique_ptr<SerialMavDriver> mavDriver;
};
} // namespace Groundstation
\ No newline at end of file
diff --git a/src/boards/Groundstation/Common/Radio/RadioBase.cpp b/src/boards/Groundstation/Common/Radio/RadioBase.cpp
index 22be5ff3b345e82f6ab3be4e18705317eec2b7c1..94cd87714748b3b606e63d23205c5a58135a1e7a 100644
--- a/src/boards/Groundstation/Common/Radio/RadioBase.cpp
+++ b/src/boards/Groundstation/Common/Radio/RadioBase.cpp
@@ -32,18 +32,7 @@ using namespace Boardcore;
bool RadioBase::sendMsg(const mavlink_message_t& msg)
{
- Lock<FastMutex> l(pending_msgs_mutex);
- if (pending_msgs_count >= MAV_PENDING_OUT_QUEUE_SIZE)
- {
- return false;
- }
- else
- {
- pending_msgs[pending_msgs_count] = msg;
- pending_msgs_count += 1;
-
- return true;
- }
+ return mavDriver->enqueueMsg(msg);
}
void RadioBase::handleDioIRQ()
@@ -58,7 +47,7 @@ RadioStats RadioBase::getStats()
{
if (started)
{
- auto mav_stats = mav_driver->getStatus();
+ auto mav_stats = mavDriver->getStatus();
return {.send_errors = mav_stats.nSendErrors,
.packet_rx_success_count =
@@ -79,14 +68,19 @@ bool RadioBase::start(std::unique_ptr<SX1278Fsk> sx1278)
{
this->sx1278 = std::move(sx1278);
- auto mav_handler = [this](RadioMavDriver* channel,
- const mavlink_message_t& msg) { handleMsg(msg); };
+ auto mav_handler =
+ [&](RadioMavDriver* channel, const mavlink_message_t& msg)
+ {
+ // Dispatch the message through the hub.
+ ModuleManager::getInstance().get<HubBase>()->dispatchIncomingMsg(msg);
+ };
- mav_driver = std::make_unique<RadioMavDriver>(
- this, mav_handler, Groundstation::MAV_SLEEP_AFTER_SEND,
- Groundstation::MAV_OUT_BUFFER_MAX_AGE);
+ mavDriver = std::make_unique<RadioMavDriver>(
+ this, pingMsgId, mav_handler,
+ Groundstation::MAV_SLEEP_AFTER_SEND,
+ Groundstation::AUTOMATIC_FLUSH_DELAY);
- if (!mav_driver->start())
+ if (!mavDriver->start())
{
return false;
}
@@ -105,13 +99,7 @@ void RadioBase::run()
while (!shouldStop())
{
- miosix::Thread::sleep(AUTOMATIC_FLUSH_PERIOD);
-
- // If enough time has passed, automatically flush.
- if (miosix::getTick() > last_eot_packet_ts + AUTOMATIC_FLUSH_DELAY)
- {
- flush();
- }
+ miosix::Thread::sleep(10);
}
}
@@ -136,32 +124,3 @@ bool RadioBase::send(uint8_t* pkt, size_t len)
return ret;
}
-
-void RadioBase::handleMsg(const mavlink_message_t& msg)
-{
- // Dispatch the message through the hub.
- ModuleManager::getInstance().get<HubBase>()->dispatchIncomingMsg(msg);
-
- if (isEndOfTransmissionPacket(msg))
- {
- last_eot_packet_ts = miosix::getTick();
- flush();
- }
-}
-
-void RadioBase::flush()
-{
- Lock<FastMutex> l(pending_msgs_mutex);
- for (size_t i = 0; i < pending_msgs_count; i++)
- {
- mav_driver->enqueueMsg(pending_msgs[i]);
- }
-
- pending_msgs_count = 0;
-}
-
-bool RadioBase::isEndOfTransmissionPacket(const mavlink_message_t& msg)
-{
- return msg.msgid == MAVLINK_MSG_ID_ROCKET_FLIGHT_TM ||
- msg.msgid == MAVLINK_MSG_ID_PAYLOAD_FLIGHT_TM;
-}
\ No newline at end of file
diff --git a/src/boards/Groundstation/Common/Radio/RadioBase.h b/src/boards/Groundstation/Common/Radio/RadioBase.h
index 3067912106fb345926e74fc8c63039ae43592115..72e04f6386f6d3ef5ab174daa91d951ebab1613b 100644
--- a/src/boards/Groundstation/Common/Radio/RadioBase.h
+++ b/src/boards/Groundstation/Common/Radio/RadioBase.h
@@ -26,7 +26,7 @@
#include <Groundstation/Common/Config/RadioConfig.h>
#include <common/Mavlink.h>
#include <common/Radio.h>
-#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/MavlinkDriver/MavlinkDriverPigna.h>
#include <radio/SX1278/SX1278Fsk.h>
#include <memory>
@@ -35,10 +35,9 @@
namespace Groundstation
{
-using RadioMavDriver =
- Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU,
- Groundstation::MAV_OUT_QUEUE_SIZE,
- MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
+using RadioMavDriver = Boardcore::MavlinkDriverPignaSlave<
+ Boardcore::SX1278Fsk::MTU, Groundstation::MAV_PENDING_OUT_QUEUE_SIZE,
+ MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
/**
* @brief Statistics of the radio.
@@ -61,7 +60,7 @@ struct RadioStats
class RadioBase : private Boardcore::ActiveObject, public Boardcore::Transceiver
{
public:
- RadioBase() {}
+ RadioBase(uint8_t pingMsgId): pingMsgId(pingMsgId) {}
/**
* @brief Send a mavlink message through this radio.
@@ -93,36 +92,18 @@ private:
bool send(uint8_t* pkt, size_t len) override;
- /**
- * @brief Called internally when a message is received.
- */
- void handleMsg(const mavlink_message_t& msg);
-
- /**
- * @brief Flush all pending messages.
- */
- void flush();
-
- /**
- * @brief Check if a message signals an end of transmission
- */
- bool isEndOfTransmissionPacket(const mavlink_message_t& msg);
-
bool started = false;
- miosix::FastMutex pending_msgs_mutex;
- mavlink_message_t pending_msgs[Groundstation::MAV_PENDING_OUT_QUEUE_SIZE];
- size_t pending_msgs_count = 0;
-
- long long last_eot_packet_ts = 0;
-
uint32_t bits_rx_count = 0;
uint32_t bits_tx_count = 0;
+ // Message ID of the periodic message
+ uint8_t pingMsgId;
+
// Objects are always destructed in reverse order, so keep them in this
// order
std::unique_ptr<Boardcore::SX1278Fsk> sx1278;
- std::unique_ptr<RadioMavDriver> mav_driver;
+ std::unique_ptr<RadioMavDriver> mavDriver;
};
} // namespace Groundstation
\ No newline at end of file