Skip to content

Commit

Permalink
IMPROVEMENT: Wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@amilcarlucas
amilcarlucas committed May 31, 2024
1 parent 61020f2 commit 837330a
Show file tree
Hide file tree
Showing 4 changed files with 248 additions and 245 deletions.
4 changes: 2 additions & 2 deletions MethodicConfigurator/ardupilot_methodic_configurator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,8 @@ def main():

vehicle_type = args.vehicle_type
if vehicle_type == "": # not explicitly set, to try to guess it
if flight_controller.vehicle_type is not None:
vehicle_type = flight_controller.vehicle_type
if flight_controller.info.vehicle_type is not None:
vehicle_type = flight_controller.info.vehicle_type
logging_info("Vehicle type not set explicitly, auto-detected %s.", vehicle_type)
else:
logging_info("Vehicle type explicitly set to %s.", vehicle_type)
Expand Down
245 changes: 24 additions & 221 deletions MethodicConfigurator/backend_flightcontroller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,6 @@
except Exception: # pylint: disable=broad-exception-caught
pass

# Get the current directory
# current_dir = os_path.dirname(os_path.abspath(__file__))

# Add the current directory to the PATH environment variable
# os.environ['PATH'] = os.environ['PATH'] + os.pathsep + current_dir

preferred_ports = [
'*FTDI*',
"*Arduino_Mega_2560*",
Expand Down Expand Up @@ -114,8 +108,6 @@ def __init__(self, reboot_time: int):
self.__target_system = None
self.__target_component = None
self.__capabilities = None
self.version = None
self.vehicle_type = None
self.info = BackendFlightcontrollerInfo()

def discover_connections(self):
Expand Down Expand Up @@ -143,7 +135,7 @@ def disconnect(self):
self.__target_system = None
self.__target_component = None
self.__capabilities = None
self.version = None
self.info = BackendFlightcontrollerInfo()

def add_connection(self, connection_string: str):
"""
Expand Down Expand Up @@ -258,62 +250,38 @@ def __create_connection_with_retry(self, progress_callback, retries: int = 3, #
self.info.set_system_and_component_ids(self.__target_system, self.__target_component)
logging_debug("Connection established with systemID %d, componentID %d.", self.__target_system,
self.__target_component)
autopilot_type = self.__decode_mav_autopilot(m.autopilot)
logging_info(f"Autopilot type {autopilot_type}")
self.info.set_autopilot_type(autopilot_type)
if m.autopilot != mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOTMEGA:
logging_error("Unsupported autopilot type %s", autopilot_type)
return f"Unsupported autopilot type {autopilot_type}"
self.vehicle_type = self.__classify_vehicle_type(m.type)
self.info.set_vehicle_type(self.vehicle_type)
self.info.set_mav_type(self.__decode_mav_type(m.type))
logging_info(f"Vehicle type {self.__decode_mav_type(m.type)} running {self.vehicle_type} firmware")

self.info.set_autopilot_type(m.autopilot)
logging_info(f"Autopilot type {self.info.autopilot_type}")
if not self.info.is_supported_autopilot_type:
logging_error("Unsupported autopilot type %s", self.info.autopilot_type)
return f"Unsupported autopilot type {self.info.autopilot_type}"

self.info.set_vehicle_type(m.type)
self.info.set_mav_type(m.type)
logging_info(f"Vehicle type {self.info.mav_type} running {self.info.vehicle_type} firmware")

self.__cmd_version()
m = self.master.recv_match(type='AUTOPILOT_VERSION', blocking=True, timeout=timeout)
if m is None:
logging_error("No AUTOPILOT_VERSION MAVLink message received, connection failed.")
return "No AUTOPILOT_VERSION MAVLink message received, connection failed."
self.__capabilities = m.capabilities
cap_list = self.__decode_flight_capabilities(self.__capabilities)
self.info.set_capabilities((", ").join([cap.removeprefix("MAV_PROTOCOL_CAPABILITY_") for cap in cap_list]))
# logging_info("Flight Controller Capabilities: %s", (", ").join(
# [capability.removeprefix("MAV_PROTOCOL_CAPABILITY_")
# for capability in _cap_list]))
v_major, v_minor, v_patch, v_fw_type = self.__decode_flight_sw_version(m.flight_sw_version)
self.version = f"{v_major}.{v_minor}.{v_patch}"
self.info.set_firmware_version(self.version + " " + v_fw_type)
logging_info("Flight Controller Version: %s %s", self.version, v_fw_type)
self.__capabilities = m.capabilities
self.info.set_capabilities(m.capabilities)
self.info.set_firmware_version(m.flight_sw_version)
logging_info("Flight Controller Version: %s", self.info.firmware_version_and_type)
# logging_info(f"Flight Controller Middleware version number: {m.middleware_sw_version}")
# logging_info(f"Flight Controller Operating system version number: {m.os_sw_version}")
self.info.set_hardware_version(m.board_version)
logging_info(f"Flight Controller HW / board version: {m.board_version}")
# Convert each value in the array to hex and join them together
flight_custom_version_hex = ''.join(chr(c) for c in m.flight_custom_version)
# middleware_custom_version_hex = ''.join(chr(c) for c in m.middleware_custom_version)
os_custom_version_hex = ''.join(chr(c) for c in m.os_custom_version)
self.info.set_git_hash(flight_custom_version_hex)
logging_info(f"Flight Controller first 8 hex bytes of the FC git hash: {flight_custom_version_hex}")
# logging_info(f"Flight Controller first 8 hex bytes of the MW git hash: {middleware_custom_version_hex}")
self.info.set_os_git_hash(os_custom_version_hex)
logging_info(f"Flight Controller first 8 hex bytes of the ChibiOS git hash: {os_custom_version_hex}")
if m.vendor_id == 0x1209 and m.product_id == 0x5740:
return "" # these are just generic ArduPilot values, there is no value in printing them
pid_vid_dict = self.__list_ardupilot_supported_usb_pid_vid()
if m.vendor_id in pid_vid_dict:
self.info.set_vendor(m.vendor_id, pid_vid_dict[m.vendor_id]['vendor'])
logging_info(f"Flight Controller board vendor: {pid_vid_dict[m.vendor_id]['vendor']}")
if m.product_id in pid_vid_dict[m.vendor_id]['PID']:
self.info.set_product(m.product_id, pid_vid_dict[m.vendor_id]['PID'][m.product_id])
logging_info(f"Flight Controller board product: {pid_vid_dict[m.vendor_id]['PID'][m.product_id]}")
else:
self.info.set_product(m.product_id, "")
logging_info(f"Flight Controller board product ID: 0x{hex(m.product_id)}")
else:
self.info.set_vendor(m.vendor_id, "")
logging_info(f"Flight Controller board vendor ID: 0x{hex(m.vendor_id)}")
self.info.set_product(m.product_id, "")
logging_info(f"Flight Controller product ID: 0x{hex(m.product_id)}")
logging_info(f"Flight Controller HW / board version: {self.info.hardware_version}")
self.info.set_git_hash(m.flight_custom_version)
logging_info(f"Flight Controller first 8 hex bytes of the FC git hash: {self.info.git_hash}")
self.info.set_os_git_hash(m.os_custom_version)
logging_info(f"Flight Controller first 8 hex bytes of the ChibiOS git hash: {self.info.os_git_hash}")

self.info.set_vendor_and_product(m.vendor_id, m.product_id)
logging_info(f"Flight Controller board vendor ID: {self.info.vendor}")
logging_info(f"Flight Controller product ID: {self.info.product}")
# logging_info(f"Flight Controller UID if provided by hardware: {m.uid}")
except (ConnectionError, SerialException, PermissionError, ConnectionRefusedError) as e:
logging_warning("Connection failed: %s", e)
Expand Down Expand Up @@ -502,171 +470,6 @@ def get_connection_tuples(self):
"""
return self.__connection_tuples

@staticmethod
def __list_ardupilot_supported_usb_pid_vid():
"""
List all ArduPilot supported USB vendor ID (VID) and product ID (PID).
source: https://ardupilot.org/dev/docs/USB-IDs.html
"""
return {
0x0483: {'vendor': 'ST Microelectronics', 'PID': {0x5740: 'ChibiOS'}},
0x1209: {'vendor': 'ArduPilot', 'PID': {0x5740: 'MAVLink',
0x5741: 'Bootloader',
}
},
0x16D0: {'vendor': 'ArduPilot', 'PID': {0x0E65: 'MAVLink'}},
0x26AC: {'vendor': '3D Robotics', 'PID': {}},
0x2DAE: {'vendor': 'Hex', 'PID': {0x1101: 'CubeBlack+',
0x1001: 'CubeBlack bootloader',
0x1011: 'CubeBlack',
0x1016: 'CubeOrange',
0x1005: 'CubePurple bootloader',
0x1015: 'CubePurple',
0x1002: 'CubeYellow bootloader',
0x1012: 'CubeYellow',
0x1003: 'CubeBlue bootloader',
0x1013: 'CubeBlue', # These where detected by microsoft copilot
0x1004: 'CubeGreen bootloader',
0x1014: 'CubeGreen',
0x1006: 'CubeRed bootloader',
0x1017: 'CubeRed',
0x1007: 'CubeOrange bootloader',
0x1018: 'CubeOrange',
0x1008: 'CubePurple bootloader',
0x1019: 'CubePurple',
}
},
0x3162: {'vendor': 'Holybro', 'PID': {0x004B: 'Durandal'}},
0x27AC: {'vendor': 'Laser Navigation', 'PID': {0x1151: 'VRBrain-v51',
0x1152: 'VRBrain-v52',
0x1154: 'VRBrain-v54',
0x1910: 'VRCore-v10',
0x1351: 'VRUBrain-v51',
}
},
}

@staticmethod
def __decode_flight_sw_version(flight_sw_version):
'''decode 32 bit flight_sw_version mavlink parameter
corresponds to ArduPilot encoding in GCS_MAVLINK::send_autopilot_version'''
fw_type_id = (flight_sw_version >> 0) % 256 # noqa E221, E222
patch = (flight_sw_version >> 8) % 256 # noqa E221, E222
minor = (flight_sw_version >> 16) % 256 # noqa E221
major = (flight_sw_version >> 24) % 256 # noqa E221
if fw_type_id == 0:
fw_type = "dev"
elif fw_type_id == 64:
fw_type = "alpha"
elif fw_type_id == 128:
fw_type = "beta"
elif fw_type_id == 192:
fw_type = "rc"
elif fw_type_id == 255:
fw_type = "official"
else:
fw_type = "undefined"
return major, minor, patch, fw_type


@staticmethod
def __decode_flight_capabilities(capabilities):
'''Decode 32 bit flight controller capabilities bitmask mavlink parameter.
Returns a list of concise English descriptions of each active capability.
'''
# Initialize an empty list to store the descriptions
descriptions = []

# Iterate through each bit in the capabilities bitmask
for bit in range(32):
# Check if the bit is set
if capabilities & (1 << bit):
# Use the bit value to get the corresponding capability enum
capability = mavutil.mavlink.enums["MAV_PROTOCOL_CAPABILITY"].get(1 << bit, "Unknown capability")
# Append the description of the capability to the list
logging_info(capability.description)
descriptions.append(capability.name)

return descriptions


# see for more info:
# import pymavlink.dialects.v20.ardupilotmega
# pymavlink.dialects.v20.ardupilotmega.enums["MAV_TYPE"]
@staticmethod
def __decode_mav_type(mav_type):
return mavutil.mavlink.enums["MAV_TYPE"].get(mav_type,
mavutil.mavlink.EnumEntry("None", "Unknown type")).description


@staticmethod
def __decode_mav_autopilot(mav_autopilot):
return mavutil.mavlink.enums["MAV_AUTOPILOT"].get(mav_autopilot,
mavutil.mavlink.EnumEntry("None", "Unknown type")).description


@staticmethod
def __classify_vehicle_type(mav_type_int):
"""
Classify the vehicle type based on the MAV_TYPE enum.
Parameters:
mav_type_int (int): The MAV_TYPE enum value.
Returns:
str: The classified vehicle type.
"""
# Define the mapping from MAV_TYPE_* integer to vehicle type category
mav_type_to_vehicle_type = {
mavutil.mavlink.MAV_TYPE_FIXED_WING: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_QUADROTOR: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_COAXIAL: 'Heli',
mavutil.mavlink.MAV_TYPE_HELICOPTER: 'Heli',
mavutil.mavlink.MAV_TYPE_ANTENNA_TRACKER: 'AntennaTracker',
mavutil.mavlink.MAV_TYPE_GCS: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_AIRSHIP: 'ArduBlimp',
mavutil.mavlink.MAV_TYPE_FREE_BALLOON: 'ArduBlimp',
mavutil.mavlink.MAV_TYPE_ROCKET: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_GROUND_ROVER: 'Rover',
mavutil.mavlink.MAV_TYPE_SURFACE_BOAT: 'Rover',
mavutil.mavlink.MAV_TYPE_SUBMARINE: 'ArduSub',
mavutil.mavlink.MAV_TYPE_HEXAROTOR: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_OCTOROTOR: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_TRICOPTER: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_FLAPPING_WING: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_KITE: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_ONBOARD_CONTROLLER: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_VTOL_DUOROTOR: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_VTOL_QUADROTOR: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_VTOL_TILTROTOR: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_VTOL_RESERVED2: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_VTOL_RESERVED3: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_VTOL_RESERVED4: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_VTOL_RESERVED5: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_GIMBAL: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_ADSB: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_PARAFOIL: 'ArduPlane',
mavutil.mavlink.MAV_TYPE_DODECAROTOR: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_CAMERA: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_CHARGING_STATION: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_FLARM: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_SERVO: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_ODID: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_DECAROTOR: 'ArduCopter',
mavutil.mavlink.MAV_TYPE_BATTERY: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_PARACHUTE: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_LOG: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_OSD: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_IMU: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_GPS: 'AP_Periph',
mavutil.mavlink.MAV_TYPE_WINCH: 'AP_Periph',
# Add more mappings as needed
}

# Return the classified vehicle type based on the MAV_TYPE enum
return mav_type_to_vehicle_type.get(mav_type_int, None)

@staticmethod
def add_argparse_arguments(parser):
parser.add_argument('--device',
Expand Down
Loading

0 comments on commit 837330a

Please sign in to comment.