aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 09:39:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Cleanup mavlink subsystem code

Browse Source
This commit is contained in:
Oleg Kalachev 2025-02-24 13:06:38 +03:00
parent c62e536b50
commit 83a8dcd63e
1 changed files with 39 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
flix/mavlink.ino
View File

@ -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, &paramRequestRead);
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, &paramSet);
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);
}
}