diff --git a/mavctl/connect/conn.py b/mavctl/connect/conn.py index 845134d..4d1fac0 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..ba76826 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(): diff --git a/mavctl/messages/location.py b/mavctl/messages/location.py index b020cf7..dcd2c31 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) diff --git a/mavctl/messages/navigator.py b/mavctl/messages/navigator.py index a32e6ee..f355329 100644 --- a/mavctl/messages/navigator.py +++ b/mavctl/messages/navigator.py @@ -1,15 +1,17 @@ import time -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 +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__) @@ -84,6 +86,8 @@ class ADSBAvoidanceConfig: 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): @@ -95,7 +99,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) # ---------------------- @@ -182,6 +188,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 # ---------------------- @@ -214,6 +241,65 @@ 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 + # ---------------------- + 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 # ---------------------- @@ -266,7 +352,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, @@ -326,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) @@ -334,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) @@ -617,3 +704,119 @@ def generate_typemask(self, ignores: 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" + + 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 + + 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. + 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 + + # 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.AUTOMATIC_FLIGHT_MODES: + return self.FLIGHT_MODE_ARMED_AUTOMATIC + + # 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 + + # ---------------------- + # 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 + + return round(percent, 1) + + # ---------------------- + # 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