aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-02-16 23:21:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add parameters for configuring the mavlink subsystem

Browse Source 
MAV_SYS_ID - mavlink system id.
MAV_RATE_SLOW - rate of slow telemetry (e. g. heartbeats).
MAV_RATE_FAST - rate of fast telemetry (e. g. attitude, imu data).
This commit is contained in:
Oleg Kalachev
2026-01-22 23:04:45 +03:00
parent dd3575174b
commit 30326a5662
2 changed files with 33 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
flix/mavlink.ino
View File

@@ -6,14 +6,13 @@
#include <MAVLink.h> #include <MAVLink.h>
#include "util.h" #include "util.h"
#define SYSTEM_ID 1
#define MAVLINK_RATE_SLOW 1
#define MAVLINK_RATE_FAST 10
extern float controlTime; extern float controlTime;
bool mavlinkConnected = false; bool mavlinkConnected = false;
String mavlinkPrintBuffer; String mavlinkPrintBuffer;
int mavlinkSysId = 1;
Rate telemetryFast(10);
Rate telemetrySlow(2);
void processMavlink() { void processMavlink() {
sendMavlink(); sendMavlink();
@@ -26,10 +25,8 @@ void sendMavlink() {
mavlink_message_t msg; mavlink_message_t msg;
uint32_t time = t * 1000; uint32_t time = t * 1000;
static Rate slow(MAVLINK_RATE_SLOW), fast(MAVLINK_RATE_FAST); if (telemetrySlow) {
mavlink_msg_heartbeat_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
if (slow) {
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
(armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) | (armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0) |
((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) | ((mode == STAB) ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0) |
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED), ((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 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); MAV_VTOL_STATE_UNDEFINED, landed ? MAV_LANDED_STATE_ON_GROUND : MAV_LANDED_STATE_IN_AIR);
sendMessage(&msg); sendMessage(&msg);
} }
if (fast && mavlinkConnected) { if (telemetryFast && mavlinkConnected) {
const float zeroQuat[] = {0, 0, 0, 0}; 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 time, attitude.w, attitude.x, -attitude.y, -attitude.z, rates.x, -rates.y, -rates.z, zeroQuat); // convert to frd
sendMessage(&msg); 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); 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 if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
float controls[8]; float controls[8];
memcpy(controls, motors, sizeof(motors)); 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); 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 acc.x * 1000, -acc.y * 1000, -acc.z * 1000, // convert to frd
gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000, gyro.x * 1000, -gyro.y * 1000, -gyro.z * 1000,
0, 0, 0, 0); 0, 0, 0, 0);
@@ -93,7 +90,7 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t m; mavlink_manual_control_t m;
mavlink_msg_manual_control_decode(&msg, &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; controlThrottle = m.z / 1000.0f;
controlPitch = m.x / 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) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
mavlink_param_request_list_t m; mavlink_param_request_list_t m;
mavlink_msg_param_request_list_decode(&msg, &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; mavlink_message_t msg;
for (int i = 0; i < parametersCount(); i++) { 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); getParameterName(i), getParameter(i), MAV_PARAM_TYPE_REAL32, parametersCount(), i);
sendMessage(&msg); sendMessage(&msg);
} }
@@ -119,7 +116,7 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
mavlink_param_request_read_t m; mavlink_param_request_read_t m;
mavlink_msg_param_request_read_decode(&msg, &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]; 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 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); memcpy(name, getParameterName(m.param_index), 16);
} }
mavlink_message_t msg; 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); name, value, MAV_PARAM_TYPE_REAL32, parametersCount(), m.param_index);
sendMessage(&msg); sendMessage(&msg);
} }
@@ -136,7 +133,7 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) { if (msg.msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t m; mavlink_param_set_t m;
mavlink_msg_param_set_decode(&msg, &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]; 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 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; if (!success) return;
// send ack // send ack
mavlink_message_t msg; 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 m.param_id, m.param_value, MAV_PARAM_TYPE_REAL32, parametersCount(), 0); // index is unknown
sendMessage(&msg); 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 if (msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { // handle to make qgc happy
mavlink_mission_request_list_t m; mavlink_mission_request_list_t m;
mavlink_msg_mission_request_list_decode(&msg, &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_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); sendMessage(&msg);
} }
if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) { if (msg.msgid == MAVLINK_MSG_ID_SERIAL_CONTROL) {
mavlink_serial_control_t m; mavlink_serial_control_t m;
mavlink_msg_serial_control_decode(&msg, &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]; char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
strlcpy(data, (const char *)m.data, m.count); // data might be not null-terminated 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_set_attitude_target_t m;
mavlink_msg_set_attitude_target_decode(&msg, &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 // copy attitude, rates and thrust targets
ratesTarget.x = m.body_roll_rate; ratesTarget.x = m.body_roll_rate;
@@ -196,7 +193,7 @@ void handleMavlink(const void *_msg) {
mavlink_set_actuator_control_target_t m; mavlink_set_actuator_control_target_t m;
mavlink_msg_set_actuator_control_target_decode(&msg, &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(); attitudeTarget.invalidate();
ratesTarget.invalidate(); ratesTarget.invalidate();
@@ -208,12 +205,12 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) { if (msg.msgid == MAVLINK_MSG_ID_LOG_REQUEST_DATA) {
mavlink_log_request_data_t m; mavlink_log_request_data_t m;
mavlink_msg_log_request_data_decode(&msg, &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 // Send all log records
for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) { for (int i = 0; i < sizeof(logBuffer) / sizeof(logBuffer[0]); i++) {
mavlink_message_t msg; 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]); sizeof(logBuffer[0]), (uint8_t *)logBuffer[i]);
sendMessage(&msg); sendMessage(&msg);
} }
@@ -223,13 +220,13 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) { if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t m; mavlink_command_long_t m;
mavlink_msg_command_long_decode(&msg, &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; mavlink_message_t response;
bool accepted = false; bool accepted = false;
if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) { if (m.command == MAV_CMD_REQUEST_MESSAGE && m.param1 == MAVLINK_MSG_ID_AUTOPILOT_VERSION) {
accepted = true; 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); MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
sendMessage(&response); sendMessage(&response);
} }
@@ -248,7 +245,7 @@ void handleMavlink(const void *_msg) {
// send command ack // send command ack
mavlink_message_t 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); sendMessage(&ack);
} }
} }
@@ -265,7 +262,7 @@ void sendMavlinkPrint() {
char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1]; char data[MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN + 1];
strlcpy(data, str + i, sizeof(data)); strlcpy(data, str + i, sizeof(data));
mavlink_message_t msg; 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, SERIAL_CONTROL_DEV_SHELL,
i + MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN < strlen(str) ? SERIAL_CONTROL_FLAG_MULTI : 0, // more chunks to go 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); 0, 0, strlen(data), (uint8_t *)data, 0, 0);

4
flix/parameters.ino
View File

@@ -87,6 +87,10 @@ Parameter parameters[] = {
{"WIFI_MODE", &wifiMode}, {"WIFI_MODE", &wifiMode},
{"WIFI_LOC_PORT", &udpLocalPort}, {"WIFI_LOC_PORT", &udpLocalPort},
{"WIFI_REM_PORT", &udpRemotePort}, {"WIFI_REM_PORT", &udpRemotePort},
// mavlink
{"MAV_SYS_ID", &mavlinkSysId},
{"MAV_RATE_SLOW", &telemetrySlow.rate},
{"MAV_RATE_FAST", &telemetryFast.rate},
}; };
void setupParameters() { void setupParameters() {