implement reviews, reduce PID gains

src/main/common/maths.c

@@ -54,7 +54,7 @@ float sin_approx(float x)
 {
     // Wrap angle to 2π with range [-π π]
     x = fmodf(x, 2.0f * M_PIf);
-    if (x < -M_PIf) x += 2.0f * M_PIf;
+    if (x <= -M_PIf) x += 2.0f * M_PIf;
     if (x > M_PIf) x -= 2.0f * M_PIf;
 
     // Use axis symmetry around x = ±π/2 for polynomial outside of range [-π/2 π/2]

src/main/fc/rc.c

@@ -753,41 +753,28 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
     isRxDataNew = true;
 
     for (int axis = 0; axis < 3; axis++) {
-        float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f); // -500 to 500
+        float rc = constrainf(rcData[axis] - rxConfig()->midrc, -500.0f, 500.0f); // -500 to 500
-
+        float rcDeadband = 0;
         if (axis == ROLL || axis == PITCH) {
+            rcDeadband = rcControlsConfig()->deadband;
 #ifdef USE_POS_HOLD_MODE
-            float tmpDeadband = rcControlsConfig()->deadband;
             if (FLIGHT_MODE(POS_HOLD_MODE)) {
+                // override deadband in POS_HOLD_MODE
                 if (posHoldConfig()->pos_hold_deadband) {
-                    // if pos_hold_deadband is defined, ignore pitch & roll within deadband zone
+                    // if pos_hold_deadband is defined, ignore pitch & roll within deadband zone (in %)
-                    tmpDeadband = posHoldConfig()->pos_hold_deadband * 5.0f;
+                    rcDeadband = posHoldConfig()->pos_hold_deadband * 5.0f;
                     // NB could attenuate RP responsiveness outside deadband here, with tmp * 0.8f or whatever
                 } else {
                     // if pos_hold_deadband is zero, prevent user adjustment of pitch or roll
-                    tmp = 0;
+                    rcDeadband  = 500;  // can't exceed this
                 }
             }
-#else
-            const float tmpDeadband = rcControlsConfig()->deadband;
 #endif
-            if (tmp > tmpDeadband) {
-                tmp -= tmpDeadband;
-            } else {
-                tmp = 0;
-            }
-            rcCommand[axis] = tmp;
         } else {
-            if (tmp > rcControlsConfig()->yaw_deadband) {
+            rcDeadband  = rcControlsConfig()->yaw_deadband;
-                tmp -= rcControlsConfig()->yaw_deadband;
+            rc *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
-            } else {
-                tmp = 0;
-            }
-            rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
-        }
-        if (rcData[axis] < rxConfig()->midrc) {
-            rcCommand[axis] = -rcCommand[axis];
         }
+        rcCommand[axis] = fapplyDeadband(rc, rcDeadband);
     }
 
     int32_t tmp;

src/main/fc/tasks.c

@@ -545,11 +545,11 @@ void tasksInit(void)
 #endif
 
 #ifdef USE_ALT_HOLD_MODE
-    setTaskEnabled(TASK_ALTHOLD, true);
+    setTaskEnabled(TASK_ALTHOLD, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS));
 #endif
 
 #ifdef USE_POS_HOLD_MODE
-    setTaskEnabled(TASK_POSHOLD, true);
+    setTaskEnabled(TASK_POSHOLD, featureIsEnabled(FEATURE_GPS));
 #endif
 
 #ifdef USE_MAG

src/main/flight/autopilot.c

@@ -40,9 +40,9 @@
 #define ALTITUDE_D_SCALE  0.01f
 #define ALTITUDE_F_SCALE  0.01f
 #define POSITION_P_SCALE  0.001f
-#define POSITION_I_SCALE  0.0005f
+#define POSITION_I_SCALE  0.0001f
-#define POSITION_D_SCALE  0.002f
+#define POSITION_D_SCALE  0.0015f
-#define POSITION_A_SCALE  0.002f
+#define POSITION_A_SCALE  0.0015f
 
 static pidCoefficient_t altitudePidCoeffs;
 static pidCoefficient_t positionPidCoeffs;
@@ -190,7 +190,7 @@ void setSticksActiveStatus(bool areSticksActive)
     posHold.sticksActive = areSticksActive;
 }
 
-void updateTargetLocation(gpsLocation_t newTargetLocation) {
+void setTargetLocation(gpsLocation_t newTargetLocation) {
     currentTargetLocation = newTargetLocation;
 }
 
@@ -237,6 +237,7 @@ bool positionControl(void) {
         // larger threshold if faster at start
         if (posHold.NS.isStarting || posHold.EW.isStarting) {
             posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
+            // 1s of flight at current speed or 10m, in cm
         }
         // primarily to detect flyaway from no Mag or badly oriented Mag
         // but must accept some overshoot at the start, especially if entering at high speed
@@ -322,7 +323,7 @@ bool positionControl(void) {
         }
 
         // ** Rotate pid Sum to quad frame of reference, into pitch and roll **
-        float headingRads = (attitude.values.yaw / 10.0f) * RAD; // will be constrained to +/-pi in sin_approx()
+        float headingRads = DECIDEGREES_TO_RADIANS(attitude.values.yaw); // will be constrained to +/-pi in sin_approx()
         const float sinHeading = sin_approx(headingRads);
         const float cosHeading = cos_approx(headingRads);
         float pidSumRoll = -sinHeading * posHold.NS.pidSum + cosHeading * posHold.EW.pidSum;

src/main/flight/autopilot.h

@@ -27,7 +27,7 @@ void autopilotInit(const autopilotConfig_t *config);
 void resetAltitudeControl(void);
 void setSticksActiveStatus(bool areSticksActive);
 void resetPositionControl(gpsLocation_t initialTargetLocation);
-void updateTargetLocation(gpsLocation_t newTargetLocation);
+void setTargetLocation(gpsLocation_t newTargetLocation);
 
 void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
 bool positionControl();

src/main/flight/imu.c

@@ -611,19 +611,22 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 #if defined(USE_GPS)
 static void isGpsHeadingUsable(float groundspeedGain, float imuCourseError, float dt)
 {
-    // ** ATTEMPT TO DETECT SUCCESSFUL IMU ORIENTATION TO GPS **
+    if (!canUseGPSHeading) {
-    static float gpsHeadingTruth = 0;
+        static float gpsHeadingTruth = 0;
-    // groundspeedGain can be 5.0 in clean forward flight, up to 10.0 max
+        // groundspeedGain can be 5.0 in clean forward flight, up to 10.0 max
-    // fabsf(imuCourseError) is 0 when headings are aligned, 1 when 90 degrees error or worse
+        // fabsf(imuCourseError) is 0 when headings are aligned, 1 when 90 degrees error or worse
-    // accumulate 'points' based on alignment and likelihood of accumulation being good
+        // accumulate 'points' based on alignment and likelihood of accumulation being good
-    gpsHeadingTruth += fmaxf(groundspeedGain - fabsf(imuCourseError), 0.0f) * dt;
+        gpsHeadingTruth += fmaxf(groundspeedGain - fabsf(imuCourseError), 0.0f) * dt;
-    // recenter at 2.5s time constant
+        // recenter at 2.5s time constant
-    // TODO: intent is to match IMU time constant, approximately, but I don't exactly know how to do that
+        // TODO: intent is to match IMU time constant, approximately, but I don't exactly know how to do that
-    gpsHeadingTruth -= 0.4 * dt * gpsHeadingTruth; 
+        gpsHeadingTruth -= 0.4 * dt * gpsHeadingTruth; 
-    // if we accumulate enough 'points' over time, the IMU probably is OK
+        // if we accumulate enough 'points' over time, the IMU probably is OK
-    // will need to reaccumulate after a disarm (will be retained partly for very brief disarms)
+        // will need to reaccumulate after a disarm (will be retained partly for very brief disarms)
-    canUseGPSHeading = !canUseGPSHeading && (gpsHeadingTruth > 2.0f);
+        canUseGPSHeading = gpsHeadingTruth > 2.0f;
-    // latches true; reset on disarm; used to stop position hold without a suitable GPS heading, if needed
+        // canUseGPSHeading blocks position hold until suitable GPS heading, when GPS is the only heading source
+        // NOTE: I think that this check only runs once after power up
+        // If the GPS heading is lost on disarming, then it will need to be reset each disarm
+    }
 }
 #endif
 

src/main/flight/pos_hold.c

@@ -59,7 +59,7 @@ void posHoldCheckSticks(void)
                 // while sticks are outside the deadband,
                 // keep updating the home location to the current GPS location each iteration
                 setSticksActiveStatus(true);
-                updateTargetLocation(gpsSol.llh);
+                setTargetLocation(gpsSol.llh);
             } else {
                 setSticksActiveStatus(false);
             }
@@ -68,17 +68,19 @@ void posHoldCheckSticks(void)
 }
 
 void posHoldStart(void) {
-    static bool wasInPosHoldMode = false;
+    static bool isInPosHoldMode = false;
     if (FLIGHT_MODE(POS_HOLD_MODE)) {
-        if (!wasInPosHoldMode) {
+        if (!isInPosHoldMode) {
+            // start position hold mode
             posHold.posHoldIsOK = true; // true when started, false when autopilot code reports failure
             setSticksActiveStatus(false);
             resetPositionControl(gpsSol.llh);
-            wasInPosHoldMode = true;
+            isInPosHoldMode = true;
         }
     } else {
+        // stop position hold mode
         posHold.posHoldIsOK = false;
-        wasInPosHoldMode = false;
+        isInPosHoldMode = false;
     }
 }