Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Better code for yaw target

Oleg Kalachev46579ce8a407
diff --git a/flix/control.ino b/flix/control.ino
index d9b5ae0..6de302c 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -36,6 +36,7 @@
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
enum { MANUAL, ACRO, STAB } mode = STAB;
+enum { YAW, YAW_RATE } yawMode = YAW;
bool armed = false;
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@@ -51,9 +52,6 @@ Quaternion attitudeTarget;
Vector ratesTarget;
Vector torqueTarget;
float thrustTarget;
- // TODO: ugly
-float yawTarget = NAN;
-bool controlYaw = false;
void control()
{
@@ -82,23 +80,26 @@ void interpretRC()
thrustTarget = controls[RC_CHANNEL_THROTTLE];
- controlYaw = armed && mode == STAB && controls[RC_CHANNEL_YAW] == 0;
- if (!controlYaw) {
- yawTarget = attitude.getYaw();
- }
-
if (mode == ACRO) {
+ yawMode = YAW_RATE;
ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
} else if (mode == STAB) {
+ yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
+
attitudeTarget = Quaternion::fromEulerZYX(
controls[RC_CHANNEL_ROLL] * MAX_TILT,
-controls[RC_CHANNEL_PITCH] * MAX_TILT,
- yawTarget); // attitude.getYaw());
+ attitudeTarget.getYaw());
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
}
+
+ if (yawMode == YAW_RATE || !motorsActive()) {
+ // update yaw target as we have not control over the yaw
+ attitudeTarget.setYaw(attitude.getYaw());
+ }
}
void controlAttitude()
@@ -134,8 +135,8 @@ void controlAttitude()
ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);
ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);
- if (controlYaw) {
- ratesTarget.z = yawPID.update(yawTarget - attitude.getYaw(), dt); // WARNING:
+ if (yawMode == YAW) {
+ ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);
}
if (!ratesTarget.finite()) {
@@ -212,6 +213,11 @@ void controlRate()
motors[3] = constrain(motors[3], 0, 1);
}
+bool motorsActive()
+{
+ return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
+}
+
const char* getModeName()
{
switch (mode) {
diff --git a/flix/util.ino b/flix/util.ino
index 3e966d4..eae6742 100644
--- a/flix/util.ino
+++ b/flix/util.ino
@@ -22,3 +22,15 @@ float randomFloat(float min, float max)
{
return min + (max - min) * (float)rand() / RAND_MAX;
}
+
+// wrap angle to [-PI, PI)
+float wrapAngle(float angle)
+{
+ angle = fmodf(angle, 2 * PI);
+ if (angle > PI) {
+ angle -= 2 * PI;
+ } else if (angle < -PI) {
+ angle += 2 * PI;
+ }
+ return angle;
+}
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 504c0df..20cb5cf 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -37,6 +37,7 @@ void controlAttitude();
void controlManual();
void controlRate();
void showTable();
+bool motorsActive();
void cliTestMotor(uint8_t n);
// mocks