From 4f5c90191440ab7e25149c3f2de504db9d031849 Mon Sep 17 00:00:00 2001
From: Federico Lolli <federico.lolli@skywarder.eu>
Date: Tue, 13 Feb 2024 13:04:32 +0100
Subject: [PATCH] [ARP] now may work
---
.../Groundstation/Common/Ports/Serial.cpp | 22 ++++++++++++++++---
.../Groundstation/Common/Ports/Serial.h | 8 ++++++-
.../nokia-groundstation-entry.cpp | 11 ++++------
3 files changed, 30 insertions(+), 11 deletions(-)
diff --git a/src/boards/Groundstation/Common/Ports/Serial.cpp b/src/boards/Groundstation/Common/Ports/Serial.cpp
index 45f4b7712..b082a4865 100644
--- a/src/boards/Groundstation/Common/Ports/Serial.cpp
+++ b/src/boards/Groundstation/Common/Ports/Serial.cpp
@@ -29,6 +29,8 @@ using namespace miosix;
using namespace Groundstation;
using namespace Boardcore;
+constexpr uint8_t SYN = 0x16;
+
bool Serial::start()
{
auto mav_handler = [this](SerialMavDriver* channel,
@@ -52,6 +54,20 @@ void Serial::sendMsg(const mavlink_message_t& msg)
}
}
+mavlink_message_t Serial::receiveMsg()
+{
+ // MUST be 272 bytes
+ constexpr uint16_t PACKET_SIZE = sizeof(mavlink_message_t);
+ uint8_t serial_buffer[PACKET_SIZE];
+ auto serial = DefaultConsole::instance().get();
+ serial->writeBlock(&SYN, 1, 0);
+ serial->readBlock(serial_buffer, PACKET_SIZE, 0);
+
+ mavlink_message_t msg;
+ memcpy(&msg, serial_buffer, PACKET_SIZE);
+ return msg;
+}
+
void Serial::handleMsg(const mavlink_message_t& msg)
{
// Dispatch the message through the hub.
@@ -60,12 +76,12 @@ void Serial::handleMsg(const mavlink_message_t& msg)
ssize_t Serial::receive(uint8_t* pkt, size_t max_len)
{
- auto serial = miosix::DefaultConsole::instance().get();
+ auto serial = DefaultConsole::instance().get();
return serial->readBlock(pkt, max_len, 0);
}
bool Serial::send(uint8_t* pkt, size_t len)
{
- auto serial = miosix::DefaultConsole::instance().get();
+ auto serial = DefaultConsole::instance().get();
return serial->writeBlock(pkt, len, 0) != static_cast<ssize_t>(len);
-}
\ No newline at end of file
+}
diff --git a/src/boards/Groundstation/Common/Ports/Serial.h b/src/boards/Groundstation/Common/Ports/Serial.h
index ea39b6394..0115282a7 100644
--- a/src/boards/Groundstation/Common/Ports/Serial.h
+++ b/src/boards/Groundstation/Common/Ports/Serial.h
@@ -53,6 +53,12 @@ public:
*/
void sendMsg(const mavlink_message_t& msg);
+ /**
+ * @brief fills the provided mavlink_message_t with the bytes received from
+ * the port.
+ */
+ mavlink_message_t receiveMsg();
+
private:
/**
* @brief Called internally when a message is received.
@@ -67,4 +73,4 @@ private:
std::unique_ptr<SerialMavDriver> mav_driver;
};
-} // namespace Groundstation
\ No newline at end of file
+} // namespace Groundstation
diff --git a/src/entrypoints/Groundstation/nokia-groundstation-entry.cpp b/src/entrypoints/Groundstation/nokia-groundstation-entry.cpp
index 8eee8fdd1..499b4e8cd 100644
--- a/src/entrypoints/Groundstation/nokia-groundstation-entry.cpp
+++ b/src/entrypoints/Groundstation/nokia-groundstation-entry.cpp
@@ -24,7 +24,6 @@
#include <Groundstation/Nokia/Buses.h>
#include <Groundstation/Nokia/Hub.h>
#include <Groundstation/Nokia/Radio/Radio.h>
-#include <common/Mavlink.h>
#include <miosix.h>
using namespace Groundstation;
@@ -84,15 +83,13 @@ int main()
printf("Init complete!\n");
}
+ // Start the main serial loop
while (1)
{
- mavlink_message_t ackMsg;
- mavlink_msg_ack_tm_pack(0x01, 0x23, &ackMsg, 0x45, 0x67);
- modules.get<HubBase>()->dispatchOutgoingMsg(ackMsg);
- Thread::sleep(1000);
- printf("Sending message...\n");
+ mavlink_message_t msg = modules.get<Serial>()->receiveMsg();
+ modules.get<HubBase>()->dispatchOutgoingMsg(msg);
}
- idleLoop();
+ // idleLoop();
return 0;
}
--
GitLab