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

upsampling filter at 5Hz

Browse source
This commit is contained in:
ctzsnooze 2024-10-28 15:00:06 +11:00
parent 532c8faa98
commit a700bff52a
4 changed files with 163 additions and 131 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -43,6 +43,7 @@
#define POSITION_I_SCALE 0.0001f #define POSITION_I_SCALE 0.0001f
#define POSITION_D_SCALE 0.0015f #define POSITION_D_SCALE 0.0015f
#define POSITION_A_SCALE 0.0015f #define POSITION_A_SCALE 0.0015f
#define UPSAMPLING_CUTOFF 5.0f
static pidCoefficient_t altitudePidCoeffs; static pidCoefficient_t altitudePidCoeffs;
static pidCoefficient_t positionPidCoeffs; static pidCoefficient_t positionPidCoeffs;
@ -69,6 +70,10 @@ typedef struct {
float lpfCutoff; float lpfCutoff;
float pt1Gain; float pt1Gain;
bool sticksActive; bool sticksActive;
float pidSumRoll;
float pidSumPitch;
pt3Filter_t upsampleRollLpf;
pt3Filter_t upsamplePitchLpf;
vectors_t NS; vectors_t NS;
vectors_t EW; vectors_t EW;
} posHoldState; } posHoldState;
@ -81,6 +86,8 @@ static posHoldState posHold = {
.lpfCutoff = 1.0f, .lpfCutoff = 1.0f,
.pt1Gain = 1.0f, .pt1Gain = 1.0f,
.sticksActive = false, .sticksActive = false,
.pidSumRoll = 0.0f,
.pidSumPitch = 0.0f,
.NS = { .NS = {
.isStarting = false, .isStarting = false,
.distance = 0.0f, .distance = 0.0f,
@ -136,6 +143,9 @@ void autopilotInit(const autopilotConfig_t *config)
// approximate filter gain // 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
pt3FilterInit(&posHold.upsampleRollLpf, upsampleCutoff);
pt3FilterInit(&posHold.upsamplePitchLpf, upsampleCutoff);
// initialise filters // initialise filters
// Reset parameters for both NS and EW // Reset parameters for both NS and EW
resetPositionControlParams(&posHold.NS); resetPositionControlParams(&posHold.NS);
@ -209,139 +219,138 @@ bool positionControl(void) {
return false; return false;
} }
posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s if (isNewDataForPosHold()) {
posHold.gpsDataFreqHz = 1.0f / posHold.gpsDataIntervalS; posHold.gpsDataIntervalS = getGpsDataIntervalSeconds(); // interval for current GPS data value 0.01s to 1.0s
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.NS);
resetPositionControlParams(&posHold.NS); resetPositionControlParams(&posHold.EW);
resetPositionControlParams(&posHold.EW); posHold.pidSumRoll = 0.0f;
autopilotAngle[AI_ROLL] = 0.0f; posHold.pidSumPitch = 0.0f;
autopilotAngle[AI_PITCH] = 0.0f; } else {
} 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
// first get xy distances from current location (gpsSol.llh) to target location float ewDistance; // cm, steps of 11.1cm, East of target is positive
float nsDistance; // cm, steps of 11.1cm, North of target is positive GPS_distances(&gpsSol.llh, &currentTargetLocation, &nsDistance, &ewDistance);
float ewDistance; // cm, steps of 11.1cm, East of target is positive float distanceCm = sqrtf(sq(nsDistance) + sq(ewDistance));
GPS_distances(&gpsSol.llh, &currentTargetLocation, &nsDistance, &ewDistance);
float distanceCm = sqrtf(sq(nsDistance) + sq(ewDistance)); posHold.NS.distance = nsDistance;
posHold.EW.distance = ewDistance;
posHold.NS.distance = nsDistance;
posHold.EW.distance = ewDistance; posHold.pt1Gain = pt1FilterGain(posHold.lpfCutoff, posHold.gpsDataIntervalS);
const float leak = 1.0f - 0.4f * posHold.gpsDataIntervalS; // gpsDataIntervalS is not more than 1.0s
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
// ** Sanity check ** if (posHold.NS.isStarting || posHold.EW.isStarting) {
// larger threshold if faster at start posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f;
if (posHold.NS.isStarting || posHold.EW.isStarting) { // 1s of flight at current speed or 10m, in cm
posHold.sanityCheckDistance = gpsSol.groundSpeed > 1000 ? gpsSol.groundSpeed : 1000.0f; }
// 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
// primarily to detect flyaway from no Mag or badly oriented Mag if (distanceCm > posHold.sanityCheckDistance) {
// but must accept some overshoot at the start, especially if entering at high speed return false; // must stay within 10m or probably flying away
if (distanceCm > posHold.sanityCheckDistance) { // value at this point is a 'best guess' to detect IMU failure in the event the user has no Mag
return false; // must stay within 10m or probably flying away // if entering poshold from a stable hover, we would only exceed this if IMU was disoriented
// value at this point is a 'best guess' to detect IMU failure in the event the user has no Mag // if entering poshold at speed, it may overshoot this value and falsely fail, if so need something more complex
// 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
}
vectors_t *vectors[] = { &posHold.NS, &posHold.EW };
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)
// ** P **
float pidP = latLong->distance * positionPidCoeffs.Kp;
// ** I **
if (!latLong->isStarting){
// only accumulate iTerm after completing the start phase
// perhaps need a timeout on the start phase ?
latLong->integral += latLong->distance * posHold.gpsDataIntervalS;
} else {
// while moving sticks, slowly leak iTerm away, approx 2s time constant
latLong->integral *= leak;
} }
float pidI = latLong->integral * positionPidCoeffs.Ki;
// ** D ** // vectors_t *vectors[] = { &posHold.NS, &posHold.EW };
// get change in distance in NS and EW directions from gps.c using the `GPS_distances` function for (int i = 0; i < 2; i++) {
// this gives cleaner velocity data than the module supplied GPS Speed and Heading information vectors_t *latLong = vectors[i];
float velocity = (latLong->distance - latLong->previousDistance) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
latLong->previousDistance = latLong->distance;
float acceleration = (velocity - latLong->previousVelocity) * posHold.gpsDataFreqHz;
latLong->previousVelocity = velocity;
// scale and filter - filter cutoffs vary during the startup phase // separate PID controllers for latitude (NorthSouth or ns) and longitude (EastWest or ew)
float pidD = velocity * positionPidCoeffs.Kd;
pt1FilterUpdateCutoff(&latLong->velocityLpf, posHold.pt1Gain);
pidD = pt1FilterApply(&latLong->velocityLpf, pidD);
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 // ** P **
const float maxDAAngle = 35.0f; // limit in degrees; arbitrary. 20 is a bit too low, allows a lot of overshoot float pidP = latLong->distance * positionPidCoeffs.Kp;
// 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? // ** I **
const float pidDA = constrainf(pidD + pidA, -maxDAAngle, maxDAAngle); if (!latLong->isStarting){
// only accumulate iTerm after completing the start phase
// ** PID Sum ** // perhaps need a timeout on the start phase ?
float pidSum = pidP + pidI + pidDA; latLong->integral += latLong->distance * posHold.gpsDataIntervalS;
// 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
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?
} else { } else {
currentTargetLocation.lon = gpsSol.llh.lon; // while moving sticks, slowly leak iTerm away, approx 2s time constant
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
float velocity = (latLong->distance - latLong->previousDistance) * posHold.gpsDataFreqHz; // cm/s, minimum step 11.1 cm/s
latLong->previousDistance = latLong->distance;
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);
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);
// ** 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
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?
} else {
currentTargetLocation.lon = gpsSol.llh.lon;
}
latLong->distance = 0.0f;
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));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(latLong->distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(latLong->pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(pidI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidDA * 10));
} }
latLong->distance = 0.0f;
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));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 1, lrintf(latLong->distance));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 2, lrintf(latLong->pidSum * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 4, lrintf(pidP * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 5, lrintf(pidI * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 6, lrintf(pidDA * 10));
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 7, lrintf(latLong->integral * 10));
} }
} }
// ** Rotate pid Sum to quad frame of reference, into pitch and roll ** // ** 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); // will be constrained to +/-pi in sin_approx()
const float sinHeading = sin_approx(headingRads); const float sinHeading = sin_approx(headingRads);
const float cosHeading = cos_approx(headingRads); const float cosHeading = cos_approx(headingRads);
float pidSumRoll = -sinHeading * posHold.NS.pidSum + cosHeading * posHold.EW.pidSum;
float pidSumPitch = cosHeading * posHold.NS.pidSum + sinHeading * posHold.EW.pidSum;
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) { posHold.pidSumRoll = -sinHeading * posHold.NS.pidSum + cosHeading * posHold.EW.pidSum;
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(pidSumRoll * 10)); posHold.pidSumPitch = cosHeading * posHold.NS.pidSum + sinHeading * posHold.EW.pidSum;
} else {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(pidSumPitch * 10));
}
// todo: fix the upsample filtering in pid.c, because
// pidSum has steps at GPS rate, and needs to change the pid.c upsampling filter for smoothness.
// ** Final output to pid.c Angle Mode **
autopilotAngle[AI_ROLL] = pidSumRoll;
autopilotAngle[AI_PITCH] = pidSumPitch;
} }
// ** 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);
if (gyroConfig()->gyro_filter_debug_axis == FD_ROLL) {
DEBUG_SET(DEBUG_AUTOPILOT_POSITION, 3, lrintf(posHold.pidSumRoll * 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, 7, lrintf(autopilotAngle[AI_PITCH] * 10));
}
return true; return true;
} }

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

@ -26,9 +26,9 @@
#include "config/config.h" #include "config/config.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/rc.h" #include "fc/rc.h"
#include "flight/autopilot.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/position.h" #include "flight/position.h"
#include "flight/autopilot.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "pos_hold.h" #include "pos_hold.h"
@ -89,10 +89,8 @@ void updatePosHold(timeUs_t currentTimeUs) {
// check for enabling Pod Hold, otherwise do as little as possible while inactive // check for enabling Pod Hold, otherwise do as little as possible while inactive
posHoldStart(); posHoldStart();
if (posHold.posHoldIsOK) { if (posHold.posHoldIsOK) {
if (isNewDataForPosHold()) { posHoldCheckSticks();
posHoldCheckSticks(); posHold.posHoldIsOK = positionControl();
posHold.posHoldIsOK = positionControl();
}
} else { } else {
autopilotAngle[AI_PITCH] = 0.0f; autopilotAngle[AI_PITCH] = 0.0f;
autopilotAngle[AI_ROLL] = 0.0f; autopilotAngle[AI_ROLL] = 0.0f;

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

@ -130,7 +130,9 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
bool compassIsHealthy; bool compassIsHealthy;
float getGpsDataIntervalSeconds(void) { return 0.01f; } float getGpsDataIntervalSeconds(void) { return 0.01f; }
float getRcDeflectionAbs(void) { return 0.0f; } float getRcDeflectionAbs(void) { return 0.0f; }
attitudeEulerAngles_t attitude; attitudeEulerAngles_t attitude;
bool isNewDataForPosHold(void){ return true; }
void parseRcChannels(const char *input, rxConfig_t *rxConfig) void parseRcChannels(const char *input, rxConfig_t *rxConfig)
{ {
@ -144,19 +146,19 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
UNUSED(dT); UNUSED(dT);
return 0.0; return 0.0;
} }
void pt1FilterInit(pt1Filter_t *filter, float k) void pt1FilterInit(pt1Filter_t *filter, float k)
{ {
UNUSED(filter); UNUSED(filter);
UNUSED(k); UNUSED(k);
} }
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k) void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k)
{ {
UNUSED(filter); UNUSED(filter);
UNUSED(k); UNUSED(k);
} }
float pt1FilterApply(pt1Filter_t *filter, float input) float pt1FilterApply(pt1Filter_t *filter, float input)
{ {
UNUSED(filter); UNUSED(filter);
@ -170,19 +172,18 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
UNUSED(dT); UNUSED(dT);
return 0.0; return 0.0;
} }
void pt2FilterInit(pt2Filter_t *filter, float k) void pt2FilterInit(pt2Filter_t *filter, float k)
{ {
UNUSED(filter); UNUSED(filter);
UNUSED(k); UNUSED(k);
} }
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k) void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
{ {
UNUSED(filter); UNUSED(filter);
UNUSED(k); UNUSED(k);
} }
float pt2FilterApply(pt2Filter_t *filter, float input) float pt2FilterApply(pt2Filter_t *filter, float input)
{ {
UNUSED(filter); UNUSED(filter);
@ -190,6 +191,29 @@ void GPS_distances(const gpsLocation_t *from, const gpsLocation_t *to, float *pN
return 0.0; return 0.0;
} }
float pt3FilterGain(float f_cut, float dT)
{
UNUSED(f_cut);
UNUSED(dT);
return 0.0;
}
void pt3FilterInit(pt3Filter_t *filter, float k)
{
UNUSED(filter);
UNUSED(k);
}
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
{
UNUSED(filter);
UNUSED(k);
}
float pt3FilterApply(pt3Filter_t *filter, float input)
{
UNUSED(filter);
UNUSED(input);
return 0.0;
}
int16_t debug[DEBUG16_VALUE_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT];
uint8_t debugMode; uint8_t debugMode;

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

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