Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Support arm/disarm mavlink commands Refactor commands handling to remove repeating ack message packing.

Oleg Kalachevd47d7b8bd4a8
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index fe1f188..6f76711 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -177,19 +177,25 @@ void handleMavlink(const void *_msg) {
mavlink_command_long_t m;
mavlink_msg_command_long_decode(&msg, &m);
if (m.target_system && m.target_system != SYSTEM_ID) return;
- mavlink_message_t ack;
mavlink_message_t response;
+ bool accepted = false;
if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
- mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid);
- sendMessage(&ack);
+ accepted = true;
mavlink_msg_autopilot_version_pack(SYSTEM_ID, 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);
- } else {
- mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
- sendMessage(&ack);
}
+
+ if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+ accepted = true;
+ armed = m.param1 == 1;
+ }
+
+ // send command ack
+ mavlink_message_t ack;
+ mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
+ sendMessage(&ack);
}
}