GCS RTK Mavcore Message

messages/__init__.py

@@ -37,6 +37,7 @@
 from mavcore.messages.heartbeat_msg import MAVState as MAVState
 from mavcore.messages.takeoff_msg import MAVFrame as MAVFrame
 from mavcore.messages.status_text_msg import MAVSeverity as MAVSeverity
+from mavcore.messages.rc_channels_msg import RCChannels as RCChannels
 from mavcore.messages.rc_override_msg import RCOverride as RCOverride
 from mavcore.messages.reboot_msg import RebootMsg as RebootMsg
 from mavcore.messages.request_msg_interval_msg import (

messages/rc_channels_msg.py

@@ -0,0 +1,69 @@
+from mavcore.mav_message import MAVMessage, thread_safe
+
+
+class RCChannels(MAVMessage):
+    """
+    Raw RC input channels as seen by the flight controller.
+
+    Channel values are PWM microseconds (typically 1000-2000).
+    These reflect pilot input *before* any AUX function mapping.
+    """
+
+    def __init__(self):
+        super().__init__("RC_CHANNELS")
+
+        self.time_boot_ms = -1  # time since system boot in ms
+
+        # Raw RC channel values (PWM)
+        self.chan1_raw = 0
+        self.chan2_raw = 0
+        self.chan3_raw = 0
+        self.chan4_raw = 0
+        self.chan5_raw = 0
+        self.chan6_raw = 0
+        self.chan7_raw = 0
+        self.chan8_raw = 0
+        self.chan9_raw = 0
+        self.chan10_raw = 0
+        self.chan11_raw = 0
+        self.chan12_raw = 0
+        self.chan13_raw = 0
+        self.chan14_raw = 0
+        self.chan15_raw = 0
+        self.chan16_raw = 0
+        self.chan17_raw = 0
+        self.chan18_raw = 0
+
+        self.chancount = 0  # number of valid channels
+
+        self.rssi = 0
+
+    def decode(self, msg):
+        self.time_boot_ms = msg.time_boot_ms
+
+        self.chan1_raw = msg.chan1_raw
+        self.chan2_raw = msg.chan2_raw
+        self.chan3_raw = msg.chan3_raw
+        self.chan4_raw = msg.chan4_raw
+        self.chan5_raw = msg.chan5_raw
+        self.chan6_raw = msg.chan6_raw
+        self.chan7_raw = msg.chan7_raw
+        self.chan8_raw = msg.chan8_raw
+        self.chan9_raw = msg.chan9_raw
+        self.chan10_raw = msg.chan10_raw
+        self.chan11_raw = msg.chan11_raw
+        self.chan12_raw = msg.chan12_raw
+        self.chan13_raw = msg.chan13_raw
+        self.chan14_raw = msg.chan14_raw
+        self.chan15_raw = msg.chan15_raw
+        self.chan16_raw = msg.chan16_raw
+        self.chan17_raw = msg.chan17_raw
+        self.chan18_raw = msg.chan18_raw
+
+        self.chancount = msg.chancount
+
+        self.rssi = msg.rssi
+
+    @thread_safe
+    def __repr__(self):
+        return f"(RC_CHANNELS) timestamp: {self.timestamp}, channels: {self.chancount}, rssi: {self.rssi}"

messages/request_msg_interval_msg.py

@@ -12,6 +12,8 @@ class IntervalMessageID(Enum):
     ATTITUDE_QUATERNION = 31
     LOCAL_POSITION_NED = 32
     GLOBAL_POSITION_INT = 33
+    RC_CHANNELS = 65
+    VFR_HUD = 74
     BATTERY_STATUS = 147
 
 

messages/rtk_msg.py

@@ -0,0 +1,36 @@
+import pymavlink.dialects.v20.all as dialect
+from mavcore.mav_message import MAVMessage, thread_safe
+
+
+class RTKData(MAVMessage):
+    """
+    Sends RTCM message.
+    """
+
+    def __init__(
+        self,
+        is_fragmented: bool,
+        fragment_id: int,
+        sequence_num: int,
+        payload: list[int],
+    ):
+        super().__init__("RTK_DATA")
+        self.flags = (
+            (sequence_num << 3) | (fragment_id << 1) | (1 if is_fragmented else 0)
+        )
+        self.data = payload
+        self.data_length = len(payload)
+
+    def encode(self, system_id, component_id):
+        if self.data_length < 180:
+            self.data = self.data + [0 for _ in range(180 - (self.data_length))]
+        return dialect.MAVLink_gps_rtcm_data_message(
+            flags=self.flags, len=self.data_length, data=self.data
+        )
+
+    @thread_safe
+    def __repr__(self):
+        return f"""(GPS_RTCM_DATA)\n
+            \tflags: {bin(self.flags)}\n
+            \tlength: {self.data_length}\n
+            \tdata: {self.data}"""

messages/sys_status_msg.py

@@ -32,3 +32,6 @@ def compass_health(self):
 
     def level_health(self):
         return bool((self.health & 2**11) >> 11)
+
+    def estop(self):
+        return bool((self.health & 2**15) >> 15)

protocols/send_rtk_protocol.py

@@ -0,0 +1,37 @@
+from mavcore.mav_protocol import MAVProtocol
+from mavcore.messages.rtk_msg import RTKData
+from mavcore.messages.command_ack_msg import CommandAck
+
+GPS_RTCM_MAX_LENGTH = 180
+
+
+class SendRTKProtocol(MAVProtocol):
+    """
+    Sends RTK message and splits it into up to 4 fragments, since payloads have a
+    maximum size of 180 bytes.
+    """
+
+    def __init__(self, target_system: int = 1, target_component: int = 0):
+        super().__init__()
+        self.target_system = target_system
+        self.target_component = target_component
+        self.sequence_num = 0
+        self.payload = []
+
+    def update(self, data):
+        self.payload = list(data)
+
+    def run(self, sender, receiver):
+        fragment_id = 0
+        while self.payload and fragment_id < 4:
+            msg_length = min(len(self.payload), GPS_RTCM_MAX_LENGTH)
+            rtk_msg = RTKData(
+                msg_length > GPS_RTCM_MAX_LENGTH,
+                fragment_id,
+                self.sequence_num,
+                self.payload[:msg_length],
+            )
+            sender.send_msg(rtk_msg)
+            fragment_id += 1
+            self.payload = self.payload[msg_length:]
+        self.sequence_num = (self.sequence_num + 1) % 32