Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.
Oleg Kalachev8418723ccc08
diff --git a/flix/control.ino b/flix/control.inoindex 5957a21..2d5db74 100644--- a/flix/control.ino+++ b/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 ratesVector torqueTarget;float thrustTarget;@@ -56,63 +57,47 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FROextern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;void control() {- interpretRC();+ interpretControls();failsafe();- if (mode == STAB) {- controlAttitude();- controlRate();- controlTorque();- } else if (mode == ACRO) {- controlRate();- controlTorque();- } else if (mode == MANUAL) {- controlTorque();- }+ controlAttitude();+ controlRates();+ controlTorque();}-void interpretRC() {- armed = controlThrottle >= 0.05 && controlArmed >= 0.5;+void interpretControls() {+ 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) {- mode = STAB;- } else if (controlMode < 0.75) {- mode = STAB;- } else {- mode = STAB;- }+ if (controlMode < 0.25) mode = STAB;+ if (controlMode < 0.75) mode = STAB;+ if (controlMode > 0.75) 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 controlratesTarget.x = controlRoll * maxRate.x;- ratesTarget.y = controlPitch* maxRate.y;+ 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()) {- // update yaw target as we don't have control over the yaw- attitudeTarget.setYaw(attitude.getYaw());+ if (mode == MANUAL) { // passthrough mode+ attitudeTarget.invalidate(); // skip attitude control+ ratesTarget.invalidate(); // skip rate control+ torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;}}void controlAttitude() {- if (!armed) {+ if (!armed || attitudeTarget.invalid()) { // skip attitude controlrollPID.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.y = pitchPID.update(error.y, dt);+ ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;+ 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);- }+ float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());+ ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;}-void controlRate() {- if (!armed) {++void controlRates() {+ if (!armed || ratesTarget.invalid()) { // skip rates controlrollRatePID.reset();pitchRatePID.reset();yawRatePID.reset();@@ -151,8 +135,10 @@ void controlRate() {}void controlTorque() {- if (!armed) {- memset(motors, 0, sizeof(motors));+ if (!torqueTarget.valid()) return; // skip torque control++ if (!armed || thrustTarget < 0.05) {+ memset(motors, 0, sizeof(motors)); // stop motors if no thrustreturn;}@@ -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";}}diff --git a/flix/flix.ino b/flix/flix.inoindex 531ad18..7d494c9 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -12,7 +12,8 @@double t = NAN; // current step time, sfloat 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 dataVector acc; // accelerometer data, m/s/sVector rates; // filtered angular rates, rad/sdiff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 88653ed..8f48ef0 100644--- a/flix/mavlink.ino+++ b/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- controlArmed = 1; // armed+ controlMode = NAN;+ controlArmed = NAN;controlTime = t;if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;diff --git a/flix/quaternion.h b/flix/quaternion.hindex 24bd1e9..29a3d04 100644--- a/flix/quaternion.h+++ b/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);}diff --git a/flix/rc.ino b/flix/rc.inoindex b0c8261..09ba957 100644--- a/flix/rc.ino+++ b/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;}diff --git a/flix/util.h b/flix/util.hindex 9e63532..90925d5 100644--- a/flix/util.h+++ b/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);diff --git a/flix/vector.h b/flix/vector.hindex b2d9a89..272e1db 100644--- a/flix/vector.h+++ b/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);}diff --git a/gazebo/flix.h b/gazebo/flix.hindex 53da2b3..f843597 100644--- a/gazebo/flix.h+++ b/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();