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

more updates from PL review

Browse source
This commit is contained in:
ctzsnooze 2024-11-20 08:23:36 +11:00
parent 4d9b17e1fe
commit 418ee0b0cf
8 changed files with 79 additions and 76 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -65,33 +65,31 @@ typedef struct earthFrame_s {
} earthFrame_t;
typedef enum {
EW = 0,
NS
lat = 0,
lon
} axisEF_t;
typedef struct autopilotState_s {
float gpsDataIntervalS;
float gpsDataFreqHz;
float sanityCheckDistance;
float upsampleCutoff;
float upsampleCutoffBF;
float pt1Cutoff;
float pt1Gain;
bool sticksActive;
float maxAngle;
float pidSumCraft[EF_AXIS_COUNT];
pt3Filter_t upsample[EF_AXIS_COUNT];
float pidSumBF[RP_AXIS_COUNT];
float iTermLeakGain;
pt3Filter_t upsampleBF[RP_AXIS_COUNT];
earthFrame_t efAxis[EF_AXIS_COUNT];
} autopilotState_t;
static autopilotState_t ap = {
.gpsDataIntervalS = 0.1f,
.gpsDataFreqHz = 10.0f,
.sanityCheckDistance = 1000.0f,
.pt1Cutoff = 1.0f,
.pt1Gain = 1.0f,
.sticksActive = false,
.pidSumCraft = { 0.0f, 0.0f },
.upsample = { {0}, {0} },
.iTermLeakGain = 0.0f,
.pidSumBF = { 0.0f, 0.0f },
.upsampleBF = { {0}, {0} },
.efAxis = { {0} }
};
@ -105,24 +103,26 @@ void resetPositionControlEFParams(earthFrame_t *efAxis)
pt1FilterInit(&efAxis->accelerationLpf, ap.pt1Gain);
efAxis->isStopping = true; // Enter starting 'phase'
efAxis->integral = 0.0f;
}
void resetPt3UpsampleFilters(void)
{
pt3FilterInit(&ap.upsample[AI_ROLL], ap.upsampleCutoff);
pt3FilterInit(&ap.upsample[AI_PITCH], ap.upsampleCutoff);
pt3FilterInit(&ap.upsampleBF[AI_ROLL], ap.upsampleCutoffBF);
pt3FilterInit(&ap.upsampleBF[AI_PITCH], ap.upsampleCutoffBF);
}
void resetPositionControl(gpsLocation_t *initialTargetLocation)
void resetPositionControl(const gpsLocation_t *initialTargetLocation)
{
// from pos_hold.c when initiating position hold at target location
currentTargetLocation = *initialTargetLocation;
ap.sticksActive = false;
// set sanity check distance according to groundspeed at start
ap.sanityCheckDistance = gpsSol.groundSpeed > 500 ? gpsSol.groundSpeed * 2.0f : 1000.0f;
resetPositionControlEFParams(&ap.efAxis[EW]);
resetPositionControlEFParams(&ap.efAxis[NS]);
// set sanity check distance according to groundspeed at start, minimum of 10m
ap.sanityCheckDistance = fmaxf(1000.0f, gpsSol.groundSpeed * 2.0f);
resetPositionControlEFParams(&ap.efAxis[lat]);
resetPositionControlEFParams(&ap.efAxis[lon]);
resetPt3UpsampleFilters(); // clear anything from previous iteration
ap.iTermLeakGain = 0.0f; // force an update of the leak gain value
}
void initializeEfAxisFilters(earthFrame_t *efAxis, float filterGain) {
@ -143,13 +143,13 @@ void autopilotInit(void)
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
ap.upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate
ap.upsampleCutoffBF = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate
resetPt3UpsampleFilters();
// Initialise PT1 filters for earth frame axes NS and EW
// Initialise PT1 filters for earth frame axes latitude and longitude
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);
initializeEfAxisFilters(&ap.efAxis[lat], ap.pt1Gain);
initializeEfAxisFilters(&ap.efAxis[lon], ap.pt1Gain);
}
void resetAltitudeControl (void) {
@ -211,11 +211,11 @@ void setSticksActiveStatus(bool areSticksActive)
ap.sticksActive = areSticksActive;
}
void setTargetLocation(gpsLocation_t newTargetLocation)
void setTargetLocation(const gpsLocation_t newTargetLocation)
{
currentTargetLocation = newTargetLocation;
ap.efAxis[EW].previousDistance = 0.0f; // reset to avoid D and A spikess
ap.efAxis[NS].previousDistance = 0.0f;
ap.efAxis[lat].previousDistance = 0.0f;
ap.efAxis[lon].previousDistance = 0.0f; // reset to avoid D and A spikess
// 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
@ -223,10 +223,10 @@ void setTargetLocation(gpsLocation_t newTargetLocation)
void updateLocation(earthFrame_t *efAxis, axisEF_t loopAxis)
{
if (loopAxis == EW) {
currentTargetLocation.lon = gpsSol.llh.lon; // update East-West / / longitude position
if (loopAxis == lat) {
currentTargetLocation.lat = gpsSol.llh.lat; // update latitude position
} else {
currentTargetLocation.lat = gpsSol.llh.lat; // update North-South / latitude position
currentTargetLocation.lon = gpsSol.llh.lon; // update longitude position
}
efAxis->previousDistance = 0.0f; // and reset the previous distance
}
@ -235,21 +235,17 @@ bool positionControl(void)
{
static uint16_t gpsStamp = 0;
if (gpsHasNewData(&gpsStamp)) {
ap.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.05 - 2.5s
ap.gpsDataFreqHz = getGpsDataFrequencyHz();
const float gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.05 - 2.5s
const float gpsDataFreqHz = getGpsDataFrequencyHz();
// first get NS and EW distances from current location (gpsSol.llh) to target location
// get lat and long 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
ap.efAxis[EW].distance = gpsDistance.x;
ap.efAxis[NS].distance = gpsDistance.y;
GPS_latLongVectors(&gpsSol.llh, &currentTargetLocation, &gpsDistance.v[lat], &gpsDistance.v[lon]); // X is lon, Y is lat
ap.efAxis[lat].distance = gpsDistance.v[lat];
ap.efAxis[lon].distance = gpsDistance.v[lon];
const float distanceCm = vector2Norm(&gpsDistance);
const float leak = 1.0f - 0.4f * ap.gpsDataIntervalS;
// leak iTerm while sticks are centered, 2s time constant approximately
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
@ -257,32 +253,39 @@ bool positionControl(void)
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 lpfGain = pt1FilterGain(ap.pt1Cutoff, gpsDataIntervalS);
if (ap.iTermLeakGain == 0.0f) {
const float leakTimeConstant = 2.5f; // 2.5s time constant, set this only once, it's not critical
ap.iTermLeakGain = pt1FilterGainFromDelay(leakTimeConstant, gpsDataIntervalS);
}
static float prevPidDASquared = 0.0f; // we limit DA on 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++) {
for (axisEF_t loopAxis = lat; loopAxis <= lon; loopAxis++) {
earthFrame_t *efAxis = &ap.efAxis[loopAxis];
// separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW)
// separate PID controllers for latitude and longitude
// ** P **
const float pidP = efAxis->distance * positionPidCoeffs.Kp;
// ** I **
efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * ap.gpsDataIntervalS;
efAxis->integral += efAxis->isStopping ? 0.0f : efAxis->distance * 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) * ap.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
float velocity = (efAxis->distance - efAxis->previousDistance) * gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s, at equator, at 10Hz, approximately
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) * ap.gpsDataFreqHz;
float acceleration = (velocity - efAxis->previousVelocity) * gpsDataFreqHz;
efAxis->previousVelocity = velocity;
pt1FilterUpdateCutoff(&efAxis->accelerationLpf, lpfGain);
const float accelerationFiltered = pt1FilterApply(&efAxis->accelerationLpf, acceleration);
@ -293,8 +296,8 @@ bool positionControl(void)
efAxis->isStopping = true;
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;
// slowly leak iTerm away
efAxis->integral *= ap.iTermLeakGain;
// increase sanity check distance depending on speed, typically maximal when sticks stop
} else if (efAxis->isStopping) {
// 'phase' after sticks stop, but before craft has stopped
@ -334,38 +337,38 @@ bool positionControl(void)
// keep update sanity check distance while sticks are out
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
ap.pidSumCraft[AI_ROLL] = 0.0f;
ap.pidSumCraft[AI_PITCH] = 0.0f;
ap.pidSumBF[AI_ROLL] = 0.0f;
ap.pidSumBF[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);
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;
ap.pidSumBF[AI_ROLL] = -sinHeading * ap.efAxis[lat].pidSum + cosHeading * ap.efAxis[lon].pidSum;
ap.pidSumBF[AI_PITCH] = cosHeading * ap.efAxis[lat].pidSum + sinHeading * ap.efAxis[lon].pidSum;
// limit angle vector to maxAngle
const float angleMagSq = sq(ap.pidSumCraft[AI_ROLL]) + sq(ap.pidSumCraft[AI_PITCH]);
const float angleMagSq = sq(ap.pidSumBF[AI_ROLL]) + sq(ap.pidSumBF[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
ap.pidSumBF[AI_ROLL] *= limiter; // Scale the roll value
ap.pidSumBF[AI_PITCH] *= limiter; // Scale the pitch value
}
}
}
// ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
autopilotAngle[AI_ROLL] = pt3FilterApply(&ap.upsample[AI_ROLL], ap.pidSumCraft[AI_ROLL]);
autopilotAngle[AI_PITCH] = pt3FilterApply(&ap.upsample[AI_PITCH], ap.pidSumCraft[AI_PITCH]);
autopilotAngle[AI_ROLL] = pt3FilterApply(&ap.upsampleBF[AI_ROLL], ap.pidSumBF[AI_ROLL]);
autopilotAngle[AI_PITCH] = pt3FilterApply(&ap.upsampleBF[AI_PITCH], ap.pidSumBF[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(ap.efAxis[EW].distance)); // cm
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[EW].pidSum * 10)); // deg * 10
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ap.efAxis[lon].distance)); // cm
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[lon].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(ap.efAxis[NS].distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[NS].pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(ap.efAxis[lat].distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(ap.efAxis[lat].pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_PITCH] * 10));
}
return true;

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

@ -24,7 +24,7 @@ extern float autopilotAngle[RP_AXIS_COUNT]; // NOTE: ANGLES ARE IN CENTIDEGREES
void autopilotInit(void);
void resetAltitudeControl(void);
void setSticksActiveStatus(bool areSticksActive);
void resetPositionControl(gpsLocation_t *initialTargetLocation);
void resetPositionControl(const gpsLocation_t *initialTargetLocation);
void moveTargetLocation(int32_t latStep, int32_t lonStep);
void posControlOnNewGpsData(void);
void posControlOutput(void);

8
src/main/flight/imu.c
View file

@ -612,17 +612,17 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
static void updateGpsHeadingUsable(float groundspeedGain, float imuCourseError, float dt)
{
if (!canUseGPSHeading) {
static float gpsHeadingTruth = 0;
static float gpsHeadingConfidence = 0;
// groundspeedGain can be 5.0 in clean forward flight, up to 10.0 max
// fabsf(imuCourseError) is 0 when headings are aligned, 1 when 90 degrees error or worse
// accumulate 'points' based on alignment and likelihood of accumulation being good
gpsHeadingTruth += fmaxf(groundspeedGain - fabsf(imuCourseError), 0.0f) * dt;
gpsHeadingConfidence += fmaxf(groundspeedGain - fabsf(imuCourseError), 0.0f) * dt;
// recenter at 2.5s time constant
// TODO: intent is to match IMU time constant, approximately, but I don't exactly know how to do that
gpsHeadingTruth -= 0.4 * dt * gpsHeadingTruth;
gpsHeadingConfidence -= 0.4 * dt * gpsHeadingConfidence;
// if we accumulate enough 'points' over time, the IMU probably is OK
// will need to reaccumulate after a disarm (will be retained partly for very brief disarms)
canUseGPSHeading = gpsHeadingTruth > 2.0f;
canUseGPSHeading = gpsHeadingConfidence > 2.0f;
// canUseGPSHeading blocks position hold until suitable GPS heading, when GPS is the only heading source
// NOTE: I think that this check only runs once after power up
// If the GPS heading is lost on disarming, then it will need to be reset each disarm

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

@ -95,7 +95,7 @@ static serialPort_t *gpsPort;
static float gpsDataIntervalSeconds = 0.1f;
static float gpsDataFrequencyHz = 10.0f;
static uint16_t currentGpsStamp = 1; // logical timer for received position update
static uint16_t currentGpsStamp = 0; // logical timer for received position update
typedef struct gpsInitData_s {
uint8_t index;
@ -2593,13 +2593,13 @@ void GPS_calculateDistanceAndDirectionToHome(void)
}
}
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist) {
if (pEWDist) {
*pEWDist = (float)(to->lon - from->lon) * GPS_cosLat * EARTH_ANGLE_TO_CM; // East-West distance, positive East
}
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pNSDist, float *pEWDist) {
if (pNSDist) {
*pNSDist = (float)(to->lat - from->lat) * EARTH_ANGLE_TO_CM; // North-South distance, positive North
}
if (pEWDist) {
*pEWDist = (float)(to->lon - from->lon) * GPS_cosLat * EARTH_ANGLE_TO_CM; // East-West distance, positive East
}
}
void onGpsNewData(void)

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

@ -390,7 +390,7 @@ 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 GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist);
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pNSDist, float *pEWDist);
void gpsSetFixState(bool state);
bool gpsHasNewData(uint16_t *stamp);

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

@ -125,12 +125,12 @@ extern "C" {
return true;
}
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist)
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pNSDist, float *pEWDist)
{
UNUSED(from);
UNUSED(to);
UNUSED(pEWDist);
UNUSED(pNSDist);
UNUSED(pEWDist);
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig)

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

@ -1156,13 +1156,14 @@ extern "C" {
UNUSED(bearing);
}
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pEWDist, float *pNSDist)
void GPS_latLongVectors(const gpsLocation_t *from, const gpsLocation_t *to, float *pNSDist, float *pEWDist)
{
UNUSED(from);
UNUSED(to);
UNUSED(pEWDist);
UNUSED(pNSDist);
UNUSED(pEWDist);
}
float vector2Norm(const vector2_t *v) {
UNUSED(*v);
return 0.0f;

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

@ -436,7 +436,6 @@ extern "C" {
bool schedulerGetIgnoreTaskExecTime() { return false; }
float gyroGetFilteredDownsampled(int) { return 0.0f; }
float baroUpsampleAltitude() { return 0.0f; }
float pt2FilterGain(float, float) { return 0.0f; }
float getBaroAltitude(void) { return 3000.0f; }
float gpsRescueGetImuYawCogGain(void) { return 1.0f; }
float getRcDeflectionAbs(int) { return 0.0f; }