aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-01-12 22:17:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Implement AUTO mode for automatic flights

Browse Source 
Support SET_ATTITUDE_TARGET, SET_ACTUATOR_CONTROL_TARGET in mavlink.
ACTUATOR_OUTPUT_STATUS is changed ACTUATOR_CONTROL_TARGET to match used message for setting motor outputs.
Add support for changing mode from mavlink.
Support automatic flights in pyflix.
This commit is contained in:
Oleg Kalachev
2025-08-28 00:49:24 +03:00
parent 10fafbc4a0
commit 40fc4b96b5
8 changed files with 181 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
flix/mavlink.ino
View File

@@ -35,7 +35,9 @@ void sendMavlink() {
lastSlow = t;
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
(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),
mode, MAV_STATE_STANDBY);
sendMessage(&msg);
@@ -56,9 +58,9 @@ void sendMavlink() {
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 actuator[32];
memcpy(actuator, motors, sizeof(motors));
mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
float controls[8];
memcpy(controls, motors, sizeof(motors));
mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
sendMessage(&msg);
mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
@@ -172,6 +174,42 @@ void handleMavlink(const void *_msg) {
doCommand(data, true);
}
if (msg.msgid == MAVLINK_MSG_ID_SET_ATTITUDE_TARGET) {
if (mode != AUTO) return;
mavlink_set_attitude_target_t m;
mavlink_msg_set_attitude_target_decode(&msg, &m);
if (m.target_system && m.target_system != SYSTEM_ID) return;
// copy attitude, rates and thrust targets
ratesTarget.x = m.body_roll_rate;
ratesTarget.y = -m.body_pitch_rate; // convert to flu
ratesTarget.z = -m.body_yaw_rate;
attitudeTarget.w = m.q[0];
attitudeTarget.x = m.q[1];
attitudeTarget.y = -m.q[2];
attitudeTarget.z = -m.q[3];
thrustTarget = m.thrust;
ratesExtra = Vector(0, 0, 0);
if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
armed = m.thrust > 0;
}
if (msg.msgid == MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET) {
if (mode != AUTO) return;
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;
attitudeTarget.invalidate();
ratesTarget.invalidate();
torqueTarget.invalidate();
memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
armed = motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
}
// Handle commands
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t m;
@@ -192,6 +230,12 @@ void handleMavlink(const void *_msg) {
armed = m.param1 == 1;
}
if (m.command == MAV_CMD_DO_SET_MODE) {
if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; // incorrect mode
accepted = true;
mode = m.param2;
}
// 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);