From c3651b25043f66cba2fb63d117d8fb13de44834a Mon Sep 17 00:00:00 2001 From: rpolishchuk3 Date: Sat, 10 Oct 2026 15:22:40 -0600 Subject: [PATCH 1/4] Added telemetry --- mavctl/messages/navigator.py | 62 +++++++++++++++++++++++++++++++++++- 1 file changed, 61 insertions(+), 1 deletion(-) diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index b08ff14..b78b264 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -1,4 +1,5 @@ import time +import threading # not implemented yet from math import atan, pi from dataclasses import dataclass from typing import Optional, Iterable @@ -6,7 +7,7 @@ from pymavlink import mavutil from mavctl._internal import util -from mavctl.messages.location import LocationGlobal, LocationLocal, Velocity +from mavctl.messages.location import LocationGlobal, LocationLocal, Velocity, Battery from mavctl.connect.conn import Connect from mavctl._internal.logger import get_logger @@ -52,6 +53,8 @@ class PositionSetpointGlobal: class Navigator: # pylint: disable=too-many-public-methods,too-many-instance-attributes """Navigator class for MAVLink drones with logging and helper functions.""" + uav_id: str = "null" # UAV identifier sent in telemetry packets + def __init__(self, ip: str = "udp:127.0.0.1:14551", baud: int = 57600, heartbeat_timeout: Optional[int] = None): @@ -150,6 +153,27 @@ def wait_vehicle_armed(self): self.master.motors_armed_wait() LOGGER.info("MAVCTL: Armed!") + # ---------------------- + # UAV identifier + # ---------------------- + def get_uav_id(self) -> str: + """Returns the UAV identifier sent in telemetry packets.""" + LOGGER.debug("UAV identifier: %s", self.uav_id) + return self.uav_id + + # ---------------------- + # Timestamp + # ---------------------- + def get_timestamp(self) -> float: + """Returns the current unix time in seconds, from the autopilot's GPS clock if available.""" + msg = self.master.recv_match(type="SYSTEM_TIME", blocking=True) + if msg and msg.time_unix_usec > 0: + timestamp = msg.time_unix_usec / 1000000 # microseconds -> seconds + else: + timestamp = time.time() + LOGGER.debug("Timestamp: %.3f", timestamp) + return timestamp + # ---------------------- # Position / Velocity access # ---------------------- @@ -182,6 +206,19 @@ def get_velocity(self) -> Velocity: return Velocity(msg.vx, msg.vy, msg.vz) return Velocity(0, 0, 0) + # ---------------------- + # Altitude AGL + # ---------------------- + def get_altitude_agl(self) -> float: + """Returns the altitude above the home (takeoff) point in metres.""" + msg = self.master.recv_match(type="GLOBAL_POSITION_INT", blocking=True) + if msg: + altitude = msg.relative_alt / 1000 # millimetres -> metres + else: + altitude = 0.0 + LOGGER.debug("Altitude AGL: %.2f m", altitude) + return altitude + # ---------------------- # Takeoff # ---------------------- @@ -508,3 +545,26 @@ def generate_typemask(self, keeps: Iterable[int]) -> int: mask |= 1 << bit return mask + + # ---------------------- + # Flight modes + # ---------------------- + FLIGHT_MODE_OFF = "off" + FLIGHT_MODE_IDLE = "idle" + FLIGHT_MODE_LINK_LOST = "link-lost" + FLIGHT_MODE_FAILSAFE = "failsafe" + FLIGHT_MODE_ARMED_PILOT = "armed-pilot" + FLIGHT_MODE_ARMED_AUTOMATIC = "armed-automatic" + + # ---------------------- + # Battery + # ---------------------- + def get_battery(self) -> Battery: + """Returns the current battery voltage, current and level.""" + msg = self.master.recv_match(type="SYS_STATUS", blocking=True) + if msg: + battery = Battery(msg.voltage_battery, msg.current_battery, msg.battery_remaining) + else: + battery = Battery(0, -1, -1) + LOGGER.debug("Battery: %s", battery) + return battery From be74c80e05fcd4e7742a0b6b88de80ba1aa6df9c Mon Sep 17 00:00:00 2001 From: rpolishchuk3 Date: Sat, 31 Oct 2026 15:37:08 -0600 Subject: [PATCH 2/4] Implemented FULL telemetry --- mavctl/connect/conn.py | 4 + mavctl/connect/heartbeat.py | 58 ++++++++++++++ mavctl/messages/location.py | 20 ++++- mavctl/messages/navigator.py | 149 ++++++++++++++++++++++++++++++++++- 4 files changed, 226 insertions(+), 5 deletions(-) diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index 75267e9..e017228 100644 --- a/mavctl/connect/conn.py +++ b/mavctl/connect/conn.py @@ -69,6 +69,10 @@ def connect(self, ip: str = "udp:127.0.0.1:14551", except Exception as e: raise ConnError("MAVCTL ERROR: Failed to receive heartbeat!: ", e) from e + def get_heartbeat_manager(self) -> HeartbeatManager: + """Returns the heartbeat manager running on this connection.""" + return self._heartbeat_manager + def disconnect(self, timeout: float = 5, device: Optional[str] = None) -> bool: """Graceful Disconnect from the MAVLink Device diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index 4d770c6..cac6a5b 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -34,6 +34,12 @@ def __init__(self, master, heartbeat_timeout: float = 1.0, max_missed_heartbeats self.last_heartbeat = 0 self.missed_heartbeats = 0 self.is_connected = False + + # Latest HEARTBEAT from the autopilot itself, and when it arrived + self.last_autopilot_heartbeat = None + self.last_autopilot_heartbeat_time = 0.0 + self._autopilot_lock = threading.Lock() + self._stop_event = threading.Event() self._monitor_thread = None self._send_thread = None @@ -81,6 +87,14 @@ def _monitor_heartbeat(self): if msg is not None: self.last_heartbeat = time.time() + + # Save the autopilot's heartbeat so Navigator can read flight state + # without its own recv_match (which would race this thread) + if self._is_autopilot_heartbeat(msg): + with self._autopilot_lock: + self.last_autopilot_heartbeat = msg + self.last_autopilot_heartbeat_time = time.time() + self.missed_heartbeats = 0 if not self.is_connected: self.is_connected = True @@ -96,6 +110,28 @@ def _monitor_heartbeat(self): if callback is not None: callback() + def _is_autopilot_heartbeat(self, msg) -> bool: + """True if this HEARTBEAT came from the vehicle's autopilot.""" + # Heartbeats from other systems, e.g. Mission Planner (sysid 255) + if msg.get_srcSystem() != self.master.target_system: + return False + + # Non-autopilot components on our system, e.g. Shepard or a camera + if msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID: + return False + + return True + + def get_last_autopilot_heartbeat(self): + """ + Returns (message, receive_time) of the latest autopilot HEARTBEAT. + message is None if no autopilot heartbeat has arrived yet. + """ + with self._autopilot_lock: + heartbeat = self.last_autopilot_heartbeat + received_at = self.last_autopilot_heartbeat_time + return heartbeat, received_at + def _send_heartbeat(self): """Send heartbeat messages""" while not self._stop_event.is_set(): @@ -108,6 +144,28 @@ def _send_heartbeat(self): ) time.sleep(self.heartbeat_timeout) + def _is_autopilot_heartbeat(self, msg) -> bool: + """True if this HEARTBEAT came from the vehicle's autopilot.""" + # Heartbeats from other systems, e.g. Mission Planner (sysid 255) + if msg.get_srcSystem() != self.master.target_system: + return False + + # Non-autopilot components on our system, e.g. Shepard or a camera + if msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID: + return False + + return True + + def get_last_autopilot_heartbeat(self): + """ + Returns (message, receive_time) of the latest autopilot HEARTBEAT. + message is None if no autopilot heartbeat has arrived yet. + """ + with self._autopilot_lock: + heartbeat = self.last_autopilot_heartbeat + received_at = self.last_autopilot_heartbeat_time + return heartbeat, received_at + def get_connection_status(self) -> bool: """Get the current connection status.""" return self.is_connected diff --git a/mavctl/messages/location.py b/mavctl/messages/location.py index b020cf7..46465ab 100644 --- a/mavctl/messages/location.py +++ b/mavctl/messages/location.py @@ -31,6 +31,23 @@ def __init__(self, voltage: float, current: int, level: int): self.current = None if current == -1 else current / 100.0 self.level = None if level == -1 else level +@dataclass +class PositionAccuracy: + horizontal: Optional[float] # in metres + vertical: Optional[float] # in metres + + def __init__(self, h_acc_mm: int, v_acc_mm: int): + # GPS_RAW_INT reports millimetres, 0 means "not provided" + if h_acc_mm > 0: + self.horizontal = h_acc_mm / 1000.0 + else: + self.horizontal = None + + if v_acc_mm > 0: + self.vertical = v_acc_mm / 1000.0 + else: + self.vertical = None + @dataclass class Velocity: north: float @@ -38,5 +55,4 @@ class Velocity: down: float def magnitude(self) -> float: - """Compute the 3D magnitude of the velocity vector.""" - return sqrt(self.north ** 2 + self.east ** 2 + self.down ** 2) + return sqrt(self.north ** 2 + self.east ** 2 + self.down ** 2) \ No newline at end of file diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index b78b264..341bb76 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -1,5 +1,5 @@ import time -import threading # not implemented yet +import threading from math import atan, pi from dataclasses import dataclass from typing import Optional, Iterable @@ -7,10 +7,11 @@ from pymavlink import mavutil from mavctl._internal import util -from mavctl.messages.location import LocationGlobal, LocationLocal, Velocity, Battery +from mavctl.messages.location import LocationGlobal, LocationLocal, Velocity, Battery, PositionAccuracy from mavctl.connect.conn import Connect from mavctl._internal.logger import get_logger + # Initialize the Logger LOGGER = get_logger(__name__) @@ -66,7 +67,9 @@ def __init__(self, ip: str = "udp:127.0.0.1:14551", baud: Serial baud rate (ignored for UDP). heartbeat_timeout: Optional heartbeat timeout. """ - self.master = Connect(ip=ip, baud=baud, heartbeat_timeout=heartbeat_timeout).master + # Keep the Connect object so its heartbeat manager stays reachable + self.connection = Connect(ip=ip, baud=baud, heartbeat_timeout=heartbeat_timeout) + self.master = self.connection.master LOGGER.info("Navigator connected to %s", ip) # ---------------------- @@ -206,6 +209,52 @@ def get_velocity(self) -> Velocity: return Velocity(msg.vx, msg.vy, msg.vz) return Velocity(0, 0, 0) + # ---------------------- + # Position accuracy + # ---------------------- + def get_position_accuracy(self, timeout: float = 2.0) -> PositionAccuracy: + """ + Returns the GPS horizontal and vertical position accuracy in metres. + + Read from GPS_RAW_INT.h_acc / v_acc. These are MAVLink 2 extension + fields, so the link must be MAVLink 2. A value of None means the GPS + did not report that accuracy (or no GPS_RAW_INT arrived in time). + """ + msg = self.master.recv_match(type="GPS_RAW_INT", blocking=True, timeout=timeout) + if msg is None: + LOGGER.warning("No GPS_RAW_INT received within %.1f s", timeout) + return PositionAccuracy(0, 0) + + h_acc_mm = getattr(msg, "h_acc", 0) + v_acc_mm = getattr(msg, "v_acc", 0) + + accuracy = PositionAccuracy(h_acc_mm, v_acc_mm) + LOGGER.debug("Position accuracy: %s", accuracy) + return accuracy + + # ---------------------- + # Latest received messages (non-blocking) + # ---------------------- + def get_latest_message(self, message_type: str, max_age: float = 3.0): + """ + Returns the newest received message of this type, or None if none has + arrived yet or it is older than max_age seconds. + + pymavlink keeps the latest copy of every message in master.messages, + filled by whichever thread is reading (the HeartbeatManager reads + non-stop). So this never blocks and never steals messages. + """ + msg = self.master.messages.get(message_type) + if msg is None: + return None + + # _timestamp is pymavlink's receive time (time.time()) for the message + age = time.time() - msg._timestamp # pylint: disable=protected-access + if age > max_age: + return None + + return msg + # ---------------------- # Altitude AGL # ---------------------- @@ -556,6 +605,100 @@ def generate_typemask(self, keeps: Iterable[int]) -> int: FLIGHT_MODE_ARMED_PILOT = "armed-pilot" FLIGHT_MODE_ARMED_AUTOMATIC = "armed-automatic" + PILOT_FLIGHT_MODES = {"STABILIZE", "LOITER", "ALT_HOLD", "ACRO"} + AUTOMATIC_FLIGHT_MODES = {"GUIDED", "RTL", "LAND", "AVOID_ADSB", "AUTO"} + _warned_flight_modes = set() + + def get_flight_mode(self, heartbeat_timeout: float = 3.0) -> str: + """ + Returns the competition flight mode string, one of: + 'off', 'idle', 'failsafe', 'armed-pilot', 'armed-automatic'. + + NO 'LINK-LOST' YET + + Arguemnts: + heartbeat_timeout: Seconds without an autopilot HEARTBEAT before the vehicle is reported as 'off'. + """ + # Read the heartbeat the HeartbeatManager thread already received. + # Calling recv_match here would race that thread for heartbeats. + heartbeat_manager = self.connection.get_heartbeat_manager() + heartbeat, received_at = heartbeat_manager.get_last_autopilot_heartbeat() + + # No autopilot heartbeat received yet + if heartbeat is None: + return self.FLIGHT_MODE_OFF + + # Autopilot has been silent too long: treat it as off + silence = time.time() - received_at + if silence > heartbeat_timeout: + LOGGER.debug("Flight mode: off (no heartbeat for %.1f s)", silence) + return self.FLIGHT_MODE_OFF + + armed_bit = heartbeat.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED + if armed_bit == 0: + return self.FLIGHT_MODE_IDLE + + if heartbeat.system_status == mavutil.mavlink.MAV_STATE_CRITICAL: + return self.FLIGHT_MODE_FAILSAFE + if heartbeat.system_status == mavutil.mavlink.MAV_STATE_EMERGENCY: + return self.FLIGHT_MODE_FAILSAFE + + mode_name = mavutil.mode_string_v10(heartbeat) + + if mode_name in self.PILOT_FLIGHT_MODES: + return self.FLIGHT_MODE_ARMED_PILOT + + if mode_name in self.AUTOMATIC_FLIGHT_MODES: + return self.FLIGHT_MODE_ARMED_AUTOMATIC + + # Warn once per unlisted mode; this is called several times a second + if mode_name not in self._warned_flight_modes: + self._warned_flight_modes.add(mode_name) + LOGGER.warning("Unclassified armed flight mode %s, reporting armed-pilot", mode_name) + return self.FLIGHT_MODE_ARMED_PILOT + + # ---------------------- + # RC link + # ---------------------- + RC_RSSI_MAX = 254 + RC_RSSI_UNKNOWN = 255 + + def get_rc_link_percent(self, crsf_lq: bool = False, max_age: float = 3.0) -> Optional[float]: + """ + Returns the RC link quality as a percentage 0-100, or None if unknown. + + Args: + crsf_lq: True when the drone runs ELRS with RSSI_TYPE = 3 and RC_OPTIONS bit 11 + max_age: ignore messages older than this many seconds + """ + # RC failsafe: the receiver stopped sending, and rssi would freeze at its last value + sys_status = self.get_latest_message("SYS_STATUS", max_age) + if sys_status is not None: + rc_bit = mavutil.mavlink.MAV_SYS_STATUS_SENSOR_RC_RECEIVER + rc_enabled = sys_status.onboard_control_sensors_enabled & rc_bit + rc_healthy = sys_status.onboard_control_sensors_health & rc_bit + if rc_enabled and not rc_healthy: + return 0.0 + + rc_channels = self.get_latest_message("RC_CHANNELS", max_age) + if rc_channels is None: + return None + + if rc_channels.rssi == self.RC_RSSI_UNKNOWN: + return None + + percent = rc_channels.rssi / self.RC_RSSI_MAX * 100.0 + + if crsf_lq: + # ArduPilot stores CRSF LQ as LQ * 2.5 on a 0-255 scale, so 100 % arrives as ~98 %. + # Undo that scaling to get the receiver's real LQ back. + percent = percent * 255.0 / 250.0 + + if percent > 100.0: + percent = 100.0 + + return round(percent, 1) + # ---------------------- # Battery # ---------------------- From b0e36167c9669cc8bf629d7eea44356f0cc26c90 Mon Sep 17 00:00:00 2001 From: rpolishchuk3 Date: Thu, 5 Nov 2026 13:59:58 -0600 Subject: [PATCH 3/4] Fix pylint errors in navigator, heartbeat and location --- mavctl/connect/heartbeat.py | 22 -------------------- mavctl/messages/location.py | 2 +- mavctl/messages/navigator.py | 40 +++++++++++++++++++----------------- 3 files changed, 22 insertions(+), 42 deletions(-) diff --git a/mavctl/connect/heartbeat.py b/mavctl/connect/heartbeat.py index cac6a5b..ba76826 100644 --- a/mavctl/connect/heartbeat.py +++ b/mavctl/connect/heartbeat.py @@ -144,28 +144,6 @@ def _send_heartbeat(self): ) time.sleep(self.heartbeat_timeout) - def _is_autopilot_heartbeat(self, msg) -> bool: - """True if this HEARTBEAT came from the vehicle's autopilot.""" - # Heartbeats from other systems, e.g. Mission Planner (sysid 255) - if msg.get_srcSystem() != self.master.target_system: - return False - - # Non-autopilot components on our system, e.g. Shepard or a camera - if msg.autopilot == mavutil.mavlink.MAV_AUTOPILOT_INVALID: - return False - - return True - - def get_last_autopilot_heartbeat(self): - """ - Returns (message, receive_time) of the latest autopilot HEARTBEAT. - message is None if no autopilot heartbeat has arrived yet. - """ - with self._autopilot_lock: - heartbeat = self.last_autopilot_heartbeat - received_at = self.last_autopilot_heartbeat_time - return heartbeat, received_at - def get_connection_status(self) -> bool: """Get the current connection status.""" return self.is_connected diff --git a/mavctl/messages/location.py b/mavctl/messages/location.py index 46465ab..dcd2c31 100644 --- a/mavctl/messages/location.py +++ b/mavctl/messages/location.py @@ -55,4 +55,4 @@ class Velocity: down: float def magnitude(self) -> float: - return sqrt(self.north ** 2 + self.east ** 2 + self.down ** 2) \ No newline at end of file + return sqrt(self.north ** 2 + self.east ** 2 + self.down ** 2) diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index 341bb76..d0a1ff1 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -1,13 +1,13 @@ import time -import threading -from math import atan, pi +from math import atan, pi, cos from dataclasses import dataclass from typing import Optional, Iterable from pymavlink import mavutil from mavctl._internal import util -from mavctl.messages.location import LocationGlobal, LocationLocal, Velocity, Battery, PositionAccuracy +from mavctl.messages.location import (LocationGlobal, LocationLocal, + Velocity, Battery, PositionAccuracy) from mavctl.connect.conn import Connect from mavctl._internal.logger import get_logger @@ -320,7 +320,10 @@ def set_position_target_local_ned( at their defaults. """ setpoint = PositionSetpointLocal(x=x, y=y, z=z) - self.set_position_local_ned(setpoint, coordinate_frame=coordinate_frame, type_mask=type_mask) + self.set_position_local_ned( + setpoint, + coordinate_frame=coordinate_frame, + type_mask=type_mask) def set_position_global(self, setpoint: PositionSetpointGlobal, coordinate_frame: int = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, @@ -616,8 +619,9 @@ def get_flight_mode(self, heartbeat_timeout: float = 3.0) -> str: NO 'LINK-LOST' YET - Arguemnts: - heartbeat_timeout: Seconds without an autopilot HEARTBEAT before the vehicle is reported as 'off'. + Arguments: + heartbeat_timeout: Seconds without an autopilot + HEARTBEAT before the vehicle is reported as 'off'. """ # Read the heartbeat the HeartbeatManager thread already received. # Calling recv_match here would race that thread for heartbeats. @@ -638,23 +642,24 @@ def get_flight_mode(self, heartbeat_timeout: float = 3.0) -> str: if armed_bit == 0: return self.FLIGHT_MODE_IDLE - if heartbeat.system_status == mavutil.mavlink.MAV_STATE_CRITICAL: - return self.FLIGHT_MODE_FAILSAFE - if heartbeat.system_status == mavutil.mavlink.MAV_STATE_EMERGENCY: + # CRITICAL or EMERGENCY system status both mean the autopilot is in failsafe + is_critical = heartbeat.system_status == mavutil.mavlink.MAV_STATE_CRITICAL + is_emergency = heartbeat.system_status == mavutil.mavlink.MAV_STATE_EMERGENCY + if is_critical or is_emergency: return self.FLIGHT_MODE_FAILSAFE mode_name = mavutil.mode_string_v10(heartbeat) - if mode_name in self.PILOT_FLIGHT_MODES: - return self.FLIGHT_MODE_ARMED_PILOT - if mode_name in self.AUTOMATIC_FLIGHT_MODES: return self.FLIGHT_MODE_ARMED_AUTOMATIC - # Warn once per unlisted mode; this is called several times a second - if mode_name not in self._warned_flight_modes: - self._warned_flight_modes.add(mode_name) - LOGGER.warning("Unclassified armed flight mode %s, reporting armed-pilot", mode_name) + # Pilot modes and unlisted modes are both reported as armed-pilot + if mode_name not in self.PILOT_FLIGHT_MODES: + # Warn once per unlisted mode; this is called several times a second + if mode_name not in self._warned_flight_modes: + self._warned_flight_modes.add(mode_name) + LOGGER.warning("Unclassified armed flight mode %s, reporting armed-pilot", + mode_name) return self.FLIGHT_MODE_ARMED_PILOT # ---------------------- @@ -694,9 +699,6 @@ def get_rc_link_percent(self, crsf_lq: bool = False, max_age: float = 3.0) -> Op # Undo that scaling to get the receiver's real LQ back. percent = percent * 255.0 / 250.0 - if percent > 100.0: - percent = 100.0 - return round(percent, 1) # ---------------------- From f0ba6d9c984fb2c39458993b06535df293042d85 Mon Sep 17 00:00:00 2001 From: rpolishchuk3 Date: Thu, 5 Nov 2026 14:04:24 -0600 Subject: [PATCH 4/4] Remove unused type_mask variables --- mavctl/messages/navigator.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index fca2da5..f355329 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -415,7 +415,6 @@ def wait_target_reached_global(self, target: Optional[PositionSetpointGlobal] = # ---------------------- def simple_goto_local(self, x: float, y: float, z: float) -> None: """Move drone to local NED coordinates.""" - type_mask = self.generate_typemask([0, 1, 2, 9]) yaw_angle = atan(y / x) if x != 0 else 0 setpoint = PositionSetpointLocal(x=x, y=y, z=-z, yaw=yaw_angle) self.set_position_local_ned(setpoint, type_mask=MASK_POS_YAW) @@ -423,7 +422,6 @@ def simple_goto_local(self, x: float, y: float, z: float) -> None: def simple_goto_global(self, lat: float, lon: float, alt: float) -> None: """Move drone to global coordinates.""" - type_mask = self.generate_typemask([0, 1, 2, 9]) start_point = self.get_global_position() yaw = util.Heading(start_point, LocationGlobal(lat, lon, alt)) setpoint = PositionSetpointGlobal(lat=lat, lon=lon, alt=alt, yaw=yaw)