diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 5b065d961a..e03678c602 100644 --- a/src/main/fc/tasks.c +++ b/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" diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index a1f704425b..6d97e65cee 100644 --- a/src/main/flight/autopilot.c +++ b/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) { diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index 3c3583938a..5bd1de040e 100644 --- a/src/main/flight/autopilot.h +++ b/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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 249729d084..5f1dc46a81 100644 --- a/src/main/flight/pid.c +++ b/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; diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index 32c86b5e64..3d1bf7cc58 100644 --- a/src/main/flight/pos_hold.c +++ b/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 diff --git a/src/main/flight/pos_hold.h b/src/main/flight/pos_hold.h index e56f3f074b..13092c09fe 100644 --- a/src/main/flight/pos_hold.h +++ b/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