Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Move mavlink commands handlers to a separated function

Oleg Kalachev5d670dcc69c1
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index be90f9a..6c15f1f 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -245,38 +245,42 @@ void handleMavlink(const void *_msg) {
}
}
- // Handle commands
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t m;
mavlink_msg_command_long_decode(&msg, &m);
if (m.target_system && m.target_system != mavlinkSysId) return;
- mavlink_message_t response;
- bool accepted = false;
- if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
- accepted = true;
- mavlink_msg_autopilot_version_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
- MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
- sendMessage(&response);
- }
+ int result = handleMavlinkCommand(&m);
+ mavlink_message_t ack;
+ mavlink_msg_command_ack_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, result, UINT8_MAX, 0, msg.sysid, msg.compid);
+ sendMessage(&ack);
+ }
+}
- if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
- if (m.param1 == 1 && controlThrottle > 0.05) return; // don't arm if throttle is not low
- accepted = true;
- armed = m.param1 == 1;
- }
+int handleMavlinkCommand(const void *_m) {
+ const mavlink_command_long_t& m = *(mavlink_command_long_t *)_m;
- if (m.command == MAV_CMD_DO_SET_MODE) {
- if (m.param2 < 0 || m.param2 > AUTO) return; // incorrect mode
- accepted = true;
- mode = m.param2;
- }
+ if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
+ mavlink_message_t response;
+ mavlink_msg_autopilot_version_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &response,
+ MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
+ sendMessage(&response);
+ return MAV_RESULT_ACCEPTED;
+ }
- // send command ack
- mavlink_message_t ack;
- mavlink_msg_command_ack_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
- sendMessage(&ack);
+ if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+ if (m.param1 == 1 && controlThrottle > 0.05) return MAV_RESULT_DENIED; // don't arm if throttle is not low
+ armed = m.param1 == 1;
+ return MAV_RESULT_ACCEPTED;
}
+
+ if (m.command == MAV_CMD_DO_SET_MODE) {
+ if (m.param2 < 0 || m.param2 > AUTO) return MAV_RESULT_DENIED; // incorrect mode
+ mode = m.param2;
+ return MAV_RESULT_ACCEPTED;
+ }
+
+ return MAV_RESULT_UNSUPPORTED;
}
// Send shell output to GCS
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 351d371..d3034d2 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -58,6 +58,7 @@ void sendMavlink();
void sendMessage(const void *msg);
void receiveMavlink();
void handleMavlink(const void *_msg);
+int handleMavlinkCommand(const void *_m);
void mavlinkPrint(const char* str);
void sendMavlinkPrint();
inline Quaternion fluToFrd(const Quaternion &q);
diff --git a/tools/pyflix/__init__.py b/tools/pyflix/__init__.py
index 67ad6a7..411ab97 100644
--- a/tools/pyflix/__init__.py
+++ b/tools/pyflix/__init__.py
@@ -1 +1 @@
-from .flix import Flix
+from .flix import Flix, mavlink
diff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.py
index c9b3b1d..8321d67 100644
--- a/tools/pyflix/flix.py
+++ b/tools/pyflix/flix.py
@@ -262,7 +262,9 @@ class Flix:
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
- self.wait('mavlink.COMMAND_ACK', value=lambda msg: msg.command == command and msg.result == mavlink.MAV_RESULT_ACCEPTED, timeout=0.1)
+ 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
diff --git a/tools/test.py b/tools/test.py
index c23e4c4..dca8046 100755
--- a/tools/test.py
+++ b/tools/test.py
@@ -2,10 +2,10 @@
# Script for testing pyflix and the simulation.
-from pytest import approx
+from pytest import approx, raises
from math import isnan, isfinite
import time
-from pyflix import Flix
+from pyflix import Flix, mavlink
def test():
print('=== Connect...')
@@ -45,3 +45,6 @@ def test():
flix.wait('mode', 'ACRO')
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