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,13 +309,26 @@ 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 ** //
+
+    if (autopilotConfig()->position_allow_yaw) {
+        // code that we know is effective for fixing user yaw inputs (none at present lol)
+        if (autopilotConfig()->position_test_yaw_fix) {
+            // test code that might fix user yaw inputs
+
+            // calculate rotation change from the initial heading
             const float currentHeadingDeg = attitude.values.yaw * 0.1f;
             float deltaHeading = currentHeadingDeg - posHold.initialHeadingDeg;
 
@@ -330,7 +350,7 @@ bool positionControl(void) {
             posHold.rollI = rotatedRollI;
             posHold.pitchI = rotatedPitchI;
 
-    // rotate smoothed DA factors
+// 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;
@@ -340,6 +360,8 @@ bool positionControl(void) {
 //         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);