From 8a12d1fa70f8e7c9ddf10115c1bdc21898dc9a1c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 19 Jan 2026 01:28:43 +0300 Subject: [PATCH] Add parameters for mavlink subsystem System ID, fast telemetry rate, slow telemetry rate. --- flix/mavlink.ino | 61 +++++++++++++++++++++------------------------ flix/parameters.ino | 4 +++ 2 files changed, 33 insertions(+), 32 deletions(-) diff --git a/flix/mavlink.ino b/flix/mavlink.ino index f894497..15bbba5 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -6,14 +6,13 @@ #include #include "util.h" -#define SYSTEM_ID 1 -#define MAVLINK_RATE_SLOW 1 -#define MAVLINK_RATE_FAST 10 - extern float controlTime; bool mavlinkConnected = false; String mavlinkPrintBuffer; +int mavlinkSysId = 1; +Rate telemetryFast(10); +Rate telemetrySlow(2); void processMavlink() { sendMavlink(); @@ -26,10 +25,8 @@ void sendMavlink() { mavlink_message_t msg; uint32_t time = t * 1000; - static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST); - - if (slow) { - mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, + if (telemetrySlow) { + mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, (armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) | ((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) | ((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), @@ -38,27 +35,27 @@ void sendMavlink() { if (!mavlinkConnected) return; // send only heartbeat until connected - mavlink_msg_extended_sys_state_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + mavlink_msg_extended_sys_state_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR); sendMessage(&msg); } - if (fast && mavlinkConnected) { + if (telemetryFast && mavlinkConnected) { const float zeroQuat[] = {0, 0, 0, 0}; - mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + mavlink_msg_attitude_quaternion_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd sendMessage(&msg); - mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0, + mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0, channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX); if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input float controls[8]; memcpy(controls, motors, sizeof(motors)); - mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls); + mavlink_msg_actuator_control_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls); sendMessage(&msg); - mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, + mavlink_msg_scaled_imu_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000, 0, 0, 0, 0); @@ -93,7 +90,7 @@ void handleMavlink(const void *_msg) { if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { mavlink_manual_control_t m; mavlink_msg_manual_control_decode(&msg, &m); - if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast + if (m.target && m.target != mavlinkSysId) return; // 0 is broadcast controlThrottle = m.z / 1000.0f; controlPitch = m.x / 1000.0f; @@ -106,11 +103,11 @@ void handleMavlink(const void *_msg) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { mavlink_param_request_list_t m; mavlink_msg_param_request_list_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; mavlink_message_t msg; for (int i = 0; i < parametersCount(); i++) { - mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i); sendMessage(&msg); } @@ -119,7 +116,7 @@ void handleMavlink(const void *_msg) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { mavlink_param_request_read_t m; mavlink_msg_param_request_read_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; 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 @@ -128,7 +125,7 @@ void handleMavlink(const void *_msg) { memcpy(name, getParameterName(m.param_index), 16); } mavlink_message_t msg; - mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index); sendMessage(&msg); } @@ -136,7 +133,7 @@ void handleMavlink(const void *_msg) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t m; mavlink_msg_param_set_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; 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 @@ -144,7 +141,7 @@ void handleMavlink(const void *_msg) { if (!success) return; // send ack mavlink_message_t msg; - mavlink_msg_param_value_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + mavlink_msg_param_value_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown sendMessage(&msg); } @@ -152,17 +149,17 @@ void handleMavlink(const void *_msg) { if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy mavlink_mission_request_list_t m; mavlink_msg_mission_request_list_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; mavlink_message_t msg; - mavlink_msg_mission_count_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, 0, 0, MAV_MISSION_TYPE_MISSION, 0); + mavlink_msg_mission_count_pack(mavlinkSysId, 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 m; mavlink_msg_serial_control_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated @@ -174,7 +171,7 @@ void handleMavlink(const void *_msg) { mavlink_set_attitude_target_t m; mavlink_msg_set_attitude_target_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; // copy attitude, rates and thrust targets ratesTarget.x = m.body_roll_rate; @@ -196,7 +193,7 @@ void handleMavlink(const void *_msg) { mavlink_set_actuator_control_target_t m; mavlink_msg_set_actuator_control_target_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; attitudeTarget.invalidate(); ratesTarget.invalidate(); @@ -208,12 +205,12 @@ void handleMavlink(const void *_msg) { if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) { mavlink_log_request_data_t m; mavlink_msg_log_request_data_decode(&msg, &m); - if (m.target_system && m.target_system != SYSTEM_ID) return; + if (m.target_system && m.target_system != mavlinkSysId) return; // Send all log records for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) { mavlink_message_t msg; - mavlink_msg_log_data_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i, + mavlink_msg_log_data_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, 0, i, sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]); sendMessage(&msg); } @@ -223,13 +220,13 @@ void handleMavlink(const void *_msg) { 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 != SYSTEM_ID) return; + 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(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &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); } @@ -248,7 +245,7 @@ void handleMavlink(const void *_msg) { // 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); + 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); } } @@ -265,7 +262,7 @@ void sendMavlinkPrint() { char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; strlcpy(data, str + i, sizeof(data)); mavlink_message_t msg; - mavlink_msg_serial_control_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, + mavlink_msg_serial_control_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, SERIAL_CONTROL_DEV_SHELL, i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go 0, 0, strlen(data), (uint8_t *)data, 0, 0); diff --git a/flix/parameters.ino b/flix/parameters.ino index ae5e868..b251fb4 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -87,6 +87,10 @@ Parameter parameters[] = { {"WIFI_MODE", &wifiMode}, {"WIFI_LOC_PORT", &udpLocalPort}, {"WIFI_REM_PORT", &udpRemotePort}, + // mavlink + {"MAV_SYS_ID", &mavlinkSysId}, + {"MAV_RATE_SLOW", &telemetrySlow.rate}, + {"MAV_RATE_FAST", &telemetryFast.rate}, }; void setupParameters() {