Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Minor code cleanups and fixes
Oleg Kalachevdbd413c23457
diff --git a/flix/cli.ino b/flix/cli.inoindex 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.inoindex 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 Hzenum { 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 modevoid 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 reversedmotors[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.inoindex 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++) {