Implement auto mode for automatic flight

Use arm/disarm gestures
Add arm/disarm commands
Add ratesExtra variable for 
Rename interpretRC to interpretControls
Rename controlRate to controlRates
Remove USER mode
Add invalidate methods for vector and quaternion
Add valid/invalid method for vector and quaternion
Add valid/invalid function
Print armed in rc command
Pass auto mode to heartbeat
Use actuator_control_target for motors

flix/cli.ino

@@ -12,6 +12,7 @@ extern float loopRate, dt;
 extern double t;
 extern uint16_t channels[16];
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern bool armed;
 
 const char* motd =
 "\nWelcome to\n"
@@ -31,6 +32,8 @@ const char* motd =
 "ps - show pitch/roll/yaw\n"
 "psq - show attitude quaternion\n"
 "imu - show IMU data\n"
+"arm - arm the drone (when no mode switch)\n"
+"disarm - disarm the drone (when no mode switch)\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
@@ -109,6 +112,10 @@ void doCommand(String str, bool echo = false) {
 		printIMUCalibration();
 		print("rate: %.0f\n", loopRate);
 		print("landed: %d\n", landed);
+	} else if (command == "arm") {
+		armed = true;
+	} else if (command == "disarm") {
+		armed = false;
 	} else if (command == "rc") {
 		print("channels: ");
 		for (int i = 0; i < 16; i++) {

flix/control.ino

@@ -34,8 +34,7 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-enum { MANUAL, ACRO, STAB, USER } mode = STAB;
+enum { MANUAL, ACRO, STAB, AUTO } mode = STAB;
-enum { YAW, YAW_RATE } yawMode = YAW;
 bool armed = false;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -49,6 +48,7 @@ float tiltMax = TILT_MAX;
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
 Vector torqueTarget;
 float thrustTarget;
 
@@ -56,63 +56,50 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
 
 void control() {
-	interpretRC();
+	interpretControls();
 	failsafe();
-	if (mode == STAB) {
+	controlAttitude();
-		controlAttitude();
+	controlRates();
-		controlRate();
+	controlTorque();
-		controlTorque();
-	} else if (mode == ACRO) {
-		controlRate();
-		controlTorque();
-	} else if (mode == MANUAL) {
-		controlTorque();
-	}
 }
 
-void interpretRC() {
+void interpretControls() {
-	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
-
 	// NOTE: put ACRO or MANUAL modes there if you want to use them
-	if (controlMode < 0.25) {
+	if (controlMode < 0.25) mode = STAB;
-		mode = STAB;
+	if (controlMode < 0.75) mode = STAB;
-	} else if (controlMode < 0.75) {
+	if (controlMode > 0.75) mode = AUTO;
-		mode = STAB;
+	if (controlArmed < 0.5) armed = false;
-	} else {
+
-		mode = STAB;
+	if (mode == AUTO) return; // pilot is not effective in AUTO mode
-	}
+
+	if (landed && controlThrottle == 0 && controlYaw > 0.95) armed = true; // arm gesture
+	if (landed && controlThrottle == 0 && controlYaw < -0.95) armed = false; // disarm gesture
 
 	thrustTarget = controlThrottle;
 
-	if (mode == ACRO) {
+	if (mode == STAB) {
-		yawMode = YAW_RATE;
+		float yawTarget = attitudeTarget.getYaw();
-		ratesTarget.x = controlRoll * maxRate.x;
+		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or yaw rate is set
-		ratesTarget.y = controlPitch* maxRate.y;
+		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
-		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
-
-	} else if (mode == STAB) {
-		yawMode = controlYaw == 0 ? YAW : YAW_RATE;
-
-		attitudeTarget = Quaternion::fromEuler(Vector(
-			controlRoll * tiltMax,
-			controlPitch * tiltMax,
-			attitudeTarget.getYaw()));
-		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
-
-	} else if (mode == MANUAL) {
-		// passthrough mode
-		yawMode = YAW_RATE;
-		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 
-	if (yawMode == YAW_RATE || !motorsActive()) {
+	if (mode == ACRO) {
-		// update yaw target as we don't have control over the yaw
+		attitudeTarget.invalidate();
-		attitudeTarget.setYaw(attitude.getYaw());
+		ratesTarget.x = controlRoll * maxRate.x;
+		ratesTarget.y = controlPitch * maxRate.y;
+		ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
+	}
+
+	if (mode == MANUAL) { // passthrough mode
+		attitudeTarget.invalidate();
+		ratesTarget.invalidate();
+		torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
 	}
 }
 
 void controlAttitude() {
-	if (!armed) {
+	if (!armed || attitudeTarget.invalid()) { // skip attitude control
 		rollPID.reset();
 		pitchPID.reset();
 		yawPID.reset();
@@ -125,17 +112,15 @@ void controlAttitude() {
 
 	Vector error = Vector::rotationVectorBetween(upTarget, upActual);
 
-	ratesTarget.x = rollPID.update(error.x, dt);
+	ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
-	ratesTarget.y = pitchPID.update(error.y, dt);
+	ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
 
-	if (yawMode == YAW) {
+	float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
-		float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
+	ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
-		ratesTarget.z = yawPID.update(yawError, dt);
-	}
 }
 
-void controlRate() {
+void controlRates() {
-	if (!armed) {
+	if (!armed || ratesTarget.invalid()) { // skip rates control
 		rollRatePID.reset();
 		pitchRatePID.reset();
 		yawRatePID.reset();
@@ -151,7 +136,9 @@ void controlRate() {
 }
 
 void controlTorque() {
-	if (!armed) {
+	if (!torqueTarget.valid()) return; // skip torque control
+
+	if (!armed || thrustTarget < 0.05) {
 		memset(motors, 0, sizeof(motors));
 		return;
 	}
@@ -172,7 +159,7 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
-		case USER: return "USER";
+		case AUTO: return "AUTO";
 		default: return "UNKNOWN";
 	}
 }

flix/failsafe.ino

@@ -10,32 +10,40 @@ extern double controlTime;
 extern float controlRoll, controlPitch, controlThrottle, controlYaw;
 
 void failsafe() {
-	armingFailsafe();
 	rcLossFailsafe();
-}
+	autoFailsafe();
-
-// Prevent arming without zero throttle input
-void armingFailsafe() {
-	static double zeroThrottleTime;
-	static double armingTime;
-	if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
-	if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime;
-	if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
 }
 
 // RC loss failsafe
 void rcLossFailsafe() {
+	if (mode == AUTO) return;
+	if (!armed) return;
 	if (t - controlTime > RC_LOSS_TIMEOUT) {
 		descend();
 	}
 }
 
+// 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) {
+		if (mode == AUTO && !isfinite(controlMode)) {
+			print("Failsafe: regain control to pilot\n");
+			mode = STAB; // regain control to the pilot
+		}
+	}
+
+	roll = controlRoll;
+	pitch = controlPitch;
+	yaw = controlYaw;
+	throttle = controlThrottle;
+}
+
 // Smooth descend on RC lost
 void descend() {
-	mode = STAB;
+	mode = AUTO;
-	controlRoll = 0;
+	thrustTarget -= dt / DESCEND_TIME;
-	controlPitch = 0;
+	if (thrustTarget < 0) thrustTarget = 0;
-	controlYaw = 0;
+	if (thrustTarget == 0) armed = false;
-	controlThrottle -= dt / DESCEND_TIME;
-	if (controlThrottle < 0) controlThrottle = 0;
 }

flix/flix.ino

@@ -12,7 +12,8 @@
 
 double t = NAN; // current step time, s
 float dt; // time delta from previous step, s
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
+float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
+float controlArmed = NAN, controlMode = NAN;
 Vector gyro; // gyroscope data
 Vector acc; // accelerometer data, m/s/s
 Vector rates; // filtered angular rates, rad/s

flix/mavlink.ino

@@ -36,7 +36,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) |
+			(mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED |
+			((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
 			mode, MAV_STATE_STANDBY);
 		sendMessage(&msg);
 
@@ -57,9 +59,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];
+		float controls[8];
-		memcpy(actuator, motors, sizeof(motors));
+		memcpy(controls, motors, sizeof(motors));
-		mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
+		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,
@@ -102,8 +104,8 @@ void handleMavlink(const void *_msg) {
 		controlPitch = m.x / 1000.0f * mavlinkControlScale;
 		controlRoll = m.y / 1000.0f * mavlinkControlScale;
 		controlYaw = m.r / 1000.0f * mavlinkControlScale;
-		controlMode = 1; // STAB mode
+		controlMode = NAN; // keep mode
-		controlArmed = 1; // armed
+		controlArmed = NAN;
 		controlTime = t;
 
 		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
@@ -174,6 +176,39 @@ 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;
+
+		memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
+	}
+
 	// Handle commands
 	if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 		mavlink_command_long_t m;
@@ -188,8 +223,12 @@ void handleMavlink(const void *_msg) {
 			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, m.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
+
+		if (m.command == MAV_CMD_DO_SET_MODE) {
+			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return;
+			mode = static_cast<decltype(mode)>(m.param2);
+			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);
 		}
 	}

flix/quaternion.h

@@ -64,6 +64,21 @@ public:
 		return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
+	bool valid() const {
+		return finite();
+	}
+
+	bool invalid() const {
+		return !valid();
+	}
+
+	void invalidate() {
+		w = NAN;
+		x = NAN;
+		y = NAN;
+		z = NAN;
+	}
+
 	float norm() const {
 		return sqrt(w * w + x * x + y * y + z * z);
 	}

flix/vector.h

@@ -21,6 +21,20 @@ public:
 		return isfinite(x) && isfinite(y) && isfinite(z);
 	}
 
+	bool valid() const {
+		return finite();
+	}
+
+	bool invalid() const {
+		return !valid();
+	}
+
+	void invalidate() {
+		x = NAN;
+		y = NAN;
+		z = NAN;
+	}
+
 	float norm() const {
 		return sqrt(x * x + y * y + z * z);
 	}

gazebo/flix.h

@@ -15,7 +15,8 @@
 double t = NAN;
 float dt;
 float motors[4];
-float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
+float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
+float controlArmed = NAN, controlMode = NAN;
 Vector acc;
 Vector gyro;
 Vector rates;
@@ -28,9 +29,9 @@ void computeLoopRate();
 void applyGyro();
 void applyAcc();
 void control();
-void interpretRC();
+void interpretControls();
 void controlAttitude();
-void controlRate();
+void controlRates();
 void controlTorque();
 const char* getModeName();
 void sendMotors();
@@ -54,8 +55,8 @@ void mavlinkPrint(const char* str);
 void sendMavlinkPrint();
 inline Quaternion fluToFrd(const Quaternion &q);
 void failsafe();
-void armingFailsafe();
 void rcLossFailsafe();
+void autoFailsafe();
 void descend();
 int parametersCount();
 const char *getParameterName(int index);

tools/pyflix/flix.py

@@ -56,7 +56,7 @@ class Flix:
             if e.errno != errno.EADDRINUSE:
                 raise
             # Port busy - using proxy
-            logger.debug('Listening on port 14560 (proxy)')
+            logger.debug('Listening on port 14555 (proxy)')
             self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254)  # type: ignore
         self.connection.target_system = system_id
         self.mavlink: mavlink.MAVLink = self.connection.mav
@@ -169,8 +169,8 @@ class Flix:
                              msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]
             self._trigger('channels', self.channels)
 
-        if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
+        if isinstance(msg, mavlink.MAVLink_actuator_control_target_message):
-            self.motors = msg.actuator[:4]  # type: ignore
+            self.motors = msg.controls[:4]  # type: ignore
             self._trigger('motors', self.motors)
 
         if isinstance(msg, mavlink.MAVLink_scaled_imu_message):