diff --git a/CMakeLists.txt b/CMakeLists.txt
index 26363322d4e7e677b86a8fc4fdb4e8264736d062..c8fdb7953d78dbc335ce4b0cddd2a3a6681fe1e2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -276,6 +276,9 @@ add_executable(test-sx1278-serial
)
sbs_target(test-sx1278-serial stm32f429zi_stm32f4discovery)
+add_executable(test-mavlinkdriver src/tests/radio/test-mavlinkdriver.cpp)
+sbs_target(test-mavlinkdriver stm32f407vg_stm32f4discovery)
+
#-----------------------------------------------------------------------------#
# Tests - Sensors #
#-----------------------------------------------------------------------------#
@@ -367,3 +370,6 @@ sbs_target(test-csvreader stm32f429zi_stm32f4discovery)
add_executable(test-buttonhandler src/tests/utils/test-buttonhandler.cpp)
sbs_target(test-buttonhandler stm32f429zi_stm32f4discovery)
+
+add_executable(test-syncpacketqueue src/tests/utils/test-syncpacketqueue.cpp)
+sbs_target(test-syncpacketqueue stm32f407vg_stm32f4discovery)
diff --git a/src/shared/radio/MavlinkDriver/MavlinkDriver.h b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
index 4c50b275c2cc0d7b78058fce9a49030b394ceb32..e1e184220fe21826a2a33419167709551439f5fb 100644
--- a/src/shared/radio/MavlinkDriver/MavlinkDriver.h
+++ b/src/shared/radio/MavlinkDriver/MavlinkDriver.h
@@ -98,10 +98,19 @@ public:
* Message is discarded if the queue is full.
*
* @param msg Message to send (mavlink struct).
- * @return true if the message could be enqueued (queue not full).
+ * @return True if the message could be enqueued.
*/
bool enqueueMsg(const mavlink_message_t& msg);
+ /**
+ * @brief Enqueue a raw packet message into the sync packet queue.
+ *
+ * @param msg Messa to send.
+ * @param size Length in bytes.
+ * @return True if the message was enqueued.
+ */
+ bool enqueueRaw(uint8_t* msg, size_t size);
+
/**
* @brief Receiver thread: reads one char at a time from the transceiver and
* tries to parse a mavlink message.
@@ -110,16 +119,18 @@ public:
* executed.
*/
void runReceiver();
+
/**
* @brief Sender Thread: Periodically flushes the message queue and sends
* all the enqueued messages.
+ *
* After every send, the thread sleeps to guarantee some silence on the
* channel.
*/
void runSender();
/**
- * @brief Synchronized status getter
+ * @brief Synchronized status getter.
*/
MavlinkStatus getStatus();
@@ -139,7 +150,8 @@ private:
}
/**
- * @brief Calls the run member function
+ * @brief Calls the run member function.
+ *
* @param arg the object pointer cast to void*
*/
static void sndLauncher(void* arg)
@@ -151,8 +163,8 @@ private:
void updateSenderStats(size_t msgCount, bool sent);
- Transceiver* device; // transceiver used to send and receive
- MavHandler onReceive; // function executed on message rcv
+ Transceiver* device; ///< transceiver used to send and receive
+ MavHandler onReceive; ///< function executed on message rcv
// Tweakable params
uint16_t sleepAfterSend;
@@ -254,7 +266,22 @@ bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueMsg(
// Update stats
updateQueueStats(appended);
- // return ok even if a packet was discarded
+ // Return ok even if a packet was discarded
+ return appended;
+}
+
+template <unsigned int PktLength, unsigned int OutQueueSize,
+ unsigned int MavMsgLength>
+bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueRaw(
+ uint8_t* msg, size_t size)
+{
+ // Append message to the queue
+ bool appended = outQueue.put(msg, size);
+
+ // Update stats
+ updateQueueStats(appended);
+
+ // Return ok even if a packet was discarded
return appended;
}
diff --git a/src/shared/sensors/UBXGPS/UBXFrame.h b/src/shared/sensors/UBXGPS/UBXFrame.h
index a54d31c0334b85c8c806d308fac776110af56a8f..9f1593baaa01a1ce8b3d7be31488e9ba12cfd05d 100644
--- a/src/shared/sensors/UBXGPS/UBXFrame.h
+++ b/src/shared/sensors/UBXGPS/UBXFrame.h
@@ -35,14 +35,14 @@ namespace Boardcore
*/
enum class UBXMessage : uint16_t
{
- UBX_NAV_PVT = 0x0701, // Navigation position velocity time solution
- UBX_ACK_NAK = 0x0005, // Message acknowledged
- UBX_ACK_ACK = 0x0105, // Message not acknowledged
- UBX_CFG_PRT = 0x0006, // Port configuration
- UBX_CFG_MSG = 0x0106, // Set message rate
- UBX_CFG_RST = 0x0406, // Reset receiver
- UBX_CFG_RATE = 0x0806, // Navigation/measurement rate settings
- UBX_CFG_NAV5 = 0x2406, // Navigation engine settings
+ UBX_NAV_PVT = 0x0701, // Navigation position velocity time solution
+ UBX_ACK_NAK = 0x0005, // Message acknowledged
+ UBX_ACK_ACK = 0x0105, // Message not acknowledged
+ UBX_CFG_PRT = 0x0006, // Port configuration
+ UBX_CFG_MSG = 0x0106, // Set message rate
+ UBX_CFG_RST = 0x0406, // Reset receiver
+ UBX_CFG_RATE = 0x0806, // Navigation/measurement rate settings
+ UBX_CFG_NAV5 = 0x2406, // Navigation engine settings
};
/**
diff --git a/src/tests/radio/test-mavlinkdriver.cpp b/src/tests/radio/test-mavlinkdriver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c7d14b5108f9304be08b6284a6efd3404a8e0e32
--- /dev/null
+++ b/src/tests/radio/test-mavlinkdriver.cpp
@@ -0,0 +1,73 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-align"
+#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+#include <mavlink_lib/pyxis/mavlink.h>
+#pragma GCC diagnostic pop
+
+#include <drivers/usart/USART.h>
+#include <miosix.h>
+#include <radio/MavlinkDriver/MavlinkDriver.h>
+#include <radio/SerialTransceiver/SerialTransceiver.h>
+#include <scheduler/TaskScheduler.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+USART usart(USART2, USARTInterface::Baudrate::B115200);
+SerialTransceiver transceiver(usart);
+MavlinkDriver<20, 10> mavlink(&transceiver);
+
+void payloadGenerator();
+
+int main()
+{
+ u2rx1::mode(Mode::ALTERNATE);
+ u2rx1::alternateFunction(7);
+ u2tx1::mode(Mode::ALTERNATE);
+ u2tx1::alternateFunction(7);
+
+ TaskScheduler scheduler;
+ scheduler.addTask(payloadGenerator, 2000);
+ scheduler.start();
+
+ usart.init();
+ mavlink.start();
+
+ while (true)
+ Thread::sleep(1000);
+}
+
+void payloadGenerator()
+{
+ // uint8_t msg[] = "012345678";
+ // mavlink.enqueueRaw(msg, 9);
+ // printf("Added 9 bytes: %s\n", msg);
+
+ // Create a mavlink packet
+ mavlink_message_t ackMsg;
+ mavlink_msg_ack_tm_pack(0x01, 0x23, &ackMsg, 0x45, 0x67);
+ mavlink.enqueueMsg(ackMsg);
+}
diff --git a/src/tests/utils/test-syncpacketqueue.cpp b/src/tests/utils/test-syncpacketqueue.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b2a09e101477755e91c134bb172a1a879e6a258
--- /dev/null
+++ b/src/tests/utils/test-syncpacketqueue.cpp
@@ -0,0 +1,72 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <miosix.h>
+#include <scheduler/TaskScheduler.h>
+#include <utils/collections/SyncPacketQueue.h>
+
+using namespace miosix;
+using namespace Boardcore;
+
+SyncPacketQueue<20, 10> queue;
+
+void payloadGenerator();
+
+int main()
+{
+ TaskScheduler scheduler;
+ scheduler.addTask(payloadGenerator, 1000);
+ scheduler.start();
+
+ while (true)
+ {
+ queue.waitUntilNotEmpty();
+ auto packet = queue.get();
+
+ if (packet.isReady())
+ {
+ queue.pop();
+
+ uint8_t packetContent[20];
+ size_t size = packet.dump(packetContent);
+
+ printf("[%.2f] ",
+ TimestampTimer::getInstance().getTimestamp() / 1e6);
+
+ for (size_t i = 0; i < size; i++)
+ printf("%c", packetContent[i]);
+
+ printf(" size: %zu\n", size);
+ }
+ else
+ {
+ Thread::sleep(50);
+ }
+ }
+}
+
+void payloadGenerator()
+{
+ uint8_t msg[] = "012345678";
+ queue.put(msg, 9);
+ printf("Filled queue with 9 bytes: %s\n", msg);
+}