Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 08:45:36 +03:00
Code Issues Wiki Activity

updates from reviews, thanks karate

Browse source
This commit is contained in:
ctzsnooze 2024-11-19 15:03:47 +11:00
parent a429707ae2
commit b6c0c9ff77
17 changed files with 125 additions and 106 deletions
Download patch file Download diff file Expand all files Collapse all files

7
src/main/blackbox/blackbox.c
View file

@ -62,14 +62,11 @@
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/position.h"
#include "flight/pos_hold.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h"
#include "flight/imu.h"
@ -80,8 +77,12 @@
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/alt_hold.h"
#include "pg/autopilot.h"
#include "pg/motor.h"
#include "pg/rx.h"
#include "pg/pos_hold.h"
#include "rx/rx.h"

5
src/main/cms/cms_menu_gps_rescue.c
View file

@ -37,10 +37,11 @@
#include "config/config.h"
#include "flight/autopilot.h"
#include "flight/gps_rescue.h"
#include "flight/position.h"
#include "pg/autopilot.h"
#include "pg/gps_rescue.h"
static uint16_t gpsRescueConfig_minStartDistM; //meters
static uint8_t gpsRescueConfig_altitudeMode;
static uint16_t gpsRescueConfig_initialClimbM; // meters

2
src/main/fc/init.c
View file

@ -832,7 +832,7 @@ void init(void)
#endif
positionInit();
autopilotInit(autopilotConfig());
autopilotInit();
#if defined(USE_VTX_COMMON) || defined(USE_VTX_CONTROL)
vtxTableInit();

3
src/main/fc/rc.c
View file

@ -47,9 +47,6 @@
#include "flight/pid_init.h"
#include "pg/rx.h"
#include "pg/pos_hold.h"
#include "pg/autopilot.h"
#include "rx/rx.h"
#include "sensors/battery.h"

1
src/main/flight/alt_hold.c
View file

@ -32,6 +32,7 @@
#include "flight/position.h"
#include "rx/rx.h"
#include "pg/autopilot.h"
#include "alt_hold.h"

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

@ -34,6 +34,7 @@
#include "rx/rx.h"
#include "sensors/gyro.h"
#include "pg/autopilot.h"
#include "autopilot.h"
#define ALTITUDE_P_SCALE 0.01f
@ -52,7 +53,7 @@ static pidCoefficient_t positionPidCoeffs;
static float altitudeI = 0.0f;
static float throttleOut = 0.0f;
typedef struct {
typedef struct earthFrame_s {
bool isStopping;
float distance;
float previousDistance;
@ -68,7 +69,7 @@ typedef enum {
NS
} axisEF_t;
typedef struct {
typedef struct autopilotState_s {
float gpsDataIntervalS;
float gpsDataFreqHz;
float sanityCheckDistance;
@ -80,9 +81,9 @@ typedef struct {
float pidSumCraft[EF_AXIS_COUNT];
pt3Filter_t upsample[EF_AXIS_COUNT];
earthFrame_t efAxis[EF_AXIS_COUNT];
} posHoldState;
} autopilotState_t;
static posHoldState posHold = {
static autopilotState_t ap = {
.gpsDataIntervalS = 0.1f,
.gpsDataFreqHz = 10.0f,
.sanityCheckDistance = 1000.0f,
@ -100,27 +101,27 @@ float autopilotAngle[RP_AXIS_COUNT];
void resetPositionControlEFParams(earthFrame_t *efAxis)
{
// at start only
pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain); // Clear and initialise the filters
pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain);
pt1FilterInit(&efAxis->velocityLpf, ap.pt1Gain); // Clear and initialise the filters
pt1FilterInit(&efAxis->accelerationLpf, ap.pt1Gain);
efAxis->isStopping = true; // Enter starting 'phase'
efAxis->integral = 0.0f;
}
void resetPt3UpsampleFilters(void)
{
pt3FilterInit(&posHold.upsample[AI_ROLL], posHold.upsampleCutoff);
pt3FilterInit(&posHold.upsample[AI_PITCH], posHold.upsampleCutoff);
pt3FilterInit(&ap.upsample[AI_ROLL], ap.upsampleCutoff);
pt3FilterInit(&ap.upsample[AI_PITCH], ap.upsampleCutoff);
}
void resetPositionControl(gpsLocation_t initialTargetLocation)
void resetPositionControl(gpsLocation_t *initialTargetLocation)
{
// from pos_hold.c when initiating position hold at target location
currentTargetLocation = initialTargetLocation;
posHold.sticksActive = false;
currentTargetLocation = *initialTargetLocation;
ap.sticksActive = false;
// set sanity check distance according to groundspeed at start
posHold.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
resetPositionControlEFParams(&posHold.efAxis[EW]);
resetPositionControlEFParams(&posHold.efAxis[NS]);
ap.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
resetPositionControlEFParams(&ap.efAxis[EW]);
resetPositionControlEFParams(&ap.efAxis[NS]);
resetPt3UpsampleFilters(); // clear anything from previous iteration
}
@ -129,26 +130,26 @@ void initializeEfAxisFilters(earthFrame_t *efAxis, float filterGain) {
pt1FilterInit(&efAxis->accelerationLpf, filterGain);
}
void autopilotInit(const autopilotConfig_t *config)
void autopilotInit(void)
{
posHold.sticksActive = false;
posHold.maxAngle = autopilotConfig()->max_angle;
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
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 * POSITION_P_SCALE;
positionPidCoeffs.Ki = config->position_I * POSITION_I_SCALE;
positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE;
positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration
ap.sticksActive = false;
ap.maxAngle = autopilotConfig()->max_angle;
altitudePidCoeffs.Kp = autopilotConfig()->altitude_P * ALTITUDE_P_SCALE;
altitudePidCoeffs.Ki = autopilotConfig()->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = autopilotConfig()->altitude_D * ALTITUDE_D_SCALE;
altitudePidCoeffs.Kf = autopilotConfig()->altitude_F * ALTITUDE_F_SCALE;
positionPidCoeffs.Kp = autopilotConfig()->position_P * POSITION_P_SCALE;
positionPidCoeffs.Ki = autopilotConfig()->position_I * POSITION_I_SCALE;
positionPidCoeffs.Kd = autopilotConfig()->position_D * POSITION_D_SCALE;
positionPidCoeffs.Kf = autopilotConfig()->position_A * POSITION_A_SCALE; // Kf used for acceleration
// initialise filters with approximate filter gain
posHold.upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate
ap.upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate
resetPt3UpsampleFilters();
// Initialise PT1 filters for earth frame axes NS and EW
posHold.pt1Cutoff = config->position_cutoff * 0.01f;
posHold.pt1Gain = pt1FilterGain(posHold.pt1Cutoff, 0.1f); // assume 10Hz GPS connection at start
initializeEfAxisFilters(&posHold.efAxis[EW], posHold.pt1Gain);
initializeEfAxisFilters(&posHold.efAxis[NS], posHold.pt1Gain);
ap.pt1Cutoff = autopilotConfig()->position_cutoff * 0.01f;
ap.pt1Gain = pt1FilterGain(ap.pt1Cutoff, 0.1f); // assume 10Hz GPS connection at start
initializeEfAxisFilters(&ap.efAxis[EW], ap.pt1Gain);
initializeEfAxisFilters(&ap.efAxis[NS], ap.pt1Gain);
}
void resetAltitudeControl (void) {
@ -207,20 +208,20 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAl
void setSticksActiveStatus(bool areSticksActive)
{
posHold.sticksActive = areSticksActive;
ap.sticksActive = areSticksActive;
}
void setTargetLocation(gpsLocation_t newTargetLocation)
{
currentTargetLocation = newTargetLocation;
posHold.efAxis[EW].previousDistance = 0.0f; // reset to avoid D and A spikess
posHold.efAxis[NS].previousDistance = 0.0f;
ap.efAxis[EW].previousDistance = 0.0f; // reset to avoid D and A spikess
ap.efAxis[NS].previousDistance = 0.0f;
// function is intended for only small changes in position
// for example, where the step distance change reflects an intended velocity, determined by a client function
// if we had a 'target_ground_speed' value, like in gps_rescue, we can make a function that starts and stops smoothly and targets that velocity
}
void resetLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
void updateLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
{
if (loopAxis == EW) {
currentTargetLocation.lon = gpsSol.llh.lon; // update East-West / / longitude position
@ -234,62 +235,63 @@ bool positionControl(void)
{
static uint16_t gpsStamp = 0;
if (gpsHasNewData(&gpsStamp)) {
posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
ap.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.05 - 2.5s
ap.gpsDataFreqHz = getGpsDataFrequencyHz();
// first get NS and EW distances from current location (gpsSol.llh) to target location
vector2_t gpsDistance;
GPS_latLongVectors(&gpsSol.llh, &currentTargetLocation, &gpsDistance.x, &gpsDistance.y); // X is EW, Y is NS
posHold.efAxis[EW].distance = gpsDistance.x;
posHold.efAxis[NS].distance = gpsDistance.y;
ap.efAxis[EW].distance = gpsDistance.x;
ap.efAxis[NS].distance = gpsDistance.y;
const float distanceCm = vector2Norm(&gpsDistance);
const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS;
const float leak = 1.0f - 0.4f * ap.gpsDataIntervalS;
// leak iTerm while sticks are centered, 2s time constant approximately
const float lpfGain = pt1FilterGain(posHold.pt1Cutoff, posHold.gpsDataIntervalS);
const float lpfGain = pt1FilterGain(ap.pt1Cutoff, ap.gpsDataIntervalS);
// ** Sanity check **
// primarily to detect flyaway from no Mag or badly oriented Mag
// must accept some overshoot at the start, especially if entering at high speed
if (distanceCm > posHold.sanityCheckDistance) {
if (distanceCm > ap.sanityCheckDistance) {
return false;
}
static float prevPidDASquared = 0.0f; // if we limit DA on true vector length
const float maxDAAngle = 35.0f; // limit in degrees; arbitrary angle
const float sqMaxDAAngle = sq(maxDAAngle);
for (axisEF_t loopAxis = EW; loopAxis <= NS; loopAxis++) {
earthFrame_t *efAxis = &posHold.efAxis[loopAxis];
earthFrame_t *efAxis = &ap.efAxis[loopAxis];
// separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW)
// ** P **
const float pidP = efAxis->distance * positionPidCoeffs.Kp;
// ** I **
efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * posHold.gpsDataIntervalS;
efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * ap.gpsDataIntervalS;
// only add to iTerm while in hold phase
const float pidI = efAxis->integral * positionPidCoeffs.Ki;
// ** D ** //
// Velocity derived from GPS position works better than module supplied GPS Speed and Heading information
float velocity = (efAxis->distance - efAxis->previousDistance) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
float velocity = (efAxis->distance - efAxis->previousDistance) * ap.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
efAxis->previousDistance = efAxis->distance;
pt1FilterUpdateCutoff(&efAxis->velocityLpf, lpfGain);
const float velocityFiltered = pt1FilterApply(&efAxis->velocityLpf, velocity);
float pidD = velocityFiltered * positionPidCoeffs.Kd;
float acceleration = (velocity - efAxis->previousVelocity) * posHold.gpsDataFreqHz;
float acceleration = (velocity - efAxis->previousVelocity) * ap.gpsDataFreqHz;
efAxis->previousVelocity = velocity;
pt1FilterUpdateCutoff(&efAxis->accelerationLpf, lpfGain);
const float accelerationFiltered = pt1FilterApply(&efAxis->accelerationLpf, acceleration);
const float pidA = accelerationFiltered * positionPidCoeffs.Kf;
if (posHold.sticksActive) {
if (ap.sticksActive) {
// sticks active 'phase'
efAxis->isStopping = true;
resetLocation(efAxis, loopAxis);
updateLocation(efAxis, loopAxis);
// while sticks are moving, reset the location on each axis, to maintain a usable D value
// slowly leak iTerm away, approx 2s time constant
efAxis->integral *= leak;
@ -299,20 +301,20 @@ bool positionControl(void)
pidD *= 1.6f; // aribitrary D boost to stop more quickly when sticks are centered
if (velocity * velocityFiltered < 0.0f) {
// when an axis has nearly stopped moving, reset it and end it's start phase
resetLocation(efAxis, loopAxis);
updateLocation(efAxis, loopAxis);
efAxis->isStopping = false;
}
}
float pidDA = pidD + pidA;
// limit sum of D and A per axis based on total DA vector length, otherwise can be too aggressive when starting at speed
// limit is 35 degrees from D and A alone, arbitrary value. 20 is a bit too low, allows a lot of overshoot
// note: an angle of more than 35 degrees can still be achieved as P and I grow
float pidDA = pidD + pidA;
const float pidDASquared = pidDA * pidDA;
float mag = sqrtf(pidDASquared + prevPidDASquared);
if (mag > maxDAAngle) {
pidDA *= (maxDAAngle / mag);
const float pidDASquared = sq(pidD + pidA);
float magSq = pidDASquared + prevPidDASquared;
if (magSq > sqMaxDAAngle && magSq > 0.0f) {
pidDA *= maxDAAngle / sqrtf(magSq);
}
prevPidDASquared = pidDASquared;
@ -328,42 +330,42 @@ bool positionControl(void)
}
} // end for loop
if (posHold.sticksActive) {
if (ap.sticksActive) {
// keep update sanity check distance while sticks are out
posHold.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
ap.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
// if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
posHold.pidSumCraft[AI_ROLL] = 0.0f;
posHold.pidSumCraft[AI_PITCH] = 0.0f;
ap.pidSumCraft[AI_ROLL] = 0.0f;
ap.pidSumCraft[AI_PITCH] = 0.0f;
} else {
// ** Rotate pid Sum to quad frame of reference, into pitch and roll **
const float headingRads = DECIDEGREES_TO_RADIANS(attitude.values.yaw);
const float sinHeading = sin_approx(headingRads);
const float cosHeading = cos_approx(headingRads);
posHold.pidSumCraft[AI_ROLL] = -sinHeading * posHold.efAxis[NS].pidSum + cosHeading * posHold.efAxis[EW].pidSum;
posHold.pidSumCraft[AI_PITCH] = cosHeading * posHold.efAxis[NS].pidSum + sinHeading * posHold.efAxis[EW].pidSum;
ap.pidSumCraft[AI_ROLL] = -sinHeading * ap.efAxis[NS].pidSum + cosHeading * ap.efAxis[EW].pidSum;
ap.pidSumCraft[AI_PITCH] = cosHeading * ap.efAxis[NS].pidSum + sinHeading * ap.efAxis[EW].pidSum;
// limit angle vector to maxAngle
const float angleMagnitude = sqrtf(posHold.pidSumCraft[AI_ROLL] * posHold.pidSumCraft[AI_ROLL] + posHold.pidSumCraft[AI_PITCH] * posHold.pidSumCraft[AI_PITCH]);
if (angleMagnitude > posHold.maxAngle && angleMagnitude > 0.0f) {
const float limiter = posHold.maxAngle / angleMagnitude;
posHold.pidSumCraft[AI_ROLL] *= limiter; // Scale the roll value
posHold.pidSumCraft[AI_PITCH] *= limiter; // Scale the pitch value
const float angleMagSq = sq(ap.pidSumCraft[AI_ROLL]) + sq(ap.pidSumCraft[AI_PITCH]);
if (angleMagSq > sq(ap.maxAngle) && angleMagSq > 0.0f) {
const float limiter = ap.maxAngle / sqrtf(angleMagSq);
ap.pidSumCraft[AI_ROLL] *= limiter; // Scale the roll value
ap.pidSumCraft[AI_PITCH] *= limiter; // Scale the pitch value
}
}
}
// ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
autopilotAngle[AI_ROLL] = pt3FilterApply(&posHold.upsample[AI_ROLL], posHold.pidSumCraft[AI_ROLL]);
autopilotAngle[AI_PITCH] = pt3FilterApply(&posHold.upsample[AI_PITCH], posHold.pidSumCraft[AI_PITCH]);
autopilotAngle[AI_ROLL] = pt3FilterApply(&ap.upsample[AI_ROLL], ap.pidSumCraft[AI_ROLL]);
autopilotAngle[AI_PITCH] = pt3FilterApply(&ap.upsample[AI_PITCH], ap.pidSumCraft[AI_PITCH]);
// note: upsampling should really be done in earth frame, to avoid 10Hz wobbles if pilot yaws and the controller is applying significant pitch or roll
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[EW].distance)); // cm
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[EW].pidSum * 10)); // deg * 10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ap.efAxis[EW].distance)); // cm
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[EW].pidSum * 10)); // deg * 10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10)); // deg * 10
} else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(posHold.efAxis[NS].distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(posHold.efAxis[NS].pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ap.efAxis[NS].distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[NS].pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_PITCH] * 10));
}
return true;
@ -381,5 +383,5 @@ float getAutopilotThrottle(void)
bool isAutopilotActive(void)
{
return !posHold.sticksActive;
return !ap.sticksActive;
}

10
src/main/flight/autopilot.h
View file

@ -17,19 +17,17 @@
#pragma once
#include "pg/autopilot.h"
#include "flight/pid.h"
#include "io/gps.h"
extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void autopilotInit(const autopilotConfig_t *config);
void autopilotInit(void);
void resetAltitudeControl(void);
void setSticksActiveStatus(bool areSticksActive);
void resetPositionControl(gpsLocation_t initialTargetLocation);
void resetPositionControl(gpsLocation_t *initialTargetLocation);
void moveTargetLocation(int32_t latStep, int32_t lonStep);
void (posControlOnNewGpsData) (void);
void (posControlOutput) (void);
void posControlOnNewGpsData(void);
void posControlOutput(void);
bool positionControl(void);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float targetAltitudeStep);

5
src/main/flight/gps_rescue.c
View file

@ -46,6 +46,7 @@
#include "io/gps.h"
#include "rx/rx.h"
#include "pg/autopilot.h"
#include "sensors/acceleration.h"
#include "gps_rescue.h"
@ -512,9 +513,9 @@ static void sensorUpdate(void)
rescueState.sensor.groundSpeedCmS = gpsSol.groundSpeed; // cm/s
rescueState.sensor.gpsDataIntervalSeconds = getGpsDataIntervalSeconds();
// Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
// Range from 50ms (20hz) to 2500ms (0.4Hz). Intended to cover common GPS data rates and exclude unusual values.
rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) / rescueState.sensor.gpsDataIntervalSeconds);
rescueState.sensor.velocityToHomeCmS = ((prevDistanceToHomeCm - rescueState.sensor.distanceToHomeCm) * getGpsDataFrequencyHz());
// positive = towards home. First value is useless since prevDistanceToHomeCm was zero.
prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;

4
src/main/flight/pid.c
View file

@ -57,6 +57,8 @@
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/autopilot.h"
#include "sensors/acceleration.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
@ -577,7 +579,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
angleLimit = 85.0f; // allow autopilot to use whatever angle it needs to stop
}
// limit pilot requested angle to half the autopilot angle to avoid excess speed and chaotic stops
angleLimit = fminf (0.5f * autopilotConfig()->max_angle, angleLimit);
angleLimit = fminf(0.5f * autopilotConfig()->max_angle, angleLimit);
}
#endif

5
src/main/flight/pos_hold.c
View file

@ -34,9 +34,10 @@
#include "rx/rx.h"
#include "sensors/compass.h"
#include "pg/pos_hold.h"
#include "pos_hold.h"
typedef struct {
typedef struct posHoldState_s {
bool posHoldIsOK;
float deadband;
bool useStickAdjustment;
@ -67,7 +68,7 @@ void posHoldStart(void)
if (!isInPosHoldMode) {
// start position hold mode
posHold.posHoldIsOK = true; // true when started, false when autopilot code reports failure
resetPositionControl(gpsSol.llh); // sets target location to current location
resetPositionControl(&gpsSol.llh); // sets target location to current location
isInPosHoldMode = true;
}
} else {

2
src/main/flight/pos_hold.h
View file

@ -17,7 +17,7 @@
#pragma once
#include "pg/pos_hold.h"
// #include "pg/pos_hold.h"
#ifdef USE_POS_HOLD_MODE
#include "common/time.h"

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

@ -92,7 +92,9 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
#define GPS_TASK_DECAY_SHIFT 9 // Smoothing factor for GPS task re-scheduler
static serialPort_t *gpsPort;
static float gpsDataIntervalSeconds;
static float gpsDataIntervalSeconds = 0.1f;
static float gpsDataFrequencyHz = 10.0f;
static uint16_t currentGpsStamp = 1; // logical timer for received position update
typedef struct gpsInitData_s {
@ -1762,7 +1764,7 @@ static bool writeGpsSolutionNmea(gpsSolutionData_t *sol, const gpsDataNmea_t *da
}
navDeltaTimeMs = (msInTenSeconds + data->time - gpsData.lastNavSolTs) % msInTenSeconds;
gpsData.lastNavSolTs = data->time;
sol->navIntervalMs = constrain(navDeltaTimeMs, 100, 2500);
sol->navIntervalMs = constrain(navDeltaTimeMs, 50, 2500);
// return only one true statement to trigger one "newGpsDataReady" flag per GPS loop
return true;
@ -2609,7 +2611,8 @@ void onGpsNewData(void)
currentGpsStamp++; // new GPS data available
gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
gpsDataIntervalSeconds = gpsSol.navIntervalMs * 0.001f; // range for navIntervalMs is constrained to 50 - 2500
gpsDataFrequencyHz = 1.0f / gpsDataIntervalSeconds;
GPS_calculateDistanceAndDirectionToHome();
if (ARMING_FLAG(ARMED)) {
@ -2648,6 +2651,11 @@ float getGpsDataIntervalSeconds(void)
return gpsDataIntervalSeconds;
}
float getGpsDataFrequencyHz(void)
{
return gpsDataFrequencyHz;
}
baudRate_e getGpsPortActualBaudRateIndex(void)
{
return lookupBaudRateIndex(serialGetBaudRate(gpsPort));

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

@ -394,6 +394,7 @@ void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, floa
void gpsSetFixState(bool state);
bool gpsHasNewData(uint16_t *stamp);
float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue
float getGpsDataIntervalSeconds(void); // range 0.05 - 2.5s
float getGpsDataFrequencyHz(void); // range 20Hz - 0.4Hz
baudRate_e getGpsPortActualBaudRateIndex(void);

4
src/main/msp/msp.c
View file

@ -83,9 +83,7 @@
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
@ -119,9 +117,11 @@
#include "osd/osd_elements.h"
#include "osd/osd_warnings.h"
#include "pg/autopilot.h"
#include "pg/beeper.h"
#include "pg/board.h"
#include "pg/dyn_notch.h"
#include "pg/gps_rescue.h"
#include "pg/gyrodev.h"
#include "pg/motor.h"
#include "pg/pos_hold.h"

11
src/test/unit/althold_unittest.cc
View file

@ -32,7 +32,6 @@ extern "C" {
#include "fc/runtime_config.h"
#include "flight/alt_hold.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/imu.h"
#include "flight/pid.h"
@ -42,6 +41,9 @@ extern "C" {
#include "rx/rx.h"
#include "pg/alt_hold.h"
#include "pg/autopilot.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
@ -110,10 +112,11 @@ extern "C" {
attitudeEulerAngles_t attitude;
gpsSolutionData_t gpsSol;
float getAltitudeCm(void) {return 0.0f;}
float getAltitudeDerivative(void) {return 0.0f;}
float getAltitudeCm(void) { return 0.0f; }
float getAltitudeDerivative(void) { return 0.0f; }
float getCosTiltAngle(void) { return 0.0f; }
float getGpsDataIntervalSeconds(void) { return 0.01f; }// gpsSolutionData_t gpsSol;
float getGpsDataIntervalSeconds(void) { return 0.01f; }
float getGpsDataFrequencyHz(void) { return 10.0f; }
float rcCommand[4];
bool gpsHasNewData(uint16_t* gpsStamp) {

6
src/test/unit/arming_prevention_unittest.cc
View file

@ -36,7 +36,6 @@ extern "C" {
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h"
#include "flight/gps_rescue.h"
#include "flight/imu.h"
@ -48,11 +47,13 @@ extern "C" {
#include "io/beeper.h"
#include "io/gps.h"
#include "pg/autopilot.h"
#include "pg/gps_rescue.h"
#include "pg/motor.h"
#include "pg/rx.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "pg/rx.h"
#include "rx/rx.h"
@ -100,6 +101,7 @@ extern "C" {
uint8_t activePidLoopDenom = 1;
float getGpsDataIntervalSeconds(void) { return 0.1f; }
float getGpsDataFrequencyHz(void) { return 10.0f; }
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) { filter->k = k; }
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) { filter->k = k; }
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k) { filter->k = k; }

3
src/test/unit/flight_imu_unittest.cc
View file

@ -42,7 +42,6 @@ extern "C" {
#include "fc/runtime_config.h"
#include "fc/rc.h"
#include "flight/autopilot.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
@ -52,6 +51,8 @@ extern "C" {
#include "rx/rx.h"
#include "pg/autopilot.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"