Implemented simple wrapper for conversions between mavlink messages and NASState struct

src/entrypoints/NASStateWrapper.h

+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Riccardo Musso
+ *
+ * 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 once
+
+#include <algorithms/NAS/NASState.h>
+#include <mavlink_lib/pyxis/mavlink.h>
+
+namespace Boardcore
+{
+
+struct NASStateWrapper
+{
+    NASState nasState;
+
+    NASStateWrapper() {}
+    NASStateWrapper(const NASState& _nasState) : nasState(_nasState) {}
+    NASStateWrapper(NASState&& _nasState) : nasState(std::move(_nasState)) {}
+
+    bool unpack(const mavlink_message_t& message)
+    {
+        if (message.msgid != MAVLINK_MSG_ID_NAS_TM)
+            return false;
+
+        // Position [m]
+        nasState.n = mavlink_msg_nas_tm_get_nas_n(&message);
+        nasState.e = mavlink_msg_nas_tm_get_nas_e(&message);
+        nasState.d = mavlink_msg_nas_tm_get_nas_d(&message);
+
+        // Velocity [m/s]
+        nasState.vn = mavlink_msg_nas_tm_get_nas_vn(&message);
+        nasState.ve = mavlink_msg_nas_tm_get_nas_ve(&message);
+        nasState.vd = mavlink_msg_nas_tm_get_nas_vd(&message);
+
+        // Attitude as quaternion
+        nasState.qx = mavlink_msg_nas_tm_get_nas_qx(&message);
+        nasState.qy = mavlink_msg_nas_tm_get_nas_qy(&message);
+        nasState.qz = mavlink_msg_nas_tm_get_nas_qz(&message);
+        nasState.qw = mavlink_msg_nas_tm_get_nas_qw(&message);
+
+        // Gyroscope bias
+        nasState.bx = mavlink_msg_nas_tm_get_nas_bias_x(&message);
+        nasState.by = mavlink_msg_nas_tm_get_nas_bias_y(&message);
+        nasState.bz = mavlink_msg_nas_tm_get_nas_bias_z(&message);
+
+        return true;
+    }
+
+    mavlink_message_t pack(uint8_t system_id, uint8_t component_id,
+                           uint64_t timestamp, uint8_t state,
+                           float ref_pressure, float ref_temperature,
+                           float ref_latitude, float ref_longitude) const
+    {
+        mavlink_message_t message;
+        mavlink_msg_nas_tm_pack(
+            system_id, component_id, &message, timestamp, state, nasState.n,
+            nasState.e, nasState.d, nasState.vn, nasState.ve, nasState.vd,
+            nasState.qx, nasState.qy, nasState.qz, nasState.qw, nasState.bx,
+            nasState.by, nasState.bz, ref_pressure, ref_temperature,
+            ref_latitude, ref_longitude);
+        return message;
+    }
+
+    const NASState& getNASState() const { return nasState; }
+    void setNASState(const NASState& _nasState) { nasState = _nasState; }
+    void setNASState(NASState&& _nasState) { nasState = std::move(_nasState); }
+
+    operator NASState() { return nasState; }
+};
+
+}  // namespace Boardcore
\ No newline at end of file

src/entrypoints/groundstation-nas-extractor.cpp

@@ -26,6 +26,8 @@
 #include <radio/SerialTransceiver/SerialTransceiver.h>
 #include <scheduler/TaskScheduler.h>
 
+#include "NASStateWrapper.h"
+
 using namespace miosix;
 using namespace Boardcore;
 
@@ -37,6 +39,7 @@ void receiveHandler(MavDriver*, const mavlink_message_t&);
 USART usart(USART1, USARTInterface::Baudrate::B115200);
 SerialTransceiver transceiver(usart);
 MavDriver mavlink(&transceiver, receiveHandler);
+NASStateWrapper nasStateWrapper;
 TaskScheduler scheduler;
 
 int main()
@@ -58,19 +61,24 @@ int main()
 
 void sendMessage()
 {
-    mavlink_message_t message;
-    mavlink_msg_nas_tm_pack(1, 1, &message, TimestampTimer::getTimestamp(), 0,
-                            0.75, 2.54, 120.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                            0, 0, 0);
-    mavlink.enqueueMsg(message);
+    NASState state;
+    state.bx = 42.;
+    state.qw = 66.5;
+    nasStateWrapper.setNASState(state);
+
+    mavlink.enqueueMsg(nasStateWrapper.pack(
+        1, 1, TimestampTimer::getTimestamp(), 0, 0, 0, 0, 0));
     printf("Sent message\n");
 }
 
 void receiveHandler(MavDriver* channel, const mavlink_message_t& msg)
 {
-    printf("Received message (%llu) %f %f %f\n",
-           mavlink_msg_nas_tm_get_timestamp(&msg),
-           mavlink_msg_nas_tm_get_nas_n(&msg),
-           mavlink_msg_nas_tm_get_nas_e(&msg),
-           mavlink_msg_nas_tm_get_nas_d(&msg));
+    if (nasStateWrapper.unpack(msg))
+    {
+
+        NASState state = nasStateWrapper.getNASState();
+
+        printf("Received message (%llu) %f %f\n",
+               mavlink_msg_nas_tm_get_timestamp(&msg), state.bx, state.qw);
+    }
 }