From 532c8faa98f9c7c463c85d2b8ea3d81776c6d0cd Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Mon, 28 Oct 2024 10:57:24 +1100 Subject: [PATCH] implement reviews, reduce PID gains --- src/main/common/maths.c | 2 +- src/main/fc/rc.c | 33 ++++++++++----------------------- src/main/fc/tasks.c | 4 ++-- src/main/flight/autopilot.c | 11 ++++++----- src/main/flight/autopilot.h | 2 +- src/main/flight/imu.c | 29 ++++++++++++++++------------- src/main/flight/pos_hold.c | 12 +++++++----- 7 files changed, 43 insertions(+), 50 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 277b61fc88..d47acd6ab9 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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] diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index ac8ce2b6fe..174a450d4d 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -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; diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 8a63a35def..fe3e871ff0 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -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 diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index 41064d1391..81ba7193cb 100644 --- a/src/main/flight/autopilot.c +++ b/src/main/flight/autopilot.c @@ -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; diff --git a/src/main/flight/autopilot.h b/src/main/flight/autopilot.h index 7a28f4c516..603a4c3c68 100644 --- a/src/main/flight/autopilot.h +++ b/src/main/flight/autopilot.h @@ -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(); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 7a86790c3a..c81e8c7f09 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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 diff --git a/src/main/flight/pos_hold.c b/src/main/flight/pos_hold.c index dc6c9c4cc8..5dd59130e8 100644 --- a/src/main/flight/pos_hold.c +++ b/src/main/flight/pos_hold.c @@ -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; } }