From dbd413c2345707b3d627f772d77d6f6d504df60a Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Sun, 12 Nov 2023 10:51:56 +0300
Subject: [PATCH] Minor code cleanups and fixes

---
 flix/cli.ino     |  2 --
 flix/control.ino | 62 +++---------------------------------------------
 flix/log.ino     |  2 +-
 3 files changed, 4 insertions(+), 62 deletions(-)

diff --git a/flix/cli.ino b/flix/cli.ino
index 3908389..3f5bdd2 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -55,8 +55,6 @@ const struct Param {
 	{"ss", &loopFreq, nullptr},
 	{"dt", &dt, nullptr},
 	{"t", &t, nullptr},
-
-	// {"m", &mode, nullptr},
 };
 
 void doCommand(String& command, String& value)
diff --git a/flix/control.ino b/flix/control.ino
index 30fc398..d9b5ae0 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -32,7 +32,7 @@
 #define YAWRATE_MAX 360 * DEG_TO_RAD
 #define MAX_TILT 30 * DEG_TO_RAD
 
-#define RATES_LFP_ALPHA 0.8
+#define RATES_LFP_ALPHA 0.8 // cutoff frequency ~ 250 Hz
 #define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
 
 enum { MANUAL, ACRO, STAB } mode = STAB;
@@ -68,7 +68,6 @@ void control()
 	}
 }
 
-
 void interpretRC()
 {
 	if (controls[RC_CHANNEL_MODE] < 0.25) {
@@ -81,6 +80,8 @@ void interpretRC()
 
 	armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_AUX] >= 0.5;
 
+	thrustTarget = controls[RC_CHANNEL_THROTTLE];
+
 	controlYaw = armed && mode == STAB && controls[RC_CHANNEL_YAW] == 0;
 	if (!controlYaw) {
 		yawTarget = attitude.getYaw();
@@ -151,25 +152,6 @@ void controlAttitude()
 	// std::cout << "rsp: " << ratesTarget.x << " " << ratesTarget.y << std::endl;
 }
 
-void controlAttitudeAlter()
-{
-	if (!armed) {
-		rollPID.reset();
-		pitchPID.reset();
-		yawPID.reset();
-		return;
-	}
-	Vector target = attitudeTarget.toEulerZYX();
-	Vector att = attitude.toEulerZYX();
-
-	ratesTarget.x = rollPID.update(target.x - att.x, dt);
-	ratesTarget.y = pitchPID.update(target.y - att.y, dt);
-
-	if (controlYaw) {
-		ratesTarget.z = yawPID.update(target.z - att.z, dt); // WARNING:
-	}
-}
-
 // passthrough mode
 void controlManual()
 {
@@ -178,8 +160,6 @@ void controlManual()
 		return;
 	}
 
-	thrustTarget = controls[RC_CHANNEL_THROTTLE]; // collective thrust
-
 	torqueTarget = ratesTarget * 0.01;
 
 	motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.y + torqueTarget.x - torqueTarget.z;
@@ -207,9 +187,6 @@ void controlRate()
 		return;
 	}
 
-	// collective thrust is throttle * 4
-	thrustTarget = controls[RC_CHANNEL_THROTTLE]; // WARNING:
-
 	Vector ratesFiltered = ratesFilter.update(rates);
 
 	torqueTarget.x = rollRatePID.update(ratesTarget.x - ratesFiltered.x, dt); // un-normalized "torque"
@@ -228,9 +205,6 @@ void controlRate()
 	motors[MOTOR_REAR_LEFT] = thrustTarget - torqueTarget.y + torqueTarget.x + torqueTarget.z;
 	motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.y - torqueTarget.x - torqueTarget.z;
 
-	//indicateSaturation();
-	// desaturate(motors[0], motors[1], motors[2], motors[3]);
-
 	// constrain and reverse, multiple by -1 if reversed
 	motors[0] = constrain(motors[0], 0, 1);
 	motors[1] = constrain(motors[1], 0, 1);
@@ -238,36 +212,6 @@ void controlRate()
 	motors[3] = constrain(motors[3], 0, 1);
 }
 
-void desaturate(float& a, float& b, float& c, float& d)
-{
-	float m = max(max(a, b), max(c, d));
-	if (m > 1) {
-		float diff = m - 1;
-		a -= diff;
-		b -= diff;
-		c -= diff;
-		d -= diff;
-	}
-	m = min(min(a, b), min(c, d));
-	if (m < 0) {
-		a -= m;
-		b -= m;
-		c -= m;
-		d -= m;
-	}
-}
-
-static bool motorsSaturation = false;
-
-inline void indicateSaturation() {
-	bool sat = motors[0] > 1 || motors[1] > 1 || motors[2] > 1 || motors[3] > 1 ||
-	           motors[0] < 0 || motors[1] < 0 || motors[2] < 0 || motors[3] < 0;
-	if (motorsSaturation != sat) {
-		motorsSaturation = sat;
-		setLED(motorsSaturation);
-	}
-}
-
 const char* getModeName()
 {
 	switch (mode) {
diff --git a/flix/log.ino b/flix/log.ino
index 3c9572f..cc360ca 100644
--- a/flix/log.ino
+++ b/flix/log.ino
@@ -43,7 +43,7 @@ void logData()
 
 void dumpLog()
 {
-	Serial.printf("t,rate.x,rate.y,rate.z,ratesTarget.x,ratesTarget.y,ratesTarget.z,"
+	Serial.printf("t,rates.x,rates.y,rates.z,ratesTarget.x,ratesTarget.y,ratesTarget.z,"
 		"attitude.x,attitude.y,attitude.z,attitudeTarget.x,attitudeTarget.y,attitudeTarget.z,thrustTarget\n");
 	for (int i = 0; i < LOG_SIZE; i++) {
 		for (int j = 0; j < LOG_COLUMNS - 1; j++) {