Better code for yaw target

okalachev · Dec 13, 2023 · 0434af8 · 0434af8
1 parent ad5ebee
commit 0434af8
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 11 deletions.
diff --git a/flix/control.ino b/flix/control.ino
@@ -37,6 +37,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);
@@ -52,9 +53,6 @@ Quaternion attitudeTarget;
 Vector ratesTarget;
 Vector torqueTarget;
 float thrustTarget;
- // TODO: ugly
-float yawTarget = NAN;
-bool controlYaw = false;
 
 void control()
 {
@@ -83,23 +81,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()
@@ -135,8 +136,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()) {
@@ -213,6 +214,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
@@ -23,3 +23,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
@@ -38,6 +38,7 @@ void controlAttitude();
 void controlManual();
 void controlRate();
 void showTable();
+bool motorsActive();
 void cliTestMotor(uint8_t n);
 
 // mocks