Block yaw, allow in CLI, option to apply yaw correction code

src/main/cli/settings.c

@@ -1862,6 +1862,8 @@ const clivalue_t valueTable[] = {
     { PARAM_NAME_POSITION_D,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_D) },
     { PARAM_NAME_POSITION_A,          VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 },     PG_AUTOPILOT, offsetof(autopilotConfig_t, position_A) },
     { PARAM_NAME_POSITION_CUTOFF,     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 10, 250 },    PG_AUTOPILOT, offsetof(autopilotConfig_t, position_cutoff) },
+    { "position_allow_yaw",           VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AUTOPILOT, offsetof(autopilotConfig_t, position_allow_yaw) },
+    { "position_test_yaw_fix",        VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AUTOPILOT, offsetof(autopilotConfig_t, position_test_yaw_fix) },
 
 // PG_MODE_ACTIVATION_CONFIG
 #if defined(USE_CUSTOM_BOX_NAMES)

src/main/fc/rc.c

@@ -49,6 +49,7 @@
 
 #include "pg/rx.h"
 #include "pg/pos_hold.h"
+#include "pg/autopilot.h"
 
 #include "rx/rx.h"
 
@@ -715,6 +716,11 @@ FAST_CODE void processRcCommand(void)
                 float rcCommandf;
                 if (axis == FD_YAW) {
                     rcCommandf = rcCommand[axis] / rcCommandYawDivider;
+#ifdef USE_POS_HOLD_MODE
+                    if (FLIGHT_MODE(POS_HOLD_MODE) && !autopilotConfig()->position_allow_yaw) {
+                        rcCommandf = 0.0f;
+                    }
+#endif
                 } else {
                     rcCommandf = rcCommand[axis] / rcCommandDivider;
                 }

src/main/flight/autopilot.c

@@ -278,6 +278,13 @@ bool positionControl(void) {
     const float pitchP = pitchProportion * posHold.distanceCm * positionPidCoeffs.Kp;
 
     // derivative and acceleration
+    // note: velocity sign reverses when moving away, vs moving towards, the target
+    // and the proportioning factor reverses on overshooting
+    // net effect is that the sign of the correction stays correct
+    // accumulated velocity on one axis can only go to zero if velocity on that axis goes to zero
+    // hence we don't rotate the filtered D and A, only the input
+    // the delayed component of the filtered output should be rotated if the quad yaws (not done yet)
+
     // roll
     float velocityRoll = rollProportion * velocity;
     float accelerationRoll = rollProportion * acceleration;
@@ -302,44 +309,59 @@ bool positionControl(void) {
     float pitchA = accelerationPitch * positionPidCoeffs.Kf;
 
     // iTerm
+    // Note: accumulated iTerm opposes wind.
+    // The accumulated amount of iTerm on each axis does not care about bearing error
+    // But accumulated iTerm should be rotated if the craft yaws (not done yet)
+    // For that I think we need to accumulate iTerm with an iTerm vector angle to North
+    // then we get separate pitch and roll values by comparing craft heading and iTerm vector
+    // current code assumes no yaw input and does not rotate the accumulated iTerm to preserve its direction
     if (!posHold.isStarting){
-        // hold previous iTerm while starting up
+        // don't change accumulated iTerm while adjusting position or during startup phase
         posHold.rollI += rollProportion * posHold.distanceCm * positionPidCoeffs.Ki * gpsDataIntervalS;
         posHold.pitchI += pitchProportion * posHold.distanceCm * positionPidCoeffs.Ki * gpsDataIntervalS;
     }
 
-    // calculate rotation change from the initial heading, and rotate the roll and pitch proportions accordingly
+    // ** test code to handle user yaw inputs ** //
-    const float currentHeadingDeg = attitude.values.yaw * 0.1f;
-    float deltaHeading = currentHeadingDeg - posHold.initialHeadingDeg;
 
-    // Normalize deltaHeading 
+    if (autopilotConfig()->position_allow_yaw) {
-    if (deltaHeading > 180.0f) {
+        // code that we know is effective for fixing user yaw inputs (none at present lol)
-        deltaHeading -= 360.0f; // Wrap around if greater than 180
+        if (autopilotConfig()->position_test_yaw_fix) {
-    } else if (deltaHeading < -180.0f) {
+            // test code that might fix user yaw inputs
-        deltaHeading += 360.0f; // Wrap around if less than -180
+
+            // calculate rotation change from the initial heading
+            const float currentHeadingDeg = attitude.values.yaw * 0.1f;
+            float deltaHeading = currentHeadingDeg - posHold.initialHeadingDeg;
+
+            // Normalize deltaHeading 
+            if (deltaHeading > 180.0f) {
+                deltaHeading -= 360.0f; // Wrap around if greater than 180
+            } else if (deltaHeading < -180.0f) {
+                deltaHeading += 360.0f; // Wrap around if less than -180
+            }
+            const float deltaHeadingRadians = deltaHeading * RAD;
+
+            float cosDeltaHeading = cos_approx(deltaHeadingRadians);
+            float sinDeltaHeading = sin_approx(deltaHeadingRadians);
+
+            // rotate iTerm if heading changes from original heading
+            const float rotatedRollI = posHold.rollI * cosDeltaHeading + posHold.pitchI * sinDeltaHeading;
+            const float rotatedPitchI = posHold.pitchI * cosDeltaHeading - posHold.rollI * sinDeltaHeading;
+
+            posHold.rollI = rotatedRollI;
+            posHold.pitchI = rotatedPitchI;
+
+// rotate smoothed DA factors (doesn't work since inverts D sign inappropriately)
+//         const float rotatedRollD = rollD * cosDeltaHeading + pitchD * sinDeltaHeading;
+//         const float rotatedPitchD = pitchD * cosDeltaHeading - rollD * sinDeltaHeading;
+//         rollD = rotatedRollD;
+//         pitchD = rotatedPitchD;
+//     
+//         const float rotatedRollA = rollA * cosDeltaHeading + pitchA * sinDeltaHeading;
+//         const float rotatedPitchA = pitchA * cosDeltaHeading - rollA * sinDeltaHeading;
+//         rollA = rotatedRollA;
+//         pitchA = rotatedPitchA;
+        }
     }
-    const float deltaHeadingRadians = deltaHeading * RAD;
-
-    float cosDeltaHeading = cos_approx(deltaHeadingRadians);
-    float sinDeltaHeading = sin_approx(deltaHeadingRadians);
-
-    // rotate iTerm if heading changes from original heading
-    const float rotatedRollI = posHold.rollI * cosDeltaHeading + posHold.pitchI * sinDeltaHeading;
-    const float rotatedPitchI = posHold.pitchI * cosDeltaHeading - posHold.rollI * sinDeltaHeading;
-
-    posHold.rollI = rotatedRollI;
-    posHold.pitchI = rotatedPitchI;
-
-    // rotate smoothed DA factors
-//     const float rotatedRollD = rollD * cosDeltaHeading + pitchD * sinDeltaHeading;
-//     const float rotatedPitchD = pitchD * cosDeltaHeading - rollD * sinDeltaHeading;
-//     rollD = rotatedRollD;
-//     pitchD = rotatedPitchD;
-// 
-//     const float rotatedRollA = rollA * cosDeltaHeading + pitchA * sinDeltaHeading;
-//     const float rotatedPitchA = pitchA * cosDeltaHeading - rollA * sinDeltaHeading;
-//     rollA = rotatedRollA;
-//     pitchA = rotatedPitchA;
 
     // limit sum of D and A because otherwise too aggressive if entering at speed
     float rollDA = rollD + rollA;

src/main/pg/autopilot.c

@@ -28,7 +28,7 @@
 
 #include "autopilot.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 2);
 
 PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
     .landing_altitude_m = 4,
@@ -44,4 +44,6 @@ PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
     .position_D = 30,
     .position_A = 30,
     .position_cutoff = 80,
+    .position_allow_yaw = false,     // for testing
+    .position_test_yaw_fix = false,  // for testing
 );

src/main/pg/autopilot.h

@@ -39,6 +39,8 @@ typedef struct autopilotConfig_s {
     uint8_t position_D;
     uint8_t position_A;
     uint8_t position_cutoff;
+    bool position_allow_yaw;
+    bool position_test_yaw_fix;
 } autopilotConfig_t;
 
 PG_DECLARE(autopilotConfig_t, autopilotConfig);

src/main/pg/pos_hold.c

@@ -33,7 +33,7 @@
 PG_REGISTER_WITH_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig, PG_POSHOLD_CONFIG, 0);
 
 PG_RESET_TEMPLATE(posHoldConfig_t, posHoldConfig,
-    .pos_hold_without_mag = false,     // position hold within this percentage stick deflection
+    .pos_hold_without_mag = false,   // position hold within this percentage stick deflection
-    .pos_hold_deadband = 5, // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks
+    .pos_hold_deadband = 5,          // deadband in percent of stick travel for roll and pitch. Must be non-zero, and exceeded, for target location to be changed with sticks
 );
 #endif