Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Minor code cleanups and fixes

Oleg Kalachevdbd413c23457
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++) {