Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Changes to method for sending mavlink commands in pymavlink Make the exception names more verbose. Make it public.
Oleg Kalacheva2dd70654c54
diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.pyindex 8321d67..6241ab3 100644--- a/tools/pyflix/flix.py+++ b/tools/pyflix/flix.py@@ -255,21 +255,6 @@ class Flix:def _flu_to_mavlink(v: Sequence[float]) -> List[float]:return Flix._mavlink_to_flu(v) # flu to mavlink is the same as mavlink to flu- def _command_send(self, command: int, params: Sequence[float]):- if len(params) != 7:- raise ValueError('Command must have 7 parameters')- for attempt in range(3):- try:- logger.debug(f'Send command {command} with params {params} (attempt #{attempt + 1})')- self.mavlink.command_long_send(self.system_id, 0, command, 0, *params) # type: ignore- ack = self.wait('mavlink.COMMAND_ACK', value=lambda msg: msg.command == command, timeout=0.1)- if ack.result != mavlink.MAV_RESULT_ACCEPTED:- raise RuntimeError(f'Command {command} failed with result {ack.result}')- return- except TimeoutError:- continue- raise RuntimeError(f'Failed to send command {command} after 3 attempts')-def _connected(self):# Reset disconnection timerself._disconnected_timer.cancel()@@ -286,6 +271,23 @@ class Flix:self.connected = Falseself._trigger('disconnected')+ def send_command(self, command: int, params: Sequence[float]):+ if len(params) != 7:+ raise ValueError('Command must have 7 parameters')+ for attempt in range(3):+ try:+ logger.debug(f'Send command {command} with params {params} (attempt #{attempt + 1})')+ self.mavlink.command_long_send(self.system_id, 0, command, 0, *params) # type: ignore+ ack: mavlink.MAVLink_command_ack_message = self.wait('mavlink.COMMAND_ACK', value=lambda msg: msg.command == command, timeout=0.1)+ if ack.result != mavlink.MAV_RESULT_ACCEPTED:+ name = getattr(mavlink.enums['MAV_CMD'].get(command, {}), 'name', f'UNKNOWN({command})')+ result = getattr(mavlink.enums['MAV_RESULT'].get(ack.result, {}), 'name', f'UNKNOWN({ack.result})')+ raise RuntimeError(f'Command {name} failed with result {result}')+ return+ except TimeoutError:+ continue+ raise RuntimeError(f'Failed to send command {command} after 3 attempts')+def get_param(self, name: str) -> float:if len(name.encode('ascii')) > 16:raise ValueError('Parameter name must be 16 characters or less')@@ -317,10 +319,10 @@ class Flix:def set_mode(self, mode: Union[str, int]):if isinstance(mode, str):mode = self._modes.index(mode.upper())- self._command_send(mavlink.MAV_CMD_DO_SET_MODE, (0, mode, 0, 0, 0, 0, 0))+ self.send_command(mavlink.MAV_CMD_DO_SET_MODE, (0, mode, 0, 0, 0, 0, 0))def set_armed(self, armed: bool):- self._command_send(mavlink.MAV_CMD_COMPONENT_ARM_DISARM, (1 if armed else 0, 0, 0, 0, 0, 0, 0))+ self.send_command(mavlink.MAV_CMD_COMPONENT_ARM_DISARM, (1 if armed else 0, 0, 0, 0, 0, 0, 0))def set_position(self, position: Sequence[float], yaw: Optional[float] = None, wait: bool = False, tolerance: float = 0.1):raise NotImplementedError('Position control is not implemented yet')diff --git a/tools/test.py b/tools/test.pyindex dca8046..582d92d 100755--- a/tools/test.py+++ b/tools/test.py@@ -46,5 +46,8 @@ def test():flix.set_mode('AUTO')flix.wait('mode', 'AUTO')- raises(RuntimeError, lambda: flix._command_send(mavlink.MAV_CMD_DO_SET_MODE, [0, 99, 0, 0, 0, 0, 0])) # invalid mode- raises(RuntimeError, lambda: flix._command_send(mavlink.MAV_CMD_DO_PARACHUTE, [0, 0, 0, 0, 0, 0, 0])) # unsupported command+ print("=== Check command errors")+ with raises(RuntimeError, match='MAV_RESULT_DENIED'):+ flix.send_command(mavlink.MAV_CMD_DO_SET_MODE, [0, 99, 0, 0, 0, 0, 0]) # invalid mode+ with raises(RuntimeError, match='MAV_RESULT_UNSUPPORTED'):+ flix.send_command(mavlink.MAV_CMD_DO_PARACHUTE, [0, 0, 0, 0, 0, 0, 0]) # unsupported command