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

fix debug, tidy up EF axis names, add comments about sign and direction

Browse source
This commit is contained in:
ctzsnooze 2024-11-01 10:55:52 +11:00
parent d3e3fd0c1a
commit 27a7d2e4af
1 changed files with 69 additions and 71 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -62,6 +62,14 @@ typedef struct {
pt1Filter_t accelerationLpf; pt1Filter_t accelerationLpf;
} earthFrame_t; } earthFrame_t;
typedef enum {
EW = 0,
NS
} axisEF_t;
earthFrame_t eastWest;
earthFrame_t northSouth;
typedef struct { typedef struct {
float gpsDataIntervalS; float gpsDataIntervalS;
float gpsDataFreqHz; float gpsDataFreqHz;
@ -72,14 +80,9 @@ typedef struct {
bool sticksActive; bool sticksActive;
float pidSum[2]; float pidSum[2];
pt3Filter_t upsample[2]; pt3Filter_t upsample[2];
earthFrame_t direction[2]; earthFrame_t efAxis[2];
} posHoldState; } posHoldState;
typedef enum {
NORTH_SOUTH = 0,
EAST_WEST
} axisEF_t;
static posHoldState posHold = { static posHoldState posHold = {
.gpsDataIntervalS = 0.1f, .gpsDataIntervalS = 0.1f,
.gpsDataFreqHz = 10.0f, .gpsDataFreqHz = 10.0f,
@ -90,41 +93,38 @@ static posHoldState posHold = {
.sticksActive = false, .sticksActive = false,
.pidSum = { 0.0f, 0.0f }, .pidSum = { 0.0f, 0.0f },
.upsample = { {0}, {0} }, .upsample = { {0}, {0} },
.direction = { {0} } .efAxis = { {0} }
}; };
earthFrame_t northSouth;
earthFrame_t eastWest;
static gpsLocation_t currentTargetLocation = {0, 0, 0}; static gpsLocation_t currentTargetLocation = {0, 0, 0};
float autopilotAngle[ANGLE_INDEX_COUNT]; float autopilotAngle[ANGLE_INDEX_COUNT];
void resetPositionControlParams(earthFrame_t *latLong) { void resetPositionControlParams(earthFrame_t *efAxis) {
// at the start, and while sticks are moving // at the start, and while sticks are moving
latLong->previousDistance = 0.0f; efAxis->previousDistance = 0.0f;
latLong->previousVelocity = 0.0f; efAxis->previousVelocity = 0.0f;
latLong->pidSum = 0.0f; efAxis->pidSum = 0.0f;
// Clear accumulation in filters // Clear accumulation in filters
pt1FilterInit(&latLong->velocityLpf, posHold.pt1Gain); pt1FilterInit(&efAxis->velocityLpf, posHold.pt1Gain);
pt1FilterInit(&latLong->accelerationLpf, posHold.pt1Gain); pt1FilterInit(&efAxis->accelerationLpf, posHold.pt1Gain);
// Initiate starting behaviour // Initiate starting behaviour
latLong->isStarting = true; efAxis->isStarting = true;
} }
void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start frmo pos_hold.c void resetPositionControl(gpsLocation_t initialTargetLocation) { // set only at the start frmo pos_hold.c
currentTargetLocation = initialTargetLocation; currentTargetLocation = initialTargetLocation;
resetPositionControlParams(&posHold.direction[NORTH_SOUTH]); resetPositionControlParams(&posHold.efAxis[EW]);
resetPositionControlParams(&posHold.direction[EAST_WEST]); resetPositionControlParams(&posHold.efAxis[NS]);
posHold.peakInitialGroundspeed = 0.0f; posHold.peakInitialGroundspeed = 0.0f;
posHold.direction[NORTH_SOUTH].integral = 0.0f; posHold.efAxis[EW].integral = 0.0f;
posHold.direction[EAST_WEST].integral = 0.0f; posHold.efAxis[NS].integral = 0.0f;
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f; posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
} }
void autopilotInit(const autopilotConfig_t *config) void autopilotInit(const autopilotConfig_t *config)
{ {
northSouth = posHold.direction[NORTH_SOUTH]; eastWest = posHold.efAxis[EW];
eastWest = posHold.direction[EAST_WEST]; northSouth = posHold.efAxis[NS];
altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE; altitudePidCoeffs.Kp = config->altitude_P * ALTITUDE_P_SCALE;
altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE; altitudePidCoeffs.Ki = config->altitude_I * ALTITUDE_I_SCALE;
altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE; altitudePidCoeffs.Kd = config->altitude_D * ALTITUDE_D_SCALE;
@ -133,16 +133,15 @@ void autopilotInit(const autopilotConfig_t *config)
positionPidCoeffs.Ki = config->position_I * POSITION_I_SCALE; positionPidCoeffs.Ki = config->position_I * POSITION_I_SCALE;
positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE; positionPidCoeffs.Kd = config->position_D * POSITION_D_SCALE;
positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration positionPidCoeffs.Kf = config->position_A * POSITION_A_SCALE; // Kf used for acceleration
// approximate filter gain // initialise filters with approximate filter gain
posHold.lpfCutoff = config->position_cutoff * 0.01f; posHold.lpfCutoff = config->position_cutoff * 0.01f;
posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, 0.1f); // assume 10Hz GPS connection at start 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 float upsampleCutoff = pt3FilterGain(UPSAMPLING_CUTOFF, 0.01f); // 5Hz, assuming 100Hz task rate
pt3FilterInit(&posHold.upsample[AI_ROLL], upsampleCutoff); pt3FilterInit(&posHold.upsample[AI_ROLL], upsampleCutoff);
pt3FilterInit(&posHold.upsample[AI_PITCH], upsampleCutoff); pt3FilterInit(&posHold.upsample[AI_PITCH], upsampleCutoff);
// initialise filters // Reset parameters and initialise PT1 filters for both NS and EW
// Reset parameters for both NS and EW resetPositionControlParams(&posHold.efAxis[EW]);
resetPositionControlParams(&posHold.direction[NORTH_SOUTH]); resetPositionControlParams(&posHold.efAxis[NS]);
resetPositionControlParams(&posHold.direction[EAST_WEST]);
} }
void resetAltitudeControl (void) { void resetAltitudeControl (void) {
@ -205,8 +204,8 @@ bool positionControl(void) {
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS; posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS;
if (posHold.sticksActive) { if (posHold.sticksActive) {
// if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode // if a Position Hold deadband is set, and sticks are outside deadband, allow pilot control in angle mode
resetPositionControlParams(&posHold.direction[NORTH_SOUTH]); resetPositionControlParams(&posHold.efAxis[EW]);
resetPositionControlParams(&posHold.direction[EAST_WEST]); resetPositionControlParams(&posHold.efAxis[NS]);
posHold.pidSum[AI_ROLL] = 0.0f; posHold.pidSum[AI_ROLL] = 0.0f;
posHold.pidSum[AI_PITCH] = 0.0f; posHold.pidSum[AI_PITCH] = 0.0f;
} else { } else {
@ -214,8 +213,8 @@ bool positionControl(void) {
vector2_t gpsDistance; vector2_t gpsDistance;
GPS_distances(&gpsSol.llh, &currentTargetLocation, &gpsDistance.y, &gpsDistance.x); // Y is north, X is south GPS_distances(&gpsSol.llh, &currentTargetLocation, &gpsDistance.y, &gpsDistance.x); // Y is north, X is south
posHold.direction[NORTH_SOUTH].distance = gpsDistance.y; posHold.efAxis[EW].distance = gpsDistance.x;
posHold.direction[EAST_WEST].distance = gpsDistance.x; posHold.efAxis[NS].distance = gpsDistance.y;
float distanceCm = vector2Norm(&gpsDistance); float distanceCm = vector2Norm(&gpsDistance);
posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS); posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS);
@ -230,45 +229,46 @@ bool positionControl(void) {
// static float prevPidDASquared = 0.0f; // if we limit DA on true vector length // static float prevPidDASquared = 0.0f; // if we limit DA on true vector length
for (int i = 0; i < 2; i++) { for (int loopAxis = 0; loopAxis < 2; loopAxis++) {
earthFrame_t *latLong = &posHold.direction[i]; earthFrame_t *efAxis = &posHold.efAxis[loopAxis];
// separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW) // separate PID controllers for latitude (NorthSouth or NS) and longitude (EastWest or EW)
// ** P ** // ** P **
float pidP = latLong->distance * positionPidCoeffs.Kp; float pidP = efAxis->distance * positionPidCoeffs.Kp;
// ** I ** // ** I **
if (!latLong->isStarting){ if (!efAxis->isStarting){
// only accumulate iTerm after completing the start phase // only accumulate iTerm after completing the start phase
// perhaps need a timeout on the start phase ? // perhaps need a timeout on the start phase ?
latLong->integral += latLong->distance * posHold.gpsDataIntervalS; efAxis->integral += efAxis->distance * posHold.gpsDataIntervalS;
} else { } else {
// while moving sticks, slowly leak iTerm away, approx 2s time constant // while moving sticks, slowly leak iTerm away, approx 2s time constant
latLong->integral *= leak; efAxis->integral *= leak;
} }
float pidI = latLong->integral * positionPidCoeffs.Ki; float pidI = efAxis->integral * positionPidCoeffs.Ki;
// ** D ** // // ** D ** //
// Velocity derived from GPS position works better than 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 float velocity = (efAxis->distance - efAxis->previousDistance) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
latLong->previousDistance = latLong->distance; efAxis->previousDistance = efAxis->distance;
pt1FilterUpdateCutoff(&latLong->velocityLpf, posHold.pt1Gain); pt1FilterUpdateCutoff(&efAxis->velocityLpf, posHold.pt1Gain);
velocity = pt1FilterApply(&latLong->velocityLpf, velocity); velocity = pt1FilterApply(&efAxis->velocityLpf, velocity);
float pidD = velocity * positionPidCoeffs.Kd; float pidD = velocity * positionPidCoeffs.Kd;
float acceleration = (velocity - latLong->previousVelocity) * posHold.gpsDataFreqHz; float acceleration = (velocity - efAxis->previousVelocity) * posHold.gpsDataFreqHz;
latLong->previousVelocity = velocity; efAxis->previousVelocity = velocity;
pt1FilterUpdateCutoff(&latLong->accelerationLpf, posHold.pt1Gain); pt1FilterUpdateCutoff(&efAxis->accelerationLpf, posHold.pt1Gain);
acceleration = pt1FilterApply(&latLong->accelerationLpf, acceleration); acceleration = pt1FilterApply(&efAxis->accelerationLpf, acceleration);
float pidA = acceleration * positionPidCoeffs.Kd; float pidA = acceleration * positionPidCoeffs.Kd;
// limit sum of D and A because otherwise can be too aggressive when starting at speed // limit sum of D and A per axis 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 const float maxDAAngle = 35.0f; // limit in degrees; arbitrary. 20 is a bit too low, allows a lot of overshoot
const float pidDA = constrainf(pidD + pidA, -maxDAAngle, maxDAAngle); 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 // note: an angle of more than 35 degrees can still be achieved as P and I grow, and on diagonal
// note: angle mode limit is absolute max, as set in CLI default is 60
/* possible economical alternative method using true length: /* possible alternate method using total vector length for the limiting, if needed:
float pidDASquared = pidDA * pidDA; float pidDASquared = pidDA * pidDA;
float mag = sqrtf(pidDASquared + prevPidDASquared) float mag = sqrtf(pidDASquared + prevPidDASquared)
if (mag > maxDAAngle) { if (mag > maxDAAngle) {
@ -280,29 +280,31 @@ bool positionControl(void) {
// ** PID Sum ** // ** PID Sum **
float pidSum = pidP + pidI + pidDA; float pidSum = pidP + pidI + pidDA;
// reset the position target when pidSum crosses zero, typically when velocity is very close to zero, ie craft has stopped // reset the location target when pidSum crosses zero for that axis, 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 // 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 if (efAxis->isStarting && efAxis->pidSum * pidSum < 0.0f) { // pidsum ns has reversed sign
resetPositionControlParams(latLong); resetPositionControlParams(efAxis);
if (i == 0) { if (loopAxis == EW) {
currentTargetLocation.lat = gpsSol.llh.lat; currentTargetLocation.lon = gpsSol.llh.lon; // update East-West / / longitude position
} else { } else {
currentTargetLocation.lon = gpsSol.llh.lon; currentTargetLocation.lat = gpsSol.llh.lat; // update North-South / latitude position
} }
latLong->distance = 0.0f; efAxis->distance = 0.0f;
posHold.sanityCheckDistance = 1000.0f; // 10m, once stable posHold.sanityCheckDistance = 1000.0f; // 10m, once stable
latLong->isStarting = false; efAxis->isStarting = false;
} }
latLong->pidSum = pidSum;
efAxis->pidSum = pidSum;
// Debugs... distances in cm, angles in degrees * 10, velocities cm/2 // Debugs... distances in cm, angles in degrees * 10, velocities cm/2
if (gyroConfig()->gyro_filter_debug_axis == i) { if (gyroConfig()->gyro_filter_debug_axis == loopAxis) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceCm)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 0, lrintf(distanceCm));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(latLong->distance)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(efAxis->distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(latLong->pidSum * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(efAxis->pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(pidI * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(pidI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidDA * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidD * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(pidA * 10));
} }
} }
} }
@ -311,8 +313,8 @@ bool positionControl(void) {
const float sinHeading = sin_approx(headingRads); const float sinHeading = sin_approx(headingRads);
const float cosHeading = cos_approx(headingRads); const float cosHeading = cos_approx(headingRads);
posHold.pidSum[AI_ROLL] = -sinHeading * posHold.direction[NORTH_SOUTH].pidSum + cosHeading * posHold.direction[EAST_WEST].pidSum; posHold.pidSum[AI_ROLL] = -sinHeading * posHold.efAxis[NS].pidSum + cosHeading * posHold.efAxis[EW].pidSum;
posHold.pidSum[AI_PITCH] = cosHeading * posHold.direction[NORTH_SOUTH].pidSum + sinHeading * posHold.direction[EAST_WEST].pidSum; posHold.pidSum[AI_PITCH] = cosHeading * posHold.efAxis[NS].pidSum + sinHeading * posHold.efAxis[EW].pidSum;
} }
// ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling** // ** Final output to pid.c Angle Mode at 100Hz with primitive upsampling**
@ -321,14 +323,10 @@ bool positionControl(void) {
// 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 // 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) { if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSum[AI_ROLL] * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_ROLL] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(autopilotAngle[AI_ROLL] * 10));
} else { } else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSum[AI_PITCH] * 10)); DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(autopilotAngle[AI_PITCH] * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(autopilotAngle[AI_PITCH] * 10));
} }
return true; return true;
} }