first position hold draft

src/main/fc/tasks.c

@@ -58,6 +58,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/position.h"
+#include "flight/autopilot.h"
 #include "flight/alt_hold.h"
 #include "flight/pos_hold.h"
 

src/main/flight/autopilot.c

@@ -38,6 +38,7 @@
 static pidCoefficient_t altitudePidCoeffs;
 static float altitudeI = 0.0f;
 static float throttleOut = 0.0f;
+float posHoldAngle[ANGLE_INDEX_COUNT];
 
 void autopilotInit(const autopilotConfig_t *config)
 {
@@ -95,6 +96,36 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float vertical
     DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 7, lrintf(altitudeF));
 }
 
+void positionControl(gpsLocation_t targetLocation, float taskIntervalS) {
+    // 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)
+    uint32_t errorDistanceCm;
+    int32_t bearing;
+    GPS_distance_cm_bearing(&gpsSol.llh, &targetLocation, false, &errorDistanceCm, &bearing);
+
+    // calculate difference between current heading (direction of nose) and bearing to target
+    const float errorBearing = (attitude.values.yaw - bearing);
+
+    // do fancy maths on errorBearing to calculate relative pitch and roll strengths -1 to 1
+    const float rollCorrection = errorBearing; // pretend that we did fancy maths on errorBearing
+    const float pitchCorrection = errorBearing; // pretend that we did fancy maths on errorBearing
+
+    // use a PID function to return drive force from velocity and distance; 0-1 or similar
+    const float pidGain = 1.0f * errorDistanceCm * taskIntervalS; // pretend line temporary to use the incoming values
+
+    const float rollSetpoint = rollCorrection * pidGain; // with some gain adjustment to give reasonable number
+    const float pitchSetpoint = pitchCorrection * pidGain;
+
+    // send setpoints to pid.c using the same method as for gpsRescueAngle
+    posHoldAngle[AI_PITCH] = pitchSetpoint;
+    posHoldAngle[AI_ROLL] = rollSetpoint;
+
+    // but for now let's not really do that until we get the PIDs sorted out :-)
+    posHoldAngle[AI_PITCH] = 0.0f;
+    posHoldAngle[AI_ROLL] = 0.0f;
+}
+
 bool isBelowLandingAltitude(void)
 {
     return getAltitudeCm() < 100.0f * autopilotConfig()->landing_altitude_m;
@@ -103,7 +134,6 @@ bool isBelowLandingAltitude(void)
 const pidCoefficient_t *getAltitudePidCoeffs(void)
 {
     return &altitudePidCoeffs;
-}
 
 float getAutopilotThrottle(void)
 {

src/main/flight/autopilot.h

@@ -19,11 +19,16 @@
 
 #include "pg/autopilot.h"
 #include "flight/pid.h"
+#include "io/gps.h"
+
+extern float posHoldAngle[ANGLE_INDEX_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
 
 void autopilotInit(const autopilotConfig_t *config);
 void resetAltitudeControl(void);
 
 void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
+void positionControl(gpsLocation_t targetLocation, float taskIntervalS);
+
 
 bool isBelowLandingAltitude(void);
 const pidCoefficient_t *getAltitudePidCoeffs(void);

src/main/flight/pid.c

@@ -569,6 +569,9 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
 
 #ifdef USE_GPS_RESCUE
     angleTarget += gpsRescueAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
+#endif
+#ifdef USE_POS_HOLD
+    angleTarget += posHoldAngle[axis] / 100.0f; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
 #endif
     const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; // stepped at 500hz with some 4ms flat spots
     const float errorAngle = angleTarget - currentAngle;

src/main/flight/pos_hold.c

@@ -33,8 +33,7 @@
 
 #include "pos_hold.h"
 
-// unused at present
+static const float taskIntervalSeconds = HZ_TO_INTERVAL(POSHOLD_TASK_RATE_HZ); // i.e. 0.01 s
-// static const float taskIntervalSeconds = HZ_TO_INTERVAL(POSHOLD_TASK_RATE_HZ); // i.e. 0.01 s
 
 posHoldState_t posHoldState;
 
@@ -88,7 +87,7 @@ void posHoldUpdate(void)
     }
 
     // run a function in autopilot.c to adjust position
-    // positionControl(posHoldState.targetLocation, taskIntervalSeconds, posHoldState.adjustRateRoll, posHoldState.adjustRatePitch);
+    positionControl(posHoldState.targetLocation, taskIntervalSeconds);
 }
 
 void updatePosHoldState(timeUs_t currentTimeUs) {
@@ -101,4 +100,5 @@ void updatePosHoldState(timeUs_t currentTimeUs) {
         posHoldUpdate();
     }
 }
-#endif
+
+#endif // USE_POS_HOLD

src/main/flight/pos_hold.h

@@ -21,7 +21,7 @@
 
 #ifdef USE_ALT_HOLD_MODE
 #include "common/time.h"
-#include "gps.h"
+#include "io/gps.h"
 
 #define POSHOLD_TASK_RATE_HZ 100         // hz