Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Move mavlink commands handlers to a separated function
Oleg Kalachev5d670dcc69c1
diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex be90f9a..6c15f1f 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -245,38 +245,42 @@ void handleMavlink(const void *_msg) {}}- // Handle commandsif (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 GCSdiff --git a/gazebo/flix.h b/gazebo/flix.hindex 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__.pyindex 67ad6a7..411ab97 100644--- a/tools/pyflix/__init__.py+++ b/tools/pyflix/__init__.py@@ -1 +1 @@-from .flix import Flix+from .flix import Flix, mavlinkdiff --git a/tools/pyflix/flix.py b/tools/pyflix/flix.pyindex 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}')returnexcept TimeoutError:continuediff --git a/tools/test.py b/tools/test.pyindex 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, raisesfrom math import isnan, isfiniteimport time-from pyflix import Flix+from pyflix import Flix, mavlinkdef 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