Implement AUTO mode for automatic flights

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.

flix/cli.ino

@@ -8,7 +8,7 @@
 #include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
-extern const int ACRO, STAB;
+extern const int ACRO, STAB, AUTO;
 extern float loopRate, dt;
 extern double t;
 extern uint16_t channels[16];
@@ -36,7 +36,7 @@ const char* motd =
 "imu - show IMU data\n"
 "arm - arm the drone\n"
 "disarm - disarm the drone\n"
-"stab/acro - set mode\n"
+"stab/acro/auto - set mode\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
@@ -120,6 +120,8 @@ void doCommand(String str, bool echo = false) {
 		mode = STAB;
 	} else if (command == "acro") {
 		mode = ACRO;
+	} else if (command == "auto") {
+		mode = AUTO;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {

flix/control.ino

@@ -35,7 +35,7 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-const int MANUAL = 0, ACRO = 1, STAB = 2; // flight modes
+const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
 int mode = STAB;
 bool armed = false;
 
@@ -71,6 +71,8 @@ void interpretControls() {
 	if (controlMode < 0.75) mode = STAB;
 	if (controlMode > 0.75) mode = STAB;
 
+	if (mode == AUTO) return; // pilot is not effective in AUTO mode
+
 	if (controlThrottle < 0.05 && controlYaw > 0.95) armed = true; // arm gesture
 	if (controlThrottle < 0.05 && controlYaw < -0.95) armed = false; // disarm gesture
 
@@ -168,6 +170,7 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
+		case AUTO: return "AUTO";
 		default: return "UNKNOWN";
 	}
 }

flix/failsafe.ino

@@ -11,6 +11,7 @@ extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 void failsafe() {
 	rcLossFailsafe();
+	autoFailsafe();
 }
 
 // RC loss failsafe
@@ -32,3 +33,16 @@ void descend() {
 		controlThrottle = 0;
 	}
 }
+
+// Allow pilot to interrupt automatic flight
+void autoFailsafe() {
+	static float roll, pitch, yaw, throttle;
+	if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
+		// controls changed
+		if (mode == AUTO) mode = STAB; // regain control by the pilot
+	}
+	roll = controlRoll;
+	pitch = controlPitch;
+	yaw = controlYaw;
+	throttle = controlThrottle;
+}

flix/mavlink.ino

@@ -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);