Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

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.py
index 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 timer
self._disconnected_timer.cancel()
@@ -286,6 +271,23 @@ class Flix:
self.connected = False
self._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.py
index 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