Skip to content
Snippets Groups Projects
Verified Commit e14a41ad authored by Giacomo Caironi's avatar Giacomo Caironi
Browse files

[Mavlink] MavlinkPigna POC for Groundstation

parent 2f364f4a
Branches mavlink-pigna
No related tags found
No related merge requests found
...@@ -30,12 +30,16 @@ namespace GroundstationBase ...@@ -30,12 +30,16 @@ namespace GroundstationBase
class RadioMain : public Groundstation::RadioBase, public Boardcore::Module class RadioMain : public Groundstation::RadioBase, public Boardcore::Module
{ {
public: public:
RadioMain(): Groundstation::RadioBase(Groundstation::MAV_PING_MSG_ID_ROCKET) {};
[[nodiscard]] bool start(); [[nodiscard]] bool start();
}; };
class RadioPayload : public Groundstation::RadioBase, public Boardcore::Module class RadioPayload : public Groundstation::RadioBase, public Boardcore::Module
{ {
public: public:
RadioPayload(): Groundstation::RadioBase(Groundstation::MAV_PING_MSG_ID_PAYLOAD) {};
[[nodiscard]] bool start(); [[nodiscard]] bool start();
}; };
......
...@@ -22,6 +22,8 @@ ...@@ -22,6 +22,8 @@
#pragma once #pragma once
#include <common/Mavlink.h>
#include <cstddef> #include <cstddef>
#include <cstdint> #include <cstdint>
...@@ -33,14 +35,11 @@ ...@@ -33,14 +35,11 @@
namespace Groundstation 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 size_t MAV_PENDING_OUT_QUEUE_SIZE = 10;
constexpr uint16_t MAV_SLEEP_AFTER_SEND = 5; constexpr uint8_t MAV_PING_MSG_ID_ROCKET = MAVLINK_MSG_ID_ROCKET_FLIGHT_TM;
constexpr size_t MAV_OUT_BUFFER_MAX_AGE = 10; constexpr uint8_t MAV_PING_MSG_ID_PAYLOAD = MAVLINK_MSG_ID_PAYLOAD_FLIGHT_TM;
/// @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 long long AUTOMATIC_FLUSH_DELAY = 2000;
/// @brief Period of the radio status telemetry. /// @brief Period of the radio status telemetry.
......
...@@ -58,9 +58,9 @@ void EthernetBase::handleINTn() ...@@ -58,9 +58,9 @@ void EthernetBase::handleINTn()
void EthernetBase::sendMsg(const mavlink_message_t& msg) 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) ...@@ -95,9 +95,9 @@ bool EthernetBase::start(std::unique_ptr<Boardcore::Wiz5500> wiz5500)
auto mav_handler = [this](EthernetMavDriver* channel, auto mav_handler = [this](EthernetMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); }; 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; return false;
} }
......
...@@ -25,7 +25,7 @@ ...@@ -25,7 +25,7 @@
#include <ActiveObject.h> #include <ActiveObject.h>
#include <common/Mavlink.h> #include <common/Mavlink.h>
#include <drivers/WIZ5500/WIZ5500.h> #include <drivers/WIZ5500/WIZ5500.h>
#include <radio/MavlinkDriver/MavlinkDriver.h> #include <radio/MavlinkDriver/MavlinkDriverV0.h>
#include <memory> #include <memory>
...@@ -36,7 +36,7 @@ Boardcore::WizIp genNewRandomIp(); ...@@ -36,7 +36,7 @@ Boardcore::WizIp genNewRandomIp();
Boardcore::WizMac genNewRandomMac(); Boardcore::WizMac genNewRandomMac();
using EthernetMavDriver = 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 class EthernetBase : public Boardcore::Transceiver
{ {
...@@ -64,7 +64,7 @@ private: ...@@ -64,7 +64,7 @@ private:
bool started = false; bool started = false;
std::unique_ptr<Boardcore::Wiz5500> wiz5500; std::unique_ptr<Boardcore::Wiz5500> wiz5500;
std::unique_ptr<EthernetMavDriver> mav_driver; std::unique_ptr<EthernetMavDriver> mavDriver;
}; };
} // namespace Groundstation } // namespace Groundstation
\ No newline at end of file
...@@ -34,9 +34,9 @@ bool Serial::start() ...@@ -34,9 +34,9 @@ bool Serial::start()
auto mav_handler = [this](SerialMavDriver* channel, auto mav_handler = [this](SerialMavDriver* channel,
const mavlink_message_t& msg) { handleMsg(msg); }; 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; return false;
} }
...@@ -46,9 +46,9 @@ bool Serial::start() ...@@ -46,9 +46,9 @@ bool Serial::start()
void Serial::sendMsg(const mavlink_message_t& msg) 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);
} }
} }
......
...@@ -24,7 +24,7 @@ ...@@ -24,7 +24,7 @@
#include <ActiveObject.h> #include <ActiveObject.h>
#include <common/Mavlink.h> #include <common/Mavlink.h>
#include <radio/MavlinkDriver/MavlinkDriver.h> #include <radio/MavlinkDriver/MavlinkDriverV0.h>
#include <memory> #include <memory>
#include <utils/ModuleManager/ModuleManager.hpp> #include <utils/ModuleManager/ModuleManager.hpp>
...@@ -33,7 +33,7 @@ namespace Groundstation ...@@ -33,7 +33,7 @@ namespace Groundstation
{ {
using SerialMavDriver = 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. * @brief Class responsible for UART communication.
...@@ -64,7 +64,7 @@ private: ...@@ -64,7 +64,7 @@ private:
bool send(uint8_t* pkt, size_t len) override; bool send(uint8_t* pkt, size_t len) override;
miosix::FastMutex mutex; miosix::FastMutex mutex;
std::unique_ptr<SerialMavDriver> mav_driver; std::unique_ptr<SerialMavDriver> mavDriver;
}; };
} // namespace Groundstation } // namespace Groundstation
\ No newline at end of file
...@@ -32,18 +32,7 @@ using namespace Boardcore; ...@@ -32,18 +32,7 @@ using namespace Boardcore;
bool RadioBase::sendMsg(const mavlink_message_t& msg) bool RadioBase::sendMsg(const mavlink_message_t& msg)
{ {
Lock<FastMutex> l(pending_msgs_mutex); return mavDriver->enqueueMsg(msg);
if (pending_msgs_count >= MAV_PENDING_OUT_QUEUE_SIZE)
{
return false;
}
else
{
pending_msgs[pending_msgs_count] = msg;
pending_msgs_count += 1;
return true;
}
} }
void RadioBase::handleDioIRQ() void RadioBase::handleDioIRQ()
...@@ -58,7 +47,7 @@ RadioStats RadioBase::getStats() ...@@ -58,7 +47,7 @@ RadioStats RadioBase::getStats()
{ {
if (started) if (started)
{ {
auto mav_stats = mav_driver->getStatus(); auto mav_stats = mavDriver->getStatus();
return {.send_errors = mav_stats.nSendErrors, return {.send_errors = mav_stats.nSendErrors,
.packet_rx_success_count = .packet_rx_success_count =
...@@ -79,14 +68,19 @@ bool RadioBase::start(std::unique_ptr<SX1278Fsk> sx1278) ...@@ -79,14 +68,19 @@ bool RadioBase::start(std::unique_ptr<SX1278Fsk> sx1278)
{ {
this->sx1278 = std::move(sx1278); this->sx1278 = std::move(sx1278);
auto mav_handler = [this](RadioMavDriver* channel, auto mav_handler =
const mavlink_message_t& msg) { handleMsg(msg); }; [&](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>( mavDriver = std::make_unique<RadioMavDriver>(
this, mav_handler, Groundstation::MAV_SLEEP_AFTER_SEND, this, pingMsgId, mav_handler,
Groundstation::MAV_OUT_BUFFER_MAX_AGE); Groundstation::MAV_SLEEP_AFTER_SEND,
Groundstation::AUTOMATIC_FLUSH_DELAY);
if (!mav_driver->start()) if (!mavDriver->start())
{ {
return false; return false;
} }
...@@ -105,13 +99,7 @@ void RadioBase::run() ...@@ -105,13 +99,7 @@ void RadioBase::run()
while (!shouldStop()) while (!shouldStop())
{ {
miosix::Thread::sleep(AUTOMATIC_FLUSH_PERIOD); miosix::Thread::sleep(10);
// If enough time has passed, automatically flush.
if (miosix::getTick() > last_eot_packet_ts + AUTOMATIC_FLUSH_DELAY)
{
flush();
}
} }
} }
...@@ -136,32 +124,3 @@ bool RadioBase::send(uint8_t* pkt, size_t len) ...@@ -136,32 +124,3 @@ bool RadioBase::send(uint8_t* pkt, size_t len)
return ret; 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
...@@ -26,7 +26,7 @@ ...@@ -26,7 +26,7 @@
#include <Groundstation/Common/Config/RadioConfig.h> #include <Groundstation/Common/Config/RadioConfig.h>
#include <common/Mavlink.h> #include <common/Mavlink.h>
#include <common/Radio.h> #include <common/Radio.h>
#include <radio/MavlinkDriver/MavlinkDriver.h> #include <radio/MavlinkDriver/MavlinkDriverPigna.h>
#include <radio/SX1278/SX1278Fsk.h> #include <radio/SX1278/SX1278Fsk.h>
#include <memory> #include <memory>
...@@ -35,9 +35,8 @@ ...@@ -35,9 +35,8 @@
namespace Groundstation namespace Groundstation
{ {
using RadioMavDriver = using RadioMavDriver = Boardcore::MavlinkDriverPignaSlave<
Boardcore::MavlinkDriver<Boardcore::SX1278Fsk::MTU, Boardcore::SX1278Fsk::MTU, Groundstation::MAV_PENDING_OUT_QUEUE_SIZE,
Groundstation::MAV_OUT_QUEUE_SIZE,
MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>; MAVLINK_MAX_DIALECT_PAYLOAD_SIZE>;
/** /**
...@@ -61,7 +60,7 @@ struct RadioStats ...@@ -61,7 +60,7 @@ struct RadioStats
class RadioBase : private Boardcore::ActiveObject, public Boardcore::Transceiver class RadioBase : private Boardcore::ActiveObject, public Boardcore::Transceiver
{ {
public: public:
RadioBase() {} RadioBase(uint8_t pingMsgId): pingMsgId(pingMsgId) {}
/** /**
* @brief Send a mavlink message through this radio. * @brief Send a mavlink message through this radio.
...@@ -93,36 +92,18 @@ private: ...@@ -93,36 +92,18 @@ private:
bool send(uint8_t* pkt, size_t len) override; 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; 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_rx_count = 0;
uint32_t bits_tx_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 // Objects are always destructed in reverse order, so keep them in this
// order // order
std::unique_ptr<Boardcore::SX1278Fsk> sx1278; std::unique_ptr<Boardcore::SX1278Fsk> sx1278;
std::unique_ptr<RadioMavDriver> mav_driver; std::unique_ptr<RadioMavDriver> mavDriver;
}; };
} // namespace Groundstation } // namespace Groundstation
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment