Move controlManual down

okalachev · Dec 13, 2023 · 9a12ae9 · 9a12ae9
1 parent dbedf2a
commit 9a12ae9
Showing 1 changed file with 14 additions and 14 deletions.
diff --git a/flix/control.ino b/flix/control.ino
@@ -128,15 +128,21 @@ void controlAttitude()
 	}
 }
 
-// passthrough mode
-void controlManual()
+void controlRate()
 {
-	if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
+	if (!armed) {
 		memset(motors, 0, sizeof(motors));
+		rollRatePID.reset();
+		pitchRatePID.reset();
+		yawRatePID.reset();
 		return;
 	}
 
-	torqueTarget = ratesTarget * 0.01;
+	Vector ratesFiltered = ratesFilter.update(rates);
+
+	torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
+	torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
+	torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);
 
 	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
 	motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;
@@ -149,21 +155,15 @@ void controlManual()
 	motors[3] = constrain(motors[3], 0, 1);
 }
 
-void controlRate()
+// passthrough mode
+void controlManual()
 {
-	if (!armed) {
+	if (controls[RC_CHANNEL_THROTTLE] < 0.1) {
 		memset(motors, 0, sizeof(motors));
-		rollRatePID.reset();
-		pitchRatePID.reset();
-		yawRatePID.reset();
 		return;
 	}
 
-	Vector ratesFiltered = ratesFilter.update(rates);
-
-	torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
-	torqueTarget.y = pitchRatePID.update(ratesTarget.y - ratesFiltered.y, dt);
-	torqueTarget.z = yawRatePID.update(ratesTarget.z - ratesFiltered.z, dt);
+	torqueTarget = ratesTarget * 0.01;
 
 	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
 	motors[MOTOR_FRONT_RIGHT] = thrustTarget + torqueTarget.y - torqueTarget.x + torqueTarget.z;