Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Cleanup mavlink subsystem code
Oleg Kalachev83a8dcd63e99
diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex cb0fca1..660d690 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -88,15 +88,15 @@ void receiveMavlink() {}void handleMavlink(const void *_msg) {- mavlink_message_t *msg = (mavlink_message_t *)_msg;-- if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {- mavlink_manual_control_t manualControl;- mavlink_msg_manual_control_decode(msg, &manualControl);- controls[throttleChannel] = manualControl.z / 1000.0f;- controls[pitchChannel] = manualControl.x / 1000.0f * mavlinkControlScale;- controls[rollChannel] = manualControl.y / 1000.0f * mavlinkControlScale;- controls[yawChannel] = manualControl.r / 1000.0f * mavlinkControlScale;+ const mavlink_message_t &msg = *(mavlink_message_t *)_msg;++ if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {+ mavlink_manual_control_t m;+ mavlink_msg_manual_control_decode(&msg, &m);+ controls[throttleChannel] = m.z / 1000.0f;+ controls[pitchChannel] = m.x / 1000.0f * mavlinkControlScale;+ controls[rollChannel] = m.y / 1000.0f * mavlinkControlScale;+ controls[yawChannel] = m.r / 1000.0f * mavlinkControlScale;controls[modeChannel] = 1; // STAB modecontrols[armedChannel] = 1; // armedcontrolsTime = t;@@ -104,72 +104,72 @@ void handleMavlink(const void *_msg) {if (abs(controls[yawChannel]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[yawChannel] = 0;}- if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {- mavlink_message_t msg;+ if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {+ mavlink_message_t m;for (int i = 0; i < parametersCount(); i++) {- mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,+ mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &m,getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);- sendMessage(&msg);+ sendMessage(&m);}}- if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {- mavlink_param_request_read_t paramRequestRead;- mavlink_msg_param_request_read_decode(msg, ¶mRequestRead);- char name[16 + 1];- strlcpy(name, paramRequestRead.param_id, sizeof(name)); // param_id might be not null-terminated- float value = strlen(name) == 0 ? getParameter(paramRequestRead.param_index) : getParameter(name);- if (paramRequestRead.param_index != -1) {- memcpy(name, getParameterName(paramRequestRead.param_index), 16);+ if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {+ mavlink_param_request_read_t m;+ mavlink_msg_param_request_read_decode(&msg, &m);+ char name[MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];+ strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated+ float value = strlen(name) == 0 ? getParameter(m.param_index) : getParameter(name);+ if (m.param_index != -1) {+ memcpy(name, getParameterName(m.param_index), 16);}mavlink_message_t msg;mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,- name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), paramRequestRead.param_index);+ name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);sendMessage(&msg);}- if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {- mavlink_param_set_t paramSet;- mavlink_msg_param_set_decode(msg, ¶mSet);- char name[16 + 1];- strlcpy(name, paramSet.param_id, sizeof(name)); // param_id might be not null-terminated- setParameter(name, paramSet.param_value);+ if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {+ mavlink_param_set_t m;+ mavlink_msg_param_set_decode(&msg, &m);+ char name[MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN + 1];+ strlcpy(name, m.param_id, sizeof(name)); // param_id might be not null-terminated+ setParameter(name, m.param_value);// send ackmavlink_message_t msg;mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,- paramSet.param_id, paramSet.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown+ m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknownsendMessage(&msg);}- if (msg->msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy+ if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happymavlink_message_t msg;mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0);sendMessage(&msg);}- if (msg->msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {- mavlink_serial_control_t serialControl;- mavlink_msg_serial_control_decode(msg, &serialControl);+ if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {+ mavlink_serial_control_t m;+ mavlink_msg_serial_control_decode(&msg, &m);char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];- strlcpy(data, (const char *)serialControl.data, serialControl.count); // data might be not null-terminated+ strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminateddoCommand(data, true);}// Handle commands- if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {- mavlink_command_long_t commandLong;- mavlink_msg_command_long_decode(msg, &commandLong);+ if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {+ mavlink_command_long_t m;+ mavlink_msg_command_long_decode(&msg, &m);mavlink_message_t ack;mavlink_message_t response;- if (commandLong.command == MAV_CMD_REQUEST_MESSAGE && commandLong.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {- mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, commandLong.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg->sysid, msg->compid);+ 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);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, commandLong.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg->sysid, msg->compid);+ 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);}}