upsampling filter at 5Hz

src/main/flight/autopilot.c

@@ -43,6 +43,7 @@
 #define POSITION_I_SCALE  0.0001f
 #define POSITION_D_SCALE  0.0015f
 #define POSITION_A_SCALE  0.0015f
+#define UPSAMPLING_CUTOFF 5.0f
 
 static pidCoefficient_t altitudePidCoeffs;
 static pidCoefficient_t positionPidCoeffs;
@@ -69,6 +70,10 @@ typedef struct {
     float lpfCutoff;
     float pt1Gain;
     bool sticksActive;
+    float pidSumRoll;
+    float pidSumPitch;
+    pt3Filter_t upsampleRollLpf;
+    pt3Filter_t upsamplePitchLpf;
     vectors_t NS;
     vectors_t EW;
 } posHoldState;
@@ -81,6 +86,8 @@ static posHoldState posHold = {
     .lpfCutoff = 1.0f,
     .pt1Gain = 1.0f,
     .sticksActive = false,
+    .pidSumRoll = 0.0f,
+    .pidSumPitch = 0.0f,
     .NS = {
         .isStarting = false,
         .distance = 0.0f,
@@ -136,6 +143,9 @@ void autopilotInit(const autopilotConfig_t *config)
     // approximate filter gain
     posHold.lpfCutoff = config->position_cutoff * 0.01f;
     posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); // assume 10Hz GPS connection at start
+    float upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF, 0.01f); // 5Hz, assuming 100Hz task rate
+    pt3FilterInit(&posHold.upsampleRollLpf, upsampleCutoff);
+    pt3FilterInit(&posHold.upsamplePitchLpf, upsampleCutoff);
     // initialise filters
 // Reset parameters for both NS and EW
     resetPositionControlParams(&posHold.NS);
@@ -209,18 +219,16 @@ bool positionControl(void) {
         return false;
     }
 
+    if (isNewDataForPosHold()) {
         posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
         posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
-
-
         if (posHold.sticksActive) {
             // if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
             resetPositionControlParams(&posHold.NS);
             resetPositionControlParams(&posHold.EW);
-        autopilotAngle[AI_ROLL] =  0.0f;
-        autopilotAngle[AI_PITCH] = 0.0f; 
+            posHold.pidSumRoll = 0.0f;
+            posHold.pidSumPitch = 0.0f;
         } else {
-
             // first get xy distances from current location (gpsSol.llh) to target location
             float nsDistance; // cm, steps of 11.1cm, North of target is positive
             float ewDistance; // cm, steps of 11.1cm, East of target is positive
@@ -318,30 +326,31 @@ bool positionControl(void) {
                     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10));
                     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(pidI * 10));
                     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidDA * 10));
-                DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(latLong->integral * 10));
                 }
             }
-
+        }
         // ** Rotate pid Sum to quad frame of reference, into pitch and roll **
         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;
-        float pidSumPitch = cosHeading * posHold.NS.pidSum + sinHeading * posHold.EW.pidSum;
+
+        posHold.pidSumRoll = -sinHeading * posHold.NS.pidSum + cosHeading * posHold.EW.pidSum;
+        posHold.pidSumPitch = cosHeading * posHold.NS.pidSum + sinHeading * posHold.EW.pidSum;
+    }
+
+    // ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
+    autopilotAngle[AI_ROLL] = pt3FilterApply(&posHold.upsampleRollLpf, posHold.pidSumRoll);
+    autopilotAngle[AI_PITCH] = pt3FilterApply(&posHold.upsamplePitchLpf, posHold.pidSumPitch);
 
     if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
-            DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(pidSumRoll * 10));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSumRoll * 10));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(autopilotAngle[AI_ROLL] * 10));
     } else {
-            DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(pidSumPitch * 10));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSumPitch * 10));
+        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(autopilotAngle[AI_PITCH] * 10));
     }
 
-        // todo: fix the upsample filtering in pid.c, because
-        // pidSum has steps at GPS rate, and needs to change the pid.c upsampling filter for smoothness.
 
-        // ** Final output to pid.c Angle Mode **
-        autopilotAngle[AI_ROLL] = pidSumRoll;
-        autopilotAngle[AI_PITCH] = pidSumPitch; 
-    }
     return true;
 }
 

src/main/flight/pos_hold.c

@@ -26,9 +26,9 @@
 #include "config/config.h"
 #include "fc/runtime_config.h"
 #include "fc/rc.h"
+#include "flight/autopilot.h"
 #include "flight/failsafe.h"
 #include "flight/position.h"
-#include "flight/autopilot.h"
 #include "rx/rx.h"
 
 #include "pos_hold.h"
@@ -89,10 +89,8 @@ void updatePosHold(timeUs_t currentTimeUs) {
     // check for enabling Pod Hold, otherwise do as little as possible while inactive
     posHoldStart();
     if (posHold.posHoldIsOK) {
-        if (isNewDataForPosHold()) {
         posHoldCheckSticks();
         posHold.posHoldIsOK = positionControl();
-        }
     } else {
         autopilotAngle[AI_PITCH] = 0.0f;
         autopilotAngle[AI_ROLL] = 0.0f;

src/test/unit/althold_unittest.cc

@@ -131,6 +131,8 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
     float getGpsDataIntervalSeconds(void) { return 0.01f; }
     float getRcDeflectionAbs(void) { return 0.0f; }
     attitudeEulerAngles_t attitude;
+    bool isNewDataForPosHold(void){ return true; }
+
 
     void parseRcChannels(const char *input, rxConfig_t *rxConfig)
     {
@@ -176,7 +178,6 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
         UNUSED(filter);
         UNUSED(k);
     }
-    
     void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
     {
         UNUSED(filter);
@@ -190,6 +191,29 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
         return 0.0;
     }
 
+    float pt3FilterGain(float f_cut, float dT)
+    {
+        UNUSED(f_cut);
+        UNUSED(dT);
+        return 0.0;
+    }
+    void pt3FilterInit(pt3Filter_t *filter, float k)
+    {
+        UNUSED(filter);
+        UNUSED(k);
+    }
+    void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
+    {
+        UNUSED(filter);
+        UNUSED(k);
+    }
+    float pt3FilterApply(pt3Filter_t *filter, float input)
+    {
+        UNUSED(filter);
+        UNUSED(input);
+        return 0.0;
+    }
+
 
     int16_t debug[DEBUG16_VALUE_COUNT];
     uint8_t debugMode;

src/test/unit/arming_prevention_unittest.cc

@@ -1192,4 +1192,5 @@ extern "C" {
 
     bool canUseGPSHeading;
     bool compassIsHealthy;
+    bool isNewDataForPosHold(void){ return true; }
 }