diff --git a/messages/__init__.py b/messages/__init__.py index 7451b3a..c93fa86 100644 --- a/messages/__init__.py +++ b/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 ( diff --git a/messages/rc_channels_msg.py b/messages/rc_channels_msg.py new file mode 100644 index 0000000..cb28d8e --- /dev/null +++ b/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}" diff --git a/messages/request_msg_interval_msg.py b/messages/request_msg_interval_msg.py index 93f2b0b..2b4c1c5 100644 --- a/messages/request_msg_interval_msg.py +++ b/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 diff --git a/messages/rtk_msg.py b/messages/rtk_msg.py new file mode 100644 index 0000000..88a4c84 --- /dev/null +++ b/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}""" diff --git a/messages/sys_status_msg.py b/messages/sys_status_msg.py index b7eabf6..0675eb3 100644 --- a/messages/sys_status_msg.py +++ b/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) diff --git a/protocols/send_rtk_protocol.py b/protocols/send_rtk_protocol.py new file mode 100644 index 0000000..a3ffab4 --- /dev/null +++ b/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