diff --git a/radio/app/customTypes.py b/radio/app/customTypes.py index 1d0b19398..b47db3ad5 100644 --- a/radio/app/customTypes.py +++ b/radio/app/customTypes.py @@ -1,3 +1,4 @@ +from enum import Enum from typing import Any, Union from typing_extensions import NotRequired, TypedDict @@ -37,3 +38,9 @@ class MotorTestAllValues(TypedDict): class SetFlightModeValueAndNumber(TypedDict): mode_number: int flight_mode: int + + +class VehicleType(Enum): + UNKNOWN = 0 + FIXED_WING = 1 + MULTIROTOR = 2 diff --git a/radio/app/drone.py b/radio/app/drone.py index 697b39294..fbb318bdf 100644 --- a/radio/app/drone.py +++ b/radio/app/drone.py @@ -22,8 +22,8 @@ from app.controllers.navController import NavController from app.controllers.paramsController import ParamsController from app.controllers.rcController import RcController -from app.customTypes import Number, Response -from app.utils import commandAccepted +from app.customTypes import Number, Response, VehicleType +from app.utils import commandAccepted, getVehicleType # Constants @@ -132,8 +132,11 @@ def __init__( self.sendConnectionStatusUpdate("Received heartbeat") - self.aircraft_type = initial_heartbeat.type - if self.aircraft_type not in (1, 2): + self.aircraft_type = getVehicleType(initial_heartbeat.type) + if self.aircraft_type not in ( + VehicleType.FIXED_WING.value, + VehicleType.MULTIROTOR.value, + ): self.logger.error("Aircraft not plane or quadcopter") self.master.close() self.master = None diff --git a/radio/app/utils.py b/radio/app/utils.py index 3fc9bd87a..da7522d8f 100644 --- a/radio/app/utils.py +++ b/radio/app/utils.py @@ -4,6 +4,8 @@ from pymavlink import mavutil from serial.tools import list_ports +from app.customTypes import VehicleType + from . import socketio @@ -207,3 +209,35 @@ def wpToMissionItemInt( wp.mission_type, ) return wp_int + + +FIXED_WING_TYPES = [ + mavutil.mavlink.MAV_TYPE_FIXED_WING, + mavutil.mavlink.MAV_TYPE_VTOL_DUOROTOR, + mavutil.mavlink.MAV_TYPE_VTOL_QUADROTOR, + mavutil.mavlink.MAV_TYPE_VTOL_TILTROTOR, + # mavutil.mavlink.MAV_TYPE_VTOL_TAILSITTER, + # mavutil.mavlink.MAV_TYPE_VTOL_TILTWING, + mavutil.mavlink.MAV_TYPE_VTOL_RESERVED2, + mavutil.mavlink.MAV_TYPE_VTOL_RESERVED3, + mavutil.mavlink.MAV_TYPE_VTOL_RESERVED4, + mavutil.mavlink.MAV_TYPE_VTOL_RESERVED5, +] + +MULTIROTOR_TYPES = [ + mavutil.mavlink.MAV_TYPE_QUADROTOR, + mavutil.mavlink.MAV_TYPE_HEXAROTOR, + mavutil.mavlink.MAV_TYPE_OCTOROTOR, + mavutil.mavlink.MAV_TYPE_TRICOPTER, + mavutil.mavlink.MAV_TYPE_DODECAROTOR, + mavutil.mavlink.MAV_TYPE_ADSB, # For cube orange (?) +] + + +def getVehicleType(typeId: int) -> int: + if typeId in FIXED_WING_TYPES: + return VehicleType.FIXED_WING.value + elif typeId in MULTIROTOR_TYPES: + return VehicleType.MULTIROTOR.value + else: + return VehicleType.UNKNOWN.value