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

implement reviews, reduce PID gains

Browse source
This commit is contained in:
ctzsnooze 2024-10-28 10:57:24 +11:00
parent d5d38a422f
commit 532c8faa98
7 changed files with 43 additions and 50 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/maths.c
View file

@ -54,7 +54,7 @@ float sin_approx(float x)
{
// Wrap angle to 2π with range [-π π]
x = fmodf(x, 2.0f * M_PIf);
if (x < -M_PIf) x += 2.0f * M_PIf;
if (x <= -M_PIf) x += 2.0f * M_PIf;
if (x > M_PIf) x -= 2.0f * M_PIf;
// Use axis symmetry around x = ±π/2 for polynomial outside of range [-π/2 π/2]

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

@ -753,41 +753,28 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
isRxDataNew = true;
for (int axis = 0; axis < 3; axis++) {
float tmp = MIN(fabsf(rcData[axis] - rxConfig()->midrc), 500.0f); // -500 to 500
float rc = constrainf(rcData[axis] - rxConfig()->midrc, -500.0f, 500.0f); // -500 to 500
float rcDeadband = 0;
if (axis == ROLL || axis == PITCH) {
rcDeadband = rcControlsConfig()->deadband;
#ifdef USE_POS_HOLD_MODE
float tmpDeadband = rcControlsConfig()->deadband;
if (FLIGHT_MODE(POS_HOLD_MODE)) {
// override deadband in POS_HOLD_MODE
if (posHoldConfig()->pos_hold_deadband) {
// if pos_hold_deadband is defined, ignore pitch & roll within deadband zone
tmpDeadband = posHoldConfig()->pos_hold_deadband * 5.0f;
// if pos_hold_deadband is defined, ignore pitch & roll within deadband zone (in %)
rcDeadband = posHoldConfig()->pos_hold_deadband * 5.0f;
// NB could attenuate RP responsiveness outside deadband here, with tmp * 0.8f or whatever
} else {
// if pos_hold_deadband is zero, prevent user adjustment of pitch or roll
tmp = 0;
rcDeadband = 500; // can't exceed this
}
}
#else
const float tmpDeadband = rcControlsConfig()->deadband;
#endif
if (tmp > tmpDeadband) {
tmp -= tmpDeadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp;
} else {
if (tmp > rcControlsConfig()->yaw_deadband) {
tmp -= rcControlsConfig()->yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = tmp * -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
}
if (rcData[axis] < rxConfig()->midrc) {
rcCommand[axis] = -rcCommand[axis];
rcDeadband = rcControlsConfig()->yaw_deadband;
rc *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
}
rcCommand[axis] = fapplyDeadband(rc, rcDeadband);
}
int32_t tmp;

4
src/main/fc/tasks.c
View file

@ -545,11 +545,11 @@ void tasksInit(void)
#endif
#ifdef USE_ALT_HOLD_MODE
setTaskEnabled(TASK_ALTHOLD, true);
setTaskEnabled(TASK_ALTHOLD, sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS));
#endif
#ifdef USE_POS_HOLD_MODE
setTaskEnabled(TASK_POSHOLD, true);
setTaskEnabled(TASK_POSHOLD, featureIsEnabled(FEATURE_GPS));
#endif
#ifdef USE_MAG

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

@ -40,9 +40,9 @@
#define ALTITUDE_D_SCALE 0.01f
#define ALTITUDE_F_SCALE 0.01f
#define POSITION_P_SCALE 0.001f
#define POSITION_I_SCALE 0.0005f
#define POSITION_D_SCALE 0.002f
#define POSITION_A_SCALE 0.002f
#define POSITION_I_SCALE 0.0001f
#define POSITION_D_SCALE 0.0015f
#define POSITION_A_SCALE 0.0015f
static pidCoefficient_t altitudePidCoeffs;
static pidCoefficient_t positionPidCoeffs;
@ -190,7 +190,7 @@ void setSticksActiveStatus(bool areSticksActive)
posHold.sticksActive = areSticksActive;
}
void updateTargetLocation(gpsLocation_t newTargetLocation) {
void setTargetLocation(gpsLocation_t newTargetLocation) {
currentTargetLocation = newTargetLocation;
}
@ -237,6 +237,7 @@ bool positionControl(void) {
// larger threshold if faster at start
if (posHold.NS.isStarting || posHold.EW.isStarting) {
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
@ -322,7 +323,7 @@ bool positionControl(void) {
}
// ** Rotate pid Sum to quad frame of reference, into pitch and roll **
float headingRads = (attitude.values.yaw / 10.0f) * RAD; // 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 cosHeading = cos_approx(headingRads);
float pidSumRoll = -sinHeading * posHold.NS.pidSum + cosHeading * posHold.EW.pidSum;

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

@ -27,7 +27,7 @@ void autopilotInit(const autopilotConfig_t *config);
void resetAltitudeControl(void);
void setSticksActiveStatus(bool areSticksActive);
void resetPositionControl(gpsLocation_t initialTargetLocation);
void updateTargetLocation(gpsLocation_t newTargetLocation);
void setTargetLocation(gpsLocation_t newTargetLocation);
void altitudeControl(float targetAltitudeCm, float taskIntervalS, float verticalVelocity, float targetAltitudeStep);
bool positionControl();

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

@ -611,19 +611,22 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
#if defined(USE_GPS)
static void isGpsHeadingUsable(float groundspeedGain, float imuCourseError, float dt)
{
// ** ATTEMPT TO DETECT SUCCESSFUL IMU ORIENTATION TO GPS **
static float gpsHeadingTruth = 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;
// 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;
// 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 = !canUseGPSHeading && (gpsHeadingTruth > 2.0f);
// latches true; reset on disarm; used to stop position hold without a suitable GPS heading, if needed
if (!canUseGPSHeading) {
static float gpsHeadingTruth = 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;
// 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;
// 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 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
}
}
#endif

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

@ -59,7 +59,7 @@ void posHoldCheckSticks(void)
// while sticks are outside the deadband,
// keep updating the home location to the current GPS location each iteration
setSticksActiveStatus(true);
updateTargetLocation(gpsSol.llh);
setTargetLocation(gpsSol.llh);
} else {
setSticksActiveStatus(false);
}
@ -68,17 +68,19 @@ void posHoldCheckSticks(void)
}
void posHoldStart(void) {
static bool wasInPosHoldMode = false;
static bool isInPosHoldMode = false;
if (FLIGHT_MODE(POS_HOLD_MODE)) {
if (!wasInPosHoldMode) {
if (!isInPosHoldMode) {
// start position hold mode
posHold.posHoldIsOK = true; // true when started, false when autopilot code reports failure
setSticksActiveStatus(false);
resetPositionControl(gpsSol.llh);
wasInPosHoldMode = true;
isInPosHoldMode = true;
}
} else {
// stop position hold mode
posHold.posHoldIsOK = false;
wasInPosHoldMode = false;
isInPosHoldMode = false;
}
}