Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

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.ino
index 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 rates
Vector torqueTarget;
float thrustTarget;
@@ -56,63 +57,47 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
extern 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 control
ratesTarget.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 control
rollPID.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 control
rollRatePID.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 thrust
return;
}
@@ -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.ino
index 531ad18..7d494c9 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -12,7 +12,8 @@
double t = NAN; // current step time, s
float 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 data
Vector acc; // accelerometer data, m/s/s
Vector rates; // filtered angular rates, rad/s
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 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.h
index 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.ino
index 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.h
index 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.h
index 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.h
index 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();