From 83a8dcd63e998e160471de1387f8f25e47cfd482 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 24 Feb 2025 13:06:38 +0300 Subject: [PATCH] Cleanup mavlink subsystem code --- flix/mavlink.ino | 78 ++++++++++++++++++++++++------------------------ 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 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; + const 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; + 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 mode controls[armedChannel] = 1; // armed controlsTime = 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 ack mavlink_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 unknown sendMessage(&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 happy mavlink_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-terminated doCommand(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); } }