Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-27 02:05:31 +03:00
Code Issues Wiki Activity

earth ref Dterm, not from GPS Speed

Browse source 
smoother than using GPS Speed and heading
This commit is contained in:
ctzsnooze 2024-10-25 16:59:52 +11:00
parent c1f79e7d73
commit 55814b3bac
3 changed files with 126 additions and 115 deletions
Download patch file Download diff file Expand all files Collapse all files

231
src/main/flight/autopilot.c
View file

@ -39,9 +39,9 @@
#define ALTITUDE_I_SCALE 0.003f #define ALTITUDE_I_SCALE 0.003f
#define ALTITUDE_D_SCALE 0.01f #define ALTITUDE_D_SCALE 0.01f
#define ALTITUDE_F_SCALE 0.01f #define ALTITUDE_F_SCALE 0.01f
#define POSITION_P_SCALE 0.0008f #define POSITION_P_SCALE 0.001f
#define POSITION_I_SCALE 0.0002f #define POSITION_I_SCALE 0.0003f
#define POSITION_D_SCALE 0.0015f #define POSITION_D_SCALE 0.003f
#define POSITION_A_SCALE 0.0008f #define POSITION_A_SCALE 0.0008f
static pidCoefficient_t altitudePidCoeffs; static pidCoefficient_t altitudePidCoeffs;
@ -54,38 +54,41 @@ typedef struct {
float distanceCm; float distanceCm;
float previousDistanceCm; float previousDistanceCm;
float sanityCheckDistance; float sanityCheckDistance;
float previousVelocity;
float initialHeadingDeg; float initialHeadingDeg;
float iTermRoll;
float iTermPitch;
bool isStarting; bool isStarting;
float peakInitialGroundspeed; float peakInitialGroundspeed;
float lpfCutoff; float lpfCutoff;
bool sticksActive; bool sticksActive;
float NSIntegral; float previousVelocityRoll;
float previousVelocityPitch;
float EWIntegral; float EWIntegral;
float NSIntegral;
float rollI;
float pitchI;
} posHoldState; } posHoldState;
static posHoldState posHold = { static posHoldState posHold = {
.distanceCm = 0.0f, .distanceCm = 0.0f,
.previousDistanceCm = 0.0f, .previousDistanceCm = 0.0f,
.sanityCheckDistance = 1000.0f, .sanityCheckDistance = 1000.0f,
.previousVelocity = 0.0f,
.initialHeadingDeg = 0.0f, .initialHeadingDeg = 0.0f,
.iTermRoll = 0.0f,
.iTermPitch = 0.0f,
.isStarting = false, .isStarting = false,
.peakInitialGroundspeed = 0.0f, .peakInitialGroundspeed = 0.0f,
.lpfCutoff = 1.0f, .lpfCutoff = 1.0f,
.sticksActive = false, .sticksActive = false,
.NSIntegral = 0.0f, .previousVelocityRoll = 0.0f,
.previousVelocityPitch = 0.0f,
.EWIntegral = 0.0f, .EWIntegral = 0.0f,
.NSIntegral = 0.0f,
.rollI = 0.0f,
.pitchI = 0.0f,
}; };
static gpsLocation_t currentTargetLocation = {0, 0, 0}; static gpsLocation_t currentTargetLocation = {0, 0, 0};
static gpsLocation_t previousLocation = {0, 0, 0};
float autopilotAngle[ANGLE_INDEX_COUNT]; float autopilotAngle[ANGLE_INDEX_COUNT];
static pt1Filter_t velocityPitchLpf;
static pt1Filter_t velocityRollLpf; static pt1Filter_t velocityRollLpf;
static pt1Filter_t velocityPitchLpf;
static pt2Filter_t accelerationRollLpf; static pt2Filter_t accelerationRollLpf;
static pt2Filter_t accelerationPitchLpf; static pt2Filter_t accelerationPitchLpf;
@ -159,7 +162,8 @@ void resetPositionControlParams(void) { // at the start, and while sticks are mo
posHold.distanceCm = 0.0f; posHold.distanceCm = 0.0f;
posHold.previousDistanceCm = 0.0f; posHold.previousDistanceCm = 0.0f;
posHold.sanityCheckDistance = 1000.0f; posHold.sanityCheckDistance = 1000.0f;
posHold.previousVelocity = 0.0f; posHold.previousVelocityRoll = 0.0f;
posHold.previousVelocityPitch = 0.0f;
posHold.lpfCutoff = autopilotConfig()->position_cutoff * 0.01f; posHold.lpfCutoff = autopilotConfig()->position_cutoff * 0.01f;
const float pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); const float pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f);
// reset all lowpass filter accumulators to zero // reset all lowpass filter accumulators to zero
@ -173,12 +177,13 @@ void resetPositionControlParams(void) { // at the start, and while sticks are mo
void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start
currentTargetLocation = initialTargetLocation; currentTargetLocation = initialTargetLocation;
resetPositionControlParams(); resetPositionControlParams();
posHold.iTermRoll = 0.0f;
posHold.iTermPitch = 0.0f;
posHold.peakInitialGroundspeed = 0.0f; posHold.peakInitialGroundspeed = 0.0f;
posHold.initialHeadingDeg = attitude.values.yaw * 0.1f; posHold.initialHeadingDeg = attitude.values.yaw * 0.1f;
posHold.NSIntegral = 0.0f;
posHold.EWIntegral = 0.0f; posHold.EWIntegral = 0.0f;
posHold.NSIntegral = 0.0f;
posHold.rollI = 0.0f;
posHold.pitchI = 0.0f;
previousLocation = gpsSol.llh;
} }
void updateTargetLocation(gpsLocation_t newTargetLocation) { void updateTargetLocation(gpsLocation_t newTargetLocation) {
@ -211,10 +216,9 @@ bool positionControl(void) {
// get distance and bearing from current location (gpsSol.llh) to target location // get distance and bearing from current location (gpsSol.llh) to target location
GPS_distance_cm_bearing(&gpsSol.llh, &currentTargetLocation, false, &distanceCm, &bearing); GPS_distance_cm_bearing(&gpsSol.llh, &currentTargetLocation, false, &distanceCm, &bearing);
posHold.distanceCm = (float)distanceCm; posHold.distanceCm = (float)distanceCm;
float bearingDeg = bearing * 0.01f;
float headingDeg = attitude.values.yaw * 0.1f; float headingDeg = attitude.values.yaw * 0.1f;
float bearingDeg = bearing * 0.01f;
// at the start, if the quad was moving, it will initially show increasing distance from start point // at the start, if the quad was moving, it will initially show increasing distance from start point
// once it has 'stopped' the PIDs will push back towards home, and the distance away will decrease // once it has 'stopped' the PIDs will push back towards home, and the distance away will decrease
@ -245,6 +249,8 @@ bool positionControl(void) {
} }
} }
posHold.previousDistanceCm = posHold.distanceCm;
float pt1Gain = pt1FilterGain(posHold.lpfCutoff, gpsDataIntervalS); float pt1Gain = pt1FilterGain(posHold.lpfCutoff, gpsDataIntervalS);
// ** simple (too simple) sanity check ** // ** simple (too simple) sanity check **
@ -264,119 +270,62 @@ bool positionControl(void) {
normalisedErrorAngle -= 360.0f; // Range: -180 to 180 normalisedErrorAngle -= 360.0f; // Range: -180 to 180
} }
// Calculate distance error proportions for pitch and roll // Calculate distance proportions for pitch and roll
const float errorAngleRadians = normalisedErrorAngle * RAD; const float errorAngleRadians = normalisedErrorAngle * RAD;
const float rollProportion = -sin_approx(errorAngleRadians); // + 1 when target is left, -1 when to right, of the craft const float rollProportion = -sin_approx(errorAngleRadians); // + 1 when target is left, -1 when to right, of the craft
const float pitchProportion = cos_approx(errorAngleRadians); // + 1 when target is ahead, -1 when behind, the craft const float pitchProportion = cos_approx(errorAngleRadians); // + 1 when target is ahead, -1 when behind, the craft
// P // ** P **
const float rollP = rollProportion * posHold.distanceCm * positionPidCoeffs.Kp; const float rollP = rollProportion * posHold.distanceCm * positionPidCoeffs.Kp;
const float pitchP = pitchProportion * posHold.distanceCm * positionPidCoeffs.Kp; const float pitchP = pitchProportion * posHold.distanceCm * positionPidCoeffs.Kp;
// derivative and acceleration // ** D ** //
// note: here we just want no velocity, so use gps groundspeed
// adjust response angle based on drift angle compared to nose of quad
// ie yaw attitude (angle of quad nose) minus groundcourse (drift direction) both in earth frame
float errorGroundCourse = (attitude.values.yaw - gpsSol.groundCourse) * 0.1f; // get change in distance in NS and EW directions from gps.c
float normGCE = fmodf(errorGroundCourse + 360.0f, 360.0f); float deltaDistanceNS;
if (normGCE > 180.0f) { float deltaDistanceEW;
normGCE -= 360.0f; // Range: -180 to 180 GPS_distances(&gpsSol.llh, &previousLocation, &deltaDistanceNS, &deltaDistanceEW);
previousLocation = gpsSol.llh;
const float velocityNS = deltaDistanceNS * gpsDataFreqHz;
const float velocityEW = deltaDistanceEW * gpsDataFreqHz;
// get sin and cos of current heading
float headingRads = headingDeg * RAD;
if (headingRads > M_PIf) {
headingRads -= 2 * M_PIf;
} else if (headingRads < -M_PIf) {
headingRads += 2 * M_PIf;
} }
const float normCGERadians = normGCE * RAD; const float sinHeading = sin_approx(headingRads);
const float rollVelProp = sin_approx(normCGERadians); // +1 when drifting rightwards, -1 when drifting leftwards const float cosHeading = cos_approx(headingRads);
const float pitchVelProp = -cos_approx(normCGERadians); // +1 when drifting backwards, -1 when drifting forwards
float velocity = gpsSol.groundSpeed; //rotate earth to quad frame, correcting the sign (hopefully)
float acceleration = (velocity - posHold.previousVelocity) * gpsDataFreqHz; // positive when moving away float velocityRoll = -sinHeading * velocityNS + cosHeading * velocityEW;
posHold.previousVelocity = velocity; float velocityPitch = cosHeading * velocityNS + sinHeading * velocityEW;
// include a target based D element. This is smoother and complements groundcourse measurements. float accelerationRoll = (velocityRoll - posHold.previousVelocityRoll) * gpsDataFreqHz;
float velocityToTarget = (posHold.distanceCm - posHold.previousDistanceCm) * gpsDataFreqHz; posHold.previousVelocityRoll = velocityRoll;
posHold.previousDistanceCm = posHold.distanceCm; float accelerationPitch = (velocityPitch - posHold.previousVelocityPitch) * gpsDataFreqHz;
posHold.previousVelocityPitch = velocityPitch;
// roll
float velocityRoll = rollVelProp * velocity + velocityToTarget * rollProportion;
float accelerationRoll = rollVelProp * acceleration;
// lowpass filters // lowpass filters
pt1FilterUpdateCutoff(&velocityRollLpf, pt1Gain); pt1FilterUpdateCutoff(&velocityRollLpf, pt1Gain);
velocityRoll = pt1FilterApply(&velocityRollLpf, velocityRoll); velocityRoll = pt1FilterApply(&velocityRollLpf, velocityRoll);
pt2FilterUpdateCutoff(&accelerationRollLpf, pt1Gain); pt2FilterUpdateCutoff(&accelerationRollLpf, pt1Gain);
accelerationRoll = pt2FilterApply(&accelerationRollLpf, accelerationRoll); accelerationRoll = pt2FilterApply(&accelerationRollLpf, accelerationRoll);
float rollD = velocityRoll * positionPidCoeffs.Kd; float rollD = velocityRoll * positionPidCoeffs.Kd;
float rollA = accelerationRoll * positionPidCoeffs.Kf; float rollA = accelerationRoll * positionPidCoeffs.Kf;
// pitch
float velocityPitch = pitchVelProp * velocity + velocityToTarget * pitchProportion;
float accelerationPitch = pitchVelProp * acceleration;
// lowpass filters
pt1FilterUpdateCutoff(&velocityPitchLpf, pt1Gain); pt1FilterUpdateCutoff(&velocityPitchLpf, pt1Gain);
velocityPitch = pt1FilterApply(&velocityPitchLpf, velocityPitch); velocityPitch = pt1FilterApply(&velocityPitchLpf, velocityPitch);
pt2FilterUpdateCutoff(&accelerationPitchLpf, pt1Gain); pt2FilterUpdateCutoff(&accelerationPitchLpf, pt1Gain);
accelerationPitch = pt2FilterApply(&accelerationPitchLpf, accelerationPitch); accelerationPitch = pt2FilterApply(&accelerationPitchLpf, accelerationPitch);
float pitchD = velocityPitch * positionPidCoeffs.Kd; float pitchD = velocityPitch * positionPidCoeffs.Kd;
float pitchA = accelerationPitch * positionPidCoeffs.Kf; float pitchA = accelerationPitch * positionPidCoeffs.Kf;
// ** test code to handle user yaw inputs ** //
if (autopilotConfig()->position_allow_yaw) {
// code that we know is effective for fixing user yaw inputs (none at present lol)
if (autopilotConfig()->position_test_yaw_fix) {
// iTerm
// Note: accumulated iTerm opposes wind, which is a constant earth-referenced vector
// Hence we accumulate earth referenced iTerm
// The sign of the iTerm input is determined in relation to the average error bearing angle
// Finally the abs value of the accumulated iTerm is proportioned to pitch and roll
// Based on the difference in angle between the nose of the quad and the current error bearing
if (!posHold.isStarting){
// only add to iTerm while not actively stopping
float bearingRadians = bearingDeg * RAD; // 0-360, so constrain to +/- π
if (bearingRadians > M_PIf) {
bearingRadians -= 2 * M_PIf;
} else if (bearingRadians < -M_PIf) {
bearingRadians += 2 * M_PIf;
}
const float error = posHold.distanceCm * positionPidCoeffs.Ki * gpsDataIntervalS;
// NS means NorthSouth in earth frame
const float NSError = -cos_approx(bearingRadians) * error;
posHold.NSIntegral += NSError; // simple non-leaky integrator
const float EWError = sin_approx(bearingRadians) * error;
posHold.EWIntegral += EWError;
// averaged correction angle, radians from North, Earth frame of reference
float EFIntegralAngleRads = atan2_approx(posHold.NSIntegral, posHold.EWIntegral);
// heading of the quad in radians
float headingRadians = headingDeg * RAD;
// get the error angle between quad heading and iTerm vector
float headingErrorRads = headingRadians - EFIntegralAngleRads;
// ensure in range +/- π
while (headingErrorRads > M_PIf) {
headingErrorRads -= 2 * M_PIf; // Wrap to the left
}
while (headingErrorRads < -M_PIf) {
headingErrorRads += 2 * M_PIf; // Wrap to the right
}
// get correction factors for roll and pitch based on error angle
posHold.iTermRoll = -sin_approx(headingErrorRads) * fabsf(posHold.NSIntegral) + cos_approx(headingErrorRads) * fabsf(posHold.EWIntegral); // +1 when iTerm points left, -1 when iTerm points right
posHold.iTermPitch = cos_approx(headingErrorRads) * fabsf(posHold.NSIntegral) + sin_approx(headingErrorRads) * fabsf(posHold.EWIntegral); // +1 when iTerm points nose forward, -1 when iTerm should pitch back
} else {
// while moving sticks, slowly leak iTerm away, approx 3s time constant
const float leak = 1.0f - 0.25f * gpsDataIntervalS; // assumes gpsDataIntervalS not more than 1.0s
posHold.NSIntegral *= leak;
posHold.EWIntegral *= leak;
}
}
}
// limit sum of D and A because otherwise too aggressive if entering at speed // limit sum of D and A because otherwise too aggressive if entering at speed
float rollDA = rollD + rollA; float rollDA = rollD + rollA;
float pitchDA = pitchD + pitchA; float pitchDA = pitchD + pitchA;
@ -386,10 +335,63 @@ bool positionControl(void) {
rollDA = constrainf(rollDA, -maxDAAngle, maxDAAngle); rollDA = constrainf(rollDA, -maxDAAngle, maxDAAngle);
pitchDA = constrainf(pitchDA, -maxDAAngle, maxDAAngle); pitchDA = constrainf(pitchDA, -maxDAAngle, maxDAAngle);
// add up pid factors // iTerm
// const float pidSumRoll = rollP + posHold.iTermRoll + rollDA; // Note: accumulated iTerm opposes wind, which is a constant earth-referenced vector
const float pidSumRoll = rollP + posHold.iTermRoll + rollDA; // Hence we accumulate earth referenced iTerm
const float pidSumPitch = pitchP + posHold.iTermPitch + pitchDA; // The sign of the iTerm input is determined in relation to the average error bearing angle
// Finally the abs value of the accumulated iTerm is proportioned to pitch and roll
// Based on the difference in angle between the nose of the quad and the current error bearing
if (!posHold.isStarting){
// only add to iTerm while not actively stopping
float bearingRadians = bearingDeg * RAD; // 0-360, so constrain to +/- π
if (bearingRadians > M_PIf) {
bearingRadians -= 2 * M_PIf;
} else if (bearingRadians < -M_PIf) {
bearingRadians += 2 * M_PIf;
}
const float error = posHold.distanceCm * positionPidCoeffs.Ki * gpsDataIntervalS;
// NS means NorthSouth in earth frame
const float NSError = -cos_approx(bearingRadians) * error;
posHold.NSIntegral += NSError; // simple non-leaky integrator
const float EWError = sin_approx(bearingRadians) * error;
posHold.EWIntegral += EWError;
// averaged iTerm correction vector, radians from North, Earth frame of reference
float EFIntegralAngleRads = atan2_approx(posHold.NSIntegral, posHold.EWIntegral);
// heading of the quad in radians
float headingRadians = headingDeg * RAD;
// get the error angle between quad heading and iTerm vector
float headingErrorRads = headingRadians - EFIntegralAngleRads;
// ensure in range +/- π
while (headingErrorRads > M_PIf) {
headingErrorRads -= 2 * M_PIf; // Wrap to the left
}
while (headingErrorRads < -M_PIf) {
headingErrorRads += 2 * M_PIf; // Wrap to the right
}
// get correction factors for roll and pitch, based on quad to average iTerm vector angle
const float sinHeadingError = sin_approx(headingErrorRads);
const float cosHeadingError = cos_approx(headingErrorRads);
// rotate NS and EW iTerm vectors to quad frame of reference
posHold.rollI = -sinHeadingError * fabsf(posHold.NSIntegral) + cosHeadingError * fabsf(posHold.EWIntegral); // +1 when iTerm points left, -1 when iTerm points right
posHold.pitchI = cosHeadingError * fabsf(posHold.NSIntegral) + sinHeadingError * fabsf(posHold.EWIntegral); // +1 when iTerm points nose forward, -1 when iTerm should pitch back
} else {
// while moving sticks, slowly leak iTerm away, approx 3s time constant
const float leak = 1.0f - 0.25f * gpsDataIntervalS; // assumes gpsDataIntervalS not more than 1.0s
posHold.NSIntegral *= leak;
posHold.EWIntegral *= leak;
}
const float pidSumRoll = rollP + posHold.rollI + rollDA;
const float pidSumPitch = pitchP + posHold.pitchI + pitchDA;
// todo: upsample filtering // todo: upsample filtering
// pidSum will have steps at GPS rate, and may require an upsampling filter for smoothness. // pidSum will have steps at GPS rate, and may require an upsampling filter for smoothness.
@ -410,21 +412,20 @@ bool positionControl(void) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(bearingDeg)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(bearingDeg));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * rollProportion)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * rollProportion));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumRoll * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumRoll * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(velocityToTarget * rollProportion)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(velocityEW * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(rollP * 10)); // degrees*10 DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(rollP * 10)); // degrees*10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.iTermRoll * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.rollI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(rollDA * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(rollDA * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.EWIntegral * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.EWIntegral * 10));
} else { } else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(bearingDeg));; DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(bearingDeg));;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * pitchProportion)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(-posHold.distanceCm * pitchProportion));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumPitch * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(pidSumPitch * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(velocityToTarget * pitchProportion)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(velocityNS *10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pitchP * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pitchP * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.iTermPitch * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(posHold.pitchI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pitchD * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pitchDA * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pitchVelProp * velocity)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.NSIntegral * 10));
// DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(posHold.NSIntegral * 10));
} }
return true; return true;
} }

9
src/main/io/gps.c
View file

@ -2594,6 +2594,15 @@ void GPS_calculateDistanceAndDirectionToHome(void)
} }
} }
void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pNSDist, float *pEWDist) {
if (pNSDist) {
*pNSDist = (to->lat - from->lat) * EARTH_ANGLE_TO_CM; // North-South distance, positive North
}
if (pEWDist) {
*pEWDist = (to->lon - from->lon) * GPS_cosLat * EARTH_ANGLE_TO_CM; // East-West distance, positive East
}
}
void onGpsNewData(void) void onGpsNewData(void)
{ {
if (!STATE(GPS_FIX)) { if (!STATE(GPS_FIX)) {

1
src/main/io/gps.h
View file

@ -389,6 +389,7 @@ void onGpsNewData(void);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat); 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 GPS_distance_cm_bearing(const gpsLocation_t *from, const gpsLocation_t *to, bool dist3d, uint32_t *dist, int32_t *bearing);
void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pNSDist, float *pEWDist);
void gpsSetFixState(bool state); void gpsSetFixState(bool state);
float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue