Keep yaw setpoint within +/-delta from the current yaw (#831)

bitcraze · Nov 18, 2021 · e0bd515 · e0bd515
1 parent 62d0126
commit e0bd515
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 1 deletion.
diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
@@ -65,7 +65,20 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
   if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
     // Rate-controled YAW is moving YAW angle setpoint
     if (setpoint->mode.yaw == modeVelocity) {
-       attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
+      attitudeDesired.yaw = capAngle(attitudeDesired.yaw + setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT);
+
+      #ifdef YAW_MAX_DELTA
+      float delta = capAngle(attitudeDesired.yaw-state->attitude.yaw);
+      // keep the yaw setpoint within +/- YAW_MAX_DELTA from the current yaw
+        if (delta > YAW_MAX_DELTA)
+        {
+          attitudeDesired.yaw = state->attitude.yaw + YAW_MAX_DELTA;
+        }
+        else if (delta < -YAW_MAX_DELTA)
+        {
+          attitudeDesired.yaw = state->attitude.yaw - YAW_MAX_DELTA;
+        }
+      #endif
     } else {
       attitudeDesired.yaw = setpoint->attitude.yaw;
     }

diff --git a/tools/make/config.mk.example b/tools/make/config.mk.example
@@ -125,3 +125,6 @@
 # CFLAGS += -DIMU_PHI=0.0f
 # CFLAGS += -DIMU_THETA=0.0f
 # CFLAGS += -DIMU_PSI=0.0f
+
+## Keep the yaw setpoint within +/- YAW_MAX_DELTA from the current heading
+# CFLAGS += -DYAW_MAX_DELTA=45.0f