Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +03:00
Code Issues Wiki Activity

sequential PT1's, refactoring from reviews

Browse source
This commit is contained in:
ctzsnooze 2024-10-30 11:56:57 +11:00
parent 8c1c2d4fbd
commit d66141edea
5 changed files with 94 additions and 114 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -24,6 +24,7 @@
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/vector.h"
#include "fc/core.h"
#include "fc/rc.h"
#include "fc/runtime_config.h"
@ -60,8 +61,8 @@ typedef struct {
float integral;
float pidSum;
pt1Filter_t velocityLpf;
pt2Filter_t accelerationLpf;
} vectors_t;
pt1Filter_t accelerationLpf;
} earthFrame_t;
typedef struct {
float gpsDataIntervalS;
@ -71,14 +72,16 @@ typedef struct {
float lpfCutoff;
float pt1Gain;
bool sticksActive;
float pidSumRoll;
float pidSumPitch;
pt3Filter_t upsampleRollLpf;
pt3Filter_t upsamplePitchLpf;
vectors_t NS;
vectors_t EW;
float pidSum[2];
pt3Filter_t upsample[2];
earthFrame_t direction[2];
} posHoldState;
typedef enum {
NORTH_SOUTH = 0,
EAST_WEST
} axisEF_t;
static posHoldState posHold = {
.gpsDataIntervalS = 0.1f,
.gpsDataFreqHz = 10.0f,
@ -87,52 +90,42 @@ static posHoldState posHold = {
.lpfCutoff = 1.0f,
.pt1Gain = 1.0f,
.sticksActive = false,
.pidSumRoll = 0.0f,
.pidSumPitch = 0.0f,
.NS = {
.isStarting = false,
.distance = 0.0f,
.previousDistance = 0.0f,
.previousVelocity = 0.0f,
.integral = 0.0f,
.pidSum = 0.0f,
},
.EW = {
.isStarting = false,
.distance = 0.0f,
.previousDistance = 0.0f,
.previousVelocity = 0.0f,
.integral = 0.0f,
.pidSum = 0.0f,
},
.pidSum = { 0.0f, 0.0f },
.upsample = { {0}, {0} },
.direction = { {0} }
};
earthFrame_t northSouth;
earthFrame_t eastWest;
static gpsLocation_t currentTargetLocation = {0, 0, 0};
float autopilotAngle[ANGLE_INDEX_COUNT];
void resetPositionControlParams(vectors_t *latLong) {
void resetPositionControlParams(earthFrame_t *latLong) {
// at the start, and while sticks are moving
latLong->previousDistance = 0.0f;
latLong->previousVelocity = 0.0f;
latLong->pidSum = 0.0f;
// Clear accumulation in filters
pt1FilterInit(&latLong->velocityLpf, posHold.pt1Gain);
pt2FilterInit(&latLong->accelerationLpf, posHold.pt1Gain);
pt1FilterInit(&latLong->accelerationLpf, posHold.pt1Gain);
// Initiate starting behaviour
latLong->isStarting = true;
}
void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start frmo pos_hold.c
currentTargetLocation = initialTargetLocation;
resetPositionControlParams(&posHold.NS);
resetPositionControlParams(&posHold.EW);
resetPositionControlParams(&posHold.direction[NORTH_SOUTH]);
resetPositionControlParams(&posHold.direction[EAST_WEST]);
posHold.peakInitialGroundspeed = 0.0f;
posHold.NS.integral = 0.0f;
posHold.EW.integral = 0.0f;
posHold.direction[NORTH_SOUTH].integral = 0.0f;
posHold.direction[EAST_WEST].integral = 0.0f;
}
void autopilotInit(const autopilotConfig_t *config)
{
northSouth = posHold.direction[NORTH_SOUTH];
eastWest = posHold.direction[EAST_WEST];
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
@ -145,12 +138,12 @@ void autopilotInit(const autopilotConfig_t *config)
posHold.lpfCutoff = config->position_cutoff * 0.01f;
posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); // assume 10Hz GPS connection at start
float upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF, 0.01f); // 5Hz, assuming 100Hz task rate
pt3FilterInit(&posHold.upsampleRollLpf, upsampleCutoff);
pt3FilterInit(&posHold.upsamplePitchLpf, upsampleCutoff);
pt3FilterInit(&posHold.upsample[AI_ROLL], upsampleCutoff);
pt3FilterInit(&posHold.upsample[AI_PITCH], upsampleCutoff);
// initialise filters
// Reset parameters for both NS and EW
resetPositionControlParams(&posHold.NS);
resetPositionControlParams(&posHold.EW);
resetPositionControlParams(&posHold.direction[NORTH_SOUTH]);
resetPositionControlParams(&posHold.direction[EAST_WEST]);
}
void resetAltitudeControl (void) {
@ -173,11 +166,12 @@ void altitudeControl(float targetAltitudeCm, float taskIntervalS, float vertical
const float altitudeF = targetAltitudeStep * altitudePidCoeffs.Kf;
const float hoverOffset = autopilotConfig()->hover_throttle - PWM_RANGE_MIN;
float throttleOffset = altitudeP + altitudeI - altitudeD + altitudeF + hoverOffset;
const float tiltMultiplier = 1.0f / fmaxf(getCosTiltAngle(), 0.5f);
// 1 = flat, 1.3 at 40 degrees, 1.56 at 50 deg, max 2.0 at 60 degrees or higher
// note: the default limit of Angle Mode is 60 degrees
throttleOffset *= tiltMultiplier;
float newThrottle = PWM_RANGE_MIN + throttleOffset;
@ -224,52 +218,48 @@ bool positionControl(void) {
return false;
}
if (isNewDataForPosHold()) {
if (isNewGPSDataAvailable()) {
posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
if (posHold.sticksActive) {
// if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
resetPositionControlParams(&posHold.NS);
resetPositionControlParams(&posHold.EW);
posHold.pidSumRoll = 0.0f;
posHold.pidSumPitch = 0.0f;
resetPositionControlParams(&posHold.direction[NORTH_SOUTH]);
resetPositionControlParams(&posHold.direction[EAST_WEST]);
posHold.pidSum[AI_ROLL] = 0.0f;
posHold.pidSum[AI_PITCH] = 0.0f;
} else {
// first get xy distances from current location (gpsSol.llh) to target location
float nsDistance; // cm, steps of 11.1cm, North of target is positive
float ewDistance; // cm, steps of 11.1cm, East of target is positive
GPS_distances(&gpsSol.llh, &currentTargetLocation, &nsDistance, &ewDistance);
float distanceCm = sqrtf(sq(nsDistance) + sq(ewDistance));
posHold.NS.distance = nsDistance;
posHold.EW.distance = ewDistance;
vector2_t gpsDistance;
GPS_distances(&gpsSol.llh, &currentTargetLocation, &gpsDistance.y, &gpsDistance.x); // Y is north, X is south
posHold.direction[NORTH_SOUTH].distance = gpsDistance.y;
posHold.direction[EAST_WEST].distance = gpsDistance.x;
float distanceCm = vector2Norm(&gpsDistance);
posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS);
const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS; // gpsDataIntervalS is not more than 1.0s
// ** Sanity check **
// larger threshold if faster at start
if (posHold.NS.isStarting || posHold.EW.isStarting) {
// 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 (posHold.direction[NORTH_SOUTH].isStarting || posHold.direction[EAST_WEST].isStarting) {
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
// larger threshold if faster at start
// 1s of flight at current speed or 10m, in cm
}
// primarily to detect flyaway from no Mag or badly oriented Mag
// but must accept some overshoot at the start, especially if entering at high speed
if (distanceCm > posHold.sanityCheckDistance) {
return false; // must stay within 10m or probably flying away
// value at this point is a 'best guess' to detect IMU failure in the event the user has no Mag
// if entering poshold from a stable hover, we would only exceed this if IMU was disoriented
// if entering poshold at speed, it may overshoot this value and falsely fail, if so need something more complex
return false;
}
vectors_t *vectors[] = { &posHold.NS, &posHold.EW };
earthFrame_t *direction[] = { &posHold.direction[NORTH_SOUTH], &posHold.direction[EAST_WEST]};
for (int i = 0; i < 2; i++) {
vectors_t *latLong = vectors[i];
// separate PID controllers for latitude (NorthSouth or ns) and longitude (EastWest or ew)
earthFrame_t *latLong = direction[i];
// separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW)
// ** P **
float pidP = latLong->distance * positionPidCoeffs.Kp;
// ** I **
if (!latLong->isStarting){
// only accumulate iTerm after completing the start phase
@ -280,41 +270,35 @@ bool positionControl(void) {
latLong->integral *= leak;
}
float pidI = latLong->integral * positionPidCoeffs.Ki;
// ** D ** //
// get change in distance in NS and EW directions from gps.c using the `GPS_distances` function
// this gives cleaner velocity data than the module supplied GPS Speed and Heading information
// Velocity derived from GPS position works better than module supplied GPS Speed and Heading information
float velocity = (latLong->distance - latLong->previousDistance) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
latLong->previousDistance = latLong->distance;
pt1FilterUpdateCutoff(&latLong->velocityLpf, posHold.pt1Gain);
velocity = pt1FilterApply(&latLong->velocityLpf, velocity);
float pidD = velocity * positionPidCoeffs.Kd;
float acceleration = (velocity - latLong->previousVelocity) * posHold.gpsDataFreqHz;
latLong->previousVelocity = velocity;
// scale and filter - filter cutoffs vary during the startup phase
float pidD = velocity * positionPidCoeffs.Kd;
pt1FilterUpdateCutoff(&latLong->velocityLpf, posHold.pt1Gain);
pidD = pt1FilterApply(&latLong->velocityLpf, pidD);
pt1FilterUpdateCutoff(&latLong->accelerationLpf, posHold.pt1Gain);
acceleration = pt1FilterApply(&latLong->accelerationLpf, acceleration);
float pidA = acceleration * positionPidCoeffs.Kd;
pt2FilterUpdateCutoff(&latLong->accelerationLpf, posHold.pt1Gain);
pidA = pt2FilterApply(&latLong->accelerationLpf, pidA);
// limit sum of D and A because otherwise can be too aggressive when starting at speed
const float maxDAAngle = 35.0f; // limit in degrees; arbitrary. 20 is a bit too low, allows a lot of overshoot
// an angle of more than 35 degrees is achieved as P and I grow
// ** todo = should this be half of the user-configurable angle_limit? Or fixed?
const float pidDA = constrainf(pidD + pidA, -maxDAAngle, maxDAAngle);
// note: an angle of more than 35 degrees can still be achieved as P and I grow
// ** PID Sum **
float pidSum = pidP + pidI + pidDA;
// terminate initial startup behaviour separately for latitude and longitude controllers
// the position target is reset when pidSum crosses zero
// this enhances the smoothness of the transition from stick input back to position hold - there is no sharp change in pidSum
// reset the position target when pidSum crosses zero, typically when velocity is very close to zero, ie craft has stopped
// this enhances the smoothness of the transition from stick input back to position hold because there is no sharp change in pidSum
if (latLong->isStarting && latLong->pidSum * pidSum < 0.0f) { // pidsum ns has reversed sign
resetPositionControlParams(latLong);
if (i == 0) {
currentTargetLocation.lat = gpsSol.llh.lat; // can we simplify this within the loop?
currentTargetLocation.lat = gpsSol.llh.lat;
} else {
currentTargetLocation.lon = gpsSol.llh.lon;
}
@ -322,7 +306,7 @@ bool positionControl(void) {
latLong->isStarting = false;
}
latLong->pidSum = pidSum;
// Debugs... distances in cm, angles in degrees * 10, velocities cm/2
if (gyroConfig()->gyro_filter_debug_axis == i) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceCm));
@ -335,23 +319,23 @@ bool positionControl(void) {
}
}
// ** Rotate pid Sum to quad frame of reference, into pitch and roll **
float headingRads = DECIDEGREES_TO_RADIANS(attitude.values.yaw); // will be constrained to +/-pi in sin_approx()
float headingRads = DECIDEGREES_TO_RADIANS(attitude.values.yaw);
const float sinHeading = sin_approx(headingRads);
const float cosHeading = cos_approx(headingRads);
posHold.pidSumRoll = -sinHeading * posHold.NS.pidSum + cosHeading * posHold.EW.pidSum;
posHold.pidSumPitch = cosHeading * posHold.NS.pidSum + sinHeading * posHold.EW.pidSum;
posHold.pidSum[AI_ROLL] = -sinHeading * posHold.direction[NORTH_SOUTH].pidSum + cosHeading * posHold.direction[EAST_WEST].pidSum;
posHold.pidSum[AI_PITCH] = cosHeading * posHold.direction[NORTH_SOUTH].pidSum + sinHeading * posHold.direction[EAST_WEST].pidSum;
}
// ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
autopilotAngle[AI_ROLL] = pt3FilterApply(&posHold.upsampleRollLpf, posHold.pidSumRoll);
autopilotAngle[AI_PITCH] = pt3FilterApply(&posHold.upsamplePitchLpf, posHold.pidSumPitch);
autopilotAngle[AI_ROLL] = pt3FilterApply(&posHold.upsample[AI_ROLL], posHold.pidSum[AI_ROLL]);
autopilotAngle[AI_PITCH] = pt3FilterApply(&posHold.upsample[AI_PITCH], posHold.pidSum[AI_PITCH]);
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSumRoll * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSum[AI_ROLL] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(autopilotAngle[AI_ROLL] * 10));
} else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSumPitch * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSum[AI_PITCH] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(autopilotAngle[AI_PITCH] * 10));
}

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

@ -93,10 +93,7 @@ GPS_svinfo_t GPS_svinfo[GPS_SV_MAXSATS_M8N];
static serialPort_t *gpsPort;
static float gpsDataIntervalSeconds;
#ifdef USE_POS_HOLD_MODE
static bool newDataForPosHold = false;
#endif
static uint16_t gpsStamp = ~0; // Initialize to an invalid state
typedef struct gpsInitData_s {
uint8_t index;
@ -2596,10 +2593,10 @@ 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
*pNSDist = (float)(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
*pEWDist = (float)(to->lon - from->lon) * GPS_cosLat * EARTH_ANGLE_TO_CM; // East-West distance, positive East
}
}
@ -2610,6 +2607,8 @@ void onGpsNewData(void)
return;
}
gpsStamp ++; // increment the stamp
gpsDataIntervalSeconds = gpsSol.navIntervalMs / 1000.0f;
GPS_calculateDistanceAndDirectionToHome();
@ -2625,19 +2624,16 @@ void onGpsNewData(void)
gpsLapTimerNewGpsData();
#endif // USE_GPS_LAP_TIMER
#ifdef USE_POS_HOLD_MODE
newDataForPosHold = true;
#endif
}
#ifdef USE_POS_HOLD_MODE
bool isNewDataForPosHold(void) {
const bool isNewData = newDataForPosHold; // true only when new data arrives
newDataForPosHold = false; // clear flag once new data has been handled
return isNewData;
bool isNewGPSDataAvailable(void) {
static uint16_t lastGpsStamp = ~0; // Initialize to an invalid state
if (lastGpsStamp != gpsStamp) {
lastGpsStamp = gpsStamp; // Update the last known stamp
return true; // New GPS data is available
}
return false; // No new data
}
#endif
void gpsSetFixState(bool state)
{

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

@ -393,6 +393,6 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
void gpsSetFixState(bool state);
float getGpsDataIntervalSeconds(void); // sends GPS Nav Data interval to GPS Rescue
bool isNewDataForPosHold(void);
bool isNewGPSDataAvailable(void);
baudRate_e getGpsPortActualBaudRateIndex(void);

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

@ -131,7 +131,7 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
float getGpsDataIntervalSeconds(void) { return 0.01f; }
float getRcDeflectionAbs(void) { return 0.0f; }
attitudeEulerAngles_t attitude;
bool isNewDataForPosHold(void){ return true; }
bool isNewGPSDataAvailable(void){ return true; }
bool wasThrottleRaised(void) { return true; }

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

@ -1192,5 +1192,5 @@ extern "C" {
bool canUseGPSHeading;
bool compassIsHealthy;
bool isNewDataForPosHold(void){ return true; }
bool isNewGPSDataAvailable(void){ return true; }
}