Refactor control subsystem

Add interpretControls function to convert pilot commands and mode into control targets and make control functions independent from the mode.
Add ratesExtra target for rates feed-forward; remove yawMode.
Rename controlRate to controlRates to reflect rates variable name.
Remove USER mode.

flix/control.ino

@@ -34,7 +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 } mode = STAB;
 enum { YAW, YAW_RATE } yawMode = YAW;
 bool armed = false;
 
@@ -49,6 +49,7 @@ float tiltMax = TILT_MAX;
 
 Quaternion attitudeTarget;
 Vector ratesTarget;
+Vector ratesExtra; // feedforward rates
 Vector torqueTarget;
 float thrustTarget;
 
@@ -56,63 +57,47 @@ 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();
-		controlRate();
+	controlRates();
 	controlTorque();
-	} else if (mode == ACRO) {
-		controlRate();
-		controlTorque();
-	} else if (mode == MANUAL) {
-		controlTorque();
-	}
 }
 
-void interpretRC() {
+void interpretControls() {
-	armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
+	armed = controlThrottle >= 0.05;
+	if (controlArmed < 0.5) armed = false;
 
 	// 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 = STAB;
-		mode = STAB;
-	} else {
-		mode = STAB;
-	}
 
 	thrustTarget = controlThrottle;
 
+	if (mode == STAB) {
+		float yawTarget = attitudeTarget.getYaw();
+		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
+	}
+
 	if (mode == ACRO) {
-		yawMode = YAW_RATE;
+		attitudeTarget.invalidate(); // skip attitude control
 		ratesTarget.x = controlRoll * maxRate.x;
 		ratesTarget.y = controlPitch * maxRate.y;
 		ratesTarget.z = -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 == MANUAL) { // passthrough mode
-		// update yaw target as we don't have control over the yaw
+		attitudeTarget.invalidate(); // skip attitude control
-		attitudeTarget.setYaw(attitude.getYaw());
+		ratesTarget.invalidate(); // skip rate control
+		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 +110,16 @@ 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());
-		ratesTarget.z = yawPID.update(yawError, dt);
+	ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
-	}
 }
 
-void controlRate() {
+
-	if (!armed) {
+void controlRates() {
+	if (!armed || ratesTarget.invalid()) { // skip rates control
 		rollRatePID.reset();
 		pitchRatePID.reset();
 		yawRatePID.reset();
@@ -151,8 +135,10 @@ void controlRate() {
 }
 
 void controlTorque() {
-	if (!armed) {
+	if (!torqueTarget.valid()) return; // skip torque control
-		memset(motors, 0, sizeof(motors));
+
+	if (!armed || thrustTarget < 0.05) {
+		memset(motors, 0, sizeof(motors)); // stop motors if no thrust
 		return;
 	}
 
@@ -172,7 +158,6 @@ const char* getModeName() {
 		case MANUAL: return "MANUAL";
 		case ACRO: return "ACRO";
 		case STAB: return "STAB";
-		case USER: return "USER";
 		default: return "UNKNOWN";
 	}
 }

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

@@ -102,8 +102,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;
-		controlArmed = 1; // armed
+		controlArmed = NAN;
 		controlTime = t;
 
 		if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;

flix/quaternion.h

@@ -64,6 +64,22 @@ 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/rc.ino

@@ -42,7 +42,7 @@ void normalizeRC() {
 	controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;
 	controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;
 	controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;
-	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default
+	controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : NAN;
 	controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;
 }
 

flix/util.h

@@ -19,6 +19,14 @@ float mapff(float x, float in_min, float in_max, float out_min, float out_max) {
 	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
 
+bool invalid(float x) {
+	return !isfinite(x);
+}
+
+bool valid(float x) {
+	return isfinite(x);
+}
+
 // Wrap angle to [-PI, PI)
 float wrapAngle(float angle) {
 	angle = fmodf(angle, 2 * PI);

flix/vector.h

@@ -21,6 +21,21 @@ 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();