first working example with smoothed acceleration

src/main/blackbox/blackbox.c

@@ -1813,7 +1813,11 @@ static bool blackboxWriteSysinfo(void)
 #endif // USE_GPS
 
 #ifdef USE_ALT_HOLD_MODE
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE, "%d", altholdConfig()->alt_hold_target_adjust_rate);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_ALT_HOLD_ADJUST_RATE, "%d", altHoldConfig()->alt_hold_adjust_rate);
+#endif
+
+#ifdef USE_POS_HOLD_MODE
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POS_HOLD_ADJUST_RATE, "%d", posHoldConfig()->pos_hold_adjust_rate);
 #endif
 
 #ifdef USE_WING

src/main/cli/settings.c

@@ -1111,11 +1111,11 @@ const clivalue_t valueTable[] = {
     { "yaw_control_reversed",       VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
 
 #ifdef USE_ALT_HOLD_MODE
-    { PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE,    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altholdConfig_t, alt_hold_target_adjust_rate) },
+    { PARAM_NAME_ALT_HOLD_ADJUST_RATE,    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_ALTHOLD_CONFIG, offsetof(altHoldConfig_t, alt_hold_adjust_rate) },
 #endif
 
-#ifdef USE_POSs_HOLD_MODE
+#ifdef USE_POS_HOLD_MODE
-    { PARAM_NAME_pos_hold_adjust_rate,    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_adjust_rate) },
+    { PARAM_NAME_POS_HOLD_ADJUST_RATE,    VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_POSHOLD_CONFIG, offsetof(posHoldConfig_t, pos_hold_adjust_rate) },
 #endif
 
 // PG_PID_CONFIG

src/main/common/maths.c

@@ -49,14 +49,33 @@
 #define sinPolyCoef7 -1.980661520e-4f                                          // Double: -1.980661520135080504411629636078917643846e-4
 #define sinPolyCoef9  2.600054768e-6f                                          // Double:  2.600054767890361277123254766503271638682e-6
 #endif
+// float sin_approx(float x)
+// {
+//     int32_t xint = x;
+//     if (xint < -32 || xint > 32) return 0.0f;                               // Stop here on error input (5 * 360 Deg)
+//     while (x >  M_PIf) x -= (2.0f * M_PIf);                                 // always wrap input angle to -PI..PI
+//     while (x < -M_PIf) x += (2.0f * M_PIf);
+//     if (x >  (0.5f * M_PIf)) x =  (0.5f * M_PIf) - (x - (0.5f * M_PIf));   // We just pick -90..+90 Degree
+//     else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
+//     float x2 = x * x;
+//     return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
+// }
+
+// alternate method suggested by chatgpt
 float sin_approx(float x)
 {
-    int32_t xint = x;
+    // Normalize input to the range -PI to PI
-    if (xint < -32 || xint > 32) return 0.0f;                               // Stop here on error input (5 * 360 Deg)
+    x = fmodf(x, 2.0f * M_PIf);
-    while (x >  M_PIf) x -= (2.0f * M_PIf);                                 // always wrap input angle to -PI..PI
+    if (x < -M_PIf) x += 2.0f * M_PIf;
-    while (x < -M_PIf) x += (2.0f * M_PIf);
+    if (x > M_PIf) x -= 2.0f * M_PIf;
-    if (x >  (0.5f * M_PIf)) x =  (0.5f * M_PIf) - (x - (0.5f * M_PIf));   // We just pick -90..+90 Degree
+
-    else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
+    // Use symmetry to handle inputs greater than ±90 degrees
+    if (x > M_PIf / 2) {
+        x = M_PIf - x; // Reflect
+    } else if (x < -M_PIf / 2) {
+        x = -M_PIf - x; // Reflect
+    }
+
     float x2 = x * x;
     return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
 }

src/main/fc/parameter_names.h

@@ -245,9 +245,13 @@
 #endif // USE_GPS
 
 #ifdef USE_ALT_HOLD_MODE
-#define PARAM_NAME_ALT_HOLD_TARGET_ADJUST_RATE "alt_hold_target_adjust_rate"
+#define PARAM_NAME_ALT_HOLD_ADJUST_RATE "alt_hold_adjust_rate"
 #endif // USE_ALT_HOLD_MODE
 
+#ifdef USE_POS_HOLD_MODE
+#define PARAM_NAME_POS_HOLD_ADJUST_RATE "pos_hold_adjust_rate"
+#endif // USE_POS_HOLD_MODE
+
 #define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
 #define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"
 #define PARAM_NAME_IMU_SMALL_ANGLE "small_angle"

src/main/fc/rc_modes.h

@@ -34,10 +34,10 @@ typedef enum {
     BOXHORIZON,
     BOXMAG,
     BOXALTHOLD,
-    BOXPOSHOLD,
     BOXHEADFREE,
     BOXPASSTHRU,
     BOXFAILSAFE,
+    BOXPOSHOLD,
     BOXGPSRESCUE,
     BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE,
 

src/main/flight/alt_hold.c

@@ -128,7 +128,7 @@ void altHoldUpdateTargetAltitude(void)
         // constant (set) deceleration target in the last 2m
         throttleAdjustmentFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
     }
-    altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altholdConfig()->alt_hold_target_adjust_rate;
+    altHoldState.targetAltitudeAdjustRate = throttleAdjustmentFactor * altHoldConfig()->alt_hold_adjust_rate;
 
     // if taskRate is 100Hz, default adjustRate of 100 adds/subtracts 1m every second, or 1cm per task run, at full stick position
     altHoldState.targetAltitudeCm += altHoldState.targetAltitudeAdjustRate  * taskIntervalSeconds;
@@ -137,7 +137,7 @@ void altHoldUpdateTargetAltitude(void)
 void altHoldUpdate(void)
 {
     // check if the user has changed the target altitude using sticks
-    if (altholdConfig()->alt_hold_target_adjust_rate) {
+    if (altHoldConfig()->alt_hold_adjust_rate) {
         altHoldUpdateTargetAltitude();
     }
 

src/main/flight/autopilot.c

@@ -22,7 +22,9 @@
 
 #include "platform.h"
 #include "build/debug.h"
+#include "common/filter.h"
 #include "common/maths.h"
+
 #include "flight/imu.h"
 #include "flight/pid.h"
 #include "flight/position.h"
@@ -34,15 +36,20 @@
 #define ALTITUDE_I_SCALE  0.003f
 #define ALTITUDE_D_SCALE  0.01f
 #define ALTITUDE_F_SCALE  0.01f
+#define POSITION_P_SCALE  0.01f
+#define POSITION_I_SCALE  0.01f
+#define POSITION_D_SCALE  0.01f
 
 static pidCoefficient_t altitudePidCoeffs;
-static pidCoefficient_t positionPidCoeffs;
 static float altitudeI = 0.0f;
 static float throttleOut = 0.0f;
+
+static pidCoefficient_t positionPidCoeffs;
 static float previousDistanceCm = 0.0f;
 static float previousVelocity = 0.0f;
-static bool newGPSData = false;
 float posHoldAngle[ANGLE_INDEX_COUNT];
+static pt1Filter_t accelerationLpf;
+static float positionAccelerationCutoffHz;
 
 void autopilotInit(const autopilotConfig_t *config)
 {
@@ -50,15 +57,13 @@ void autopilotInit(const autopilotConfig_t *config)
     altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
     altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
     altitudePidCoeffs.Kf = config->altitude_F * ALTITUDE_F_SCALE;
-    positionPidCoeffs.Kp = config->position_P * ALTITUDE_P_SCALE;
+    positionPidCoeffs.Kp = config->position_P * POSITION_P_SCALE;
-    positionPidCoeffs.Ki = config->position_I * ALTITUDE_I_SCALE;
+    positionPidCoeffs.Ki = config->position_I * POSITION_I_SCALE;
-    positionPidCoeffs.Kd = config->position_D * ALTITUDE_D_SCALE;
+    positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE;
-//    positionPidCoeffs.Kf = config->position_F * ALTITUDE_F_SCALE;
-}
 
-void posHoldNewGpsData(void)
+    positionAccelerationCutoffHz = config->position_filter_hz / 100.0f;
-{
+    const float gain = pt1FilterGain(positionAccelerationCutoffHz, 0.1f); // assume 10Hz GPS connection at start
-    newGPSData = true;
+    pt1FilterInit(&accelerationLpf, gain);
 }
 
 const pidCoefficient_t *getAltitudePidCoeffs(void)
@@ -123,9 +128,8 @@ void positionControl(gpsLocation_t targetLocation) {
     // gpsSol.llh = current gps location
     // get distance and bearing from current location to target location
     // void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t* to, bool dist3d, uint32_t *pDist, int32_t *pBearing)
-    float rollSetpoint = 0.0f;
+    static float rollSetpoint = 0.0f;
-    float pitchSetpoint = 0.0f;
+    static float pitchSetpoint = 0.0f;
-    if (newGPSData) {
     uint32_t distanceCm;
     int32_t bearing; // degrees * 100
     GPS_distance_cm_bearing(&gpsSol.llh, &targetLocation, false, &distanceCm, &bearing);
@@ -137,7 +141,6 @@ void positionControl(gpsLocation_t targetLocation) {
         normalisedErrorAngle -= 360.0f; // Range: -180 to 180
     }
 
-        // Convert to radians for sine and cosine functions
     float errorAngleRadians = normalisedErrorAngle * (M_PI / 180.0f);
 
     // Calculate correction factors using sine and cosine
@@ -152,37 +155,38 @@ void positionControl(gpsLocation_t targetLocation) {
     previousVelocity = velocity;
     // ** THIS WILL NEED A FILTER LIKE FOR GPS RESCUE**
 
-        float velocityP = velocity * positionPidCoeffs.Kp; // proportional to velocity
+    const float gain = pt1FilterGain(positionAccelerationCutoffHz, gpsDataIntervalS);
-        float velocityI = distanceCm * positionPidCoeffs.Ki; // proportional to distance error
+    pt1FilterUpdateCutoff(&accelerationLpf, gain);
-        float velocityD = acceleration * positionPidCoeffs.Kd; // proportional to acceleration *needs filtering*
+    acceleration = pt1FilterApply(&accelerationLpf, acceleration);
-        // F would be a value proportional to target change (not done yet)
+
+    float velocityP = velocity * positionPidCoeffs.Kp;     // velocity away from intended location
+    float velocityI = distanceCm * positionPidCoeffs.Ki;   // integral of velocity is distance from intended location
+    float velocityD = acceleration * positionPidCoeffs.Kd; // acceleration away from intended location
     
     float pidSum = velocityP + velocityI + velocityD; // greater when position error is bad and getting worse
 
-        // value sent to 
+    rollSetpoint = rollCorrection * pidSum;
-        rollSetpoint = rollCorrection * pidSum; // with some gain adjustment to give reasonable number
     pitchSetpoint = pitchCorrection * pidSum;
 
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle));
+    DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(normalisedErrorAngle)); //-180 to +180
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(rollCorrection));
+    DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(rollCorrection * 100));
-        DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pitchCorrection));
+    DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pitchCorrection * 100));
     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(distanceCm));
     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(velocityP));
     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(velocityI));
     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(velocityD));
 
-        newGPSData = false;
+    // send setpoints to pid.c using a method similar to that in gpsRescueAngle[axis]
-    }
+    // value sent needs shoiuld be in degrees * 100
-    // send setpoints to pid.c using the same method as for gpsRescueAngle
+    // values will have steps at GPS rate, if too jumpy we would need to upsample smooth them
-    // value sent should be in degrees * 100 (why??)
     posHoldAngle[AI_ROLL] = rollSetpoint;
     posHoldAngle[AI_PITCH] = pitchSetpoint;
 
     DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(rollSetpoint));
 
     // but for now let's not really do that until we get the PIDs sorted out :-)
-    posHoldAngle[AI_PITCH] = 0.0f;
+//    posHoldAngle[AI_PITCH] = 0.0f;
-    posHoldAngle[AI_ROLL] = 0.0f;
+//    posHoldAngle[AI_ROLL] = 0.0f;
 }
 
 bool isBelowLandingAltitude(void)

src/main/flight/pos_hold.c

@@ -53,7 +53,6 @@ void posHoldProcessTransitions(void) {
     if (FLIGHT_MODE(POS_HOLD_MODE)) {
         if (!posHoldState.isPosHoldActive) {
             posHoldReset();
-            DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, 22);
             posHoldState.isPosHoldActive = true;
         }
     } else {
@@ -68,26 +67,27 @@ void posHoldUpdateTargetLocation(void)
     // We need to provide a big deadband in rc.c
 
     if (!failsafeIsActive()) {
-
+        const float deadband = 0.2f;
-    // most easily...
+        if ((getRcDeflectionAbs(FD_ROLL) > deadband) || (getRcDeflectionAbs(FD_PITCH) > deadband))  {
-    // fly the quad, in angle mode, enabling a deadband via rc.c (?)
+            // allow user to fly the quad, in angle mode, enabling a 20% deadband via rc.c (?)
-    // while sticks are inside the deadband,
+            // while sticks are outside the deadband,
             // set the target location to the current GPS location each iteration
-    // posHoldState.targetLocation = currentLocation;
             posHoldState.targetLocation = gpsSol.llh;
         }
-
+    }
 }
 
 void posHoldUpdate(void)
 {
-    // check if the user has changed the target altitude using sticks
+    // check if the user wants to change the target position using sticks
     if (posHoldConfig()->pos_hold_adjust_rate) {
         posHoldUpdateTargetLocation();
     }
 
-    // run a function in autopilot.c to adjust position
+    if (getIsNewDataForPosHold()) {
         positionControl(posHoldState.targetLocation);
+    } else {
+    }
 }
 
 void updatePosHoldState(timeUs_t currentTimeUs) {

src/main/io/gps.c

@@ -53,9 +53,9 @@
 #include "fc/gps_lap_timer.h"
 #include "fc/runtime_config.h"
 
+#include "flight/gps_rescue.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
-#include "flight/gps_rescue.h"
 
 #include "scheduler/scheduler.h"
 
@@ -93,6 +93,7 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
 
 static serialPort_t *gpsPort;
 static float gpsDataIntervalSeconds;
+static bool newDataForPosHold = false;
 
 typedef struct gpsInitData_s {
     uint8_t index;
@@ -2608,13 +2609,17 @@ void onGpsNewData(void)
     gpsRescueNewGpsData();
 #endif
 
-#ifdef USE_POS_HOLD
-    posHoldNewGpsData();
-#endif
-
 #ifdef USE_GPS_LAP_TIMER
     gpsLapTimerNewGpsData();
 #endif // USE_GPS_LAP_TIMER
+
+    newDataForPosHold = true;
+}
+
+bool getIsNewDataForPosHold(void) {
+    bool currentState = newDataForPosHold; // true only when new data arrives
+    newDataForPosHold = false; // clear flag once new data has been handled
+    return currentState;
 }
 
 void gpsSetFixState(bool state)

src/main/io/gps.h

@@ -389,6 +389,9 @@ void onGpsNewData(void);
 void GPS_reset_home_position(void);
 void GPS_calc_longitude_scaling(int32_t lat);
 void GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing);
+
 void gpsSetFixState(bool state);
 float getGpsDataIntervalSeconds(void);      // sends GPS Nav Data interval to GPS Rescue
+bool getIsNewDataForPosHold(void);
+
 baudRate_e getGpsPortActualBaudRateIndex(void);

src/main/pg/alt_hold.c

@@ -29,9 +29,9 @@
 
 #include "alt_hold.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 4);
 
-PG_RESET_TEMPLATE(altholdConfig_t, altholdConfig,
+PG_RESET_TEMPLATE(altHoldConfig_t, altHoldConfig,
-    .alt_hold_target_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
+    .alt_hold_adjust_rate = 100, // max rate of change of altitude target using sticks in cm/s
 );
 #endif

src/main/pg/alt_hold.h

@@ -24,9 +24,9 @@
 
 #include "pg/pg.h"
 
-typedef struct altholdConfig_s {
+typedef struct altHoldConfig_s {
-    uint8_t alt_hold_target_adjust_rate;
+    uint8_t alt_hold_adjust_rate;
-} altholdConfig_t;
+} altHoldConfig_t;
 
-PG_DECLARE(altholdConfig_t, altholdConfig);
+PG_DECLARE(altHoldConfig_t, altHoldConfig);
 

src/main/pg/autopilot.c

@@ -38,8 +38,9 @@ PG_RESET_TEMPLATE(autopilotConfig_t, autopilotConfig,
     .altitude_I = 15,
     .altitude_D = 15,
     .altitude_F = 15,
-    .position_P = 8,
+    .position_P = 15,
     .position_I = 40,
     .position_D = 15,
 //    .position_F = 0,
+    .position_filter_hz = 75,
 );

src/main/pg/autopilot.h

@@ -37,6 +37,7 @@ typedef struct autopilotConfig_s {
     uint8_t position_I;
     uint8_t position_D;
 //    uint8_t position_F;
+    uint8_t position_filter_hz;
 } autopilotConfig_t;
 
 PG_DECLARE(autopilotConfig_t, autopilotConfig);

src/main/telemetry/crsf.c

@@ -459,7 +459,7 @@ void crsfFrameFlightMode(sbuf_t *dst)
         flightMode = "ANGL";
     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
         flightMode = "ALTH";
-    } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
+    } else if (FLIGHT_MODE(POS_HOLD_MODE)) {
         flightMode = "POSH";
     } else if (FLIGHT_MODE(HORIZON_MODE)) {
         flightMode = "HOR";

src/test/unit/althold_unittest.cc

@@ -44,7 +44,7 @@ extern "C" {
     PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
     PG_REGISTER(positionConfig_t, positionConfig, PG_POSITION, 0);
     PG_REGISTER(autopilotConfig_t, autopilotConfig, PG_AUTOPILOT, 0);
-    PG_REGISTER(altholdConfig_t, altholdConfig, PG_ALTHOLD_CONFIG, 0);
+    PG_REGISTER(altHoldConfig_t, altHoldConfig, PG_ALTHOLD_CONFIG, 0);
 
     extern altHoldState_t altHoldState;
     void altHoldInit(void);
@@ -103,6 +103,17 @@ extern "C" {
     float getCosTiltAngle(void) { return 0.0f; }
     float rcCommand[4];
     
+    void GPS_distance_cm_bearing(&gpsSol.llh, &targetLocation, false, &distanceCm, &bearing)
+    {
+       UNUSED(gpsSol.llh);
+       UNUSED(targetLocation);
+       UNUSED(false);
+       UNUSED(distanceCm);
+       UNUSED(bearing);
+
+    }
+
+
     void parseRcChannels(const char *input, rxConfig_t *rxConfig)
     {
         UNUSED(input);