Add commands for switching modes

Make mode simple int instead of enum for simplify using in other subsystems

flix/cli.ino

@@ -8,10 +8,12 @@
 #include "util.h"
 
 extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;
+extern const int ACRO, STAB, AUTO;
 extern float loopRate, dt;
 extern double t;
 extern uint16_t channels[16];
 extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
+extern int mode;
 extern bool armed;
 
 const char* motd =
@@ -34,6 +36,7 @@ const char* motd =
 "imu - show IMU data\n"
 "arm - arm the drone (when no armed switch)\n"
 "disarm - disarm the drone (when no armed switch)\n"
+"stab/acro/auto - set mode (when no mode switch)\n"
 "rc - show RC data\n"
 "mot - show motor output\n"
 "log - dump in-RAM log\n"
@@ -116,6 +119,12 @@ void doCommand(String str, bool echo = false) {
 		armed = true;
 	} else if (command == "disarm") {
 		armed = false;
+	} else if (command == "stab") {
+		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

@@ -34,7 +34,8 @@
 #define TILT_MAX radians(30)
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
-enum { MANUAL, ACRO, STAB, AUTO } mode = STAB;
+const int MANUAL = 0, ACRO = 1, STAB = 2, AUTO = 3; // flight modes
+int mode = STAB;
 bool armed = false;
 
 PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -79,7 +80,7 @@ void interpretControls() {
 
 	if (mode == STAB) {
 		float yawTarget = attitudeTarget.getYaw();
-		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or yaw rate is set
+		if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or pilot commands yaw rate
 		attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
 		ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
 	}
@@ -139,7 +140,7 @@ void controlTorque() {
 	if (!torqueTarget.valid()) return; // skip torque control
 
 	if (!armed || thrustTarget < 0.05) {
-		memset(motors, 0, sizeof(motors));
+		memset(motors, 0, sizeof(motors)); // stop motors if no thrust
 		return;
 	}
 

flix/failsafe.ino

@@ -3,7 +3,9 @@
 
 // Fail-safe functions
 
-#define RC_LOSS_TIMEOUT 0.2
+#include "util.h"
+
+#define RC_LOSS_TIMEOUT 0.5
 #define DESCEND_TIME 3.0 // time to descend from full throttle to zero
 
 extern double controlTime;
@@ -28,7 +30,7 @@ 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)) {
+		if (mode == AUTO && invalid(controlMode)) {
 			mode = STAB; // regain control to the pilot
 		}
 	}

flix/mavlink.ino

@@ -226,8 +226,14 @@ void handleMavlink(const void *_msg) {
 		}
 
 		if (m.command == MAV_CMD_DO_SET_MODE) {
-			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return;
-			mode = static_cast<decltype(mode)>(m.param2);
+			if (!(m.param2 >= 0 && m.param2 <= AUTO)) return; // incorrect mode
+			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);
+		}
+
+		if (m.command == MAV_CMD_COMPONENT_ARM_DISARM) {
+			armed = m.param1 == 1;
 			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);
 		}

tools/pyflix/flix.py

@@ -173,6 +173,10 @@ class Flix:
             self.motors = msg.controls[:4]  # type: ignore
             self._trigger('motors', self.motors)
 
+        # TODO: to be removed: the old way of passing motor outputs
+        if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
+            self.motors = msg.actuator[:4]  # type: ignore
+
         if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
             self.acc = self._mavlink_to_flu([msg.xacc / 1000, msg.yacc / 1000, msg.zacc / 1000])
             self.gyro = self._mavlink_to_flu([msg.xgyro / 1000, msg.ygyro / 1000, msg.zgyro / 1000])