diff --git a/docs/Settings.md b/docs/Settings.md index f126548cfb..77690c30be 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -5708,7 +5708,7 @@ Enable the alternate softserial method. This is the method used in iNav 3.0 and | Default | Min | Max | | --- | --- | --- | -| ON | | | +| ON | OFF | ON | --- diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c index 346c6ace3f..63091415f7 100644 --- a/src/main/common/fp_pid.c +++ b/src/main/common/fp_pid.c @@ -47,7 +47,13 @@ float navPidApply3( const float dTermScaler ) { float newProportional, newDerivative, newFeedForward; - float error = setpoint - measurement; + float error = 0.0f; + + if (pid->errorLpfHz > 0.0f) { + error = pt1FilterApply4(&pid->error_filter_state, setpoint - measurement, pid->errorLpfHz, dt); + } else { + error = setpoint - measurement; + } /* P-term */ newProportional = error * pid->param.kP * gainScaler; @@ -68,7 +74,7 @@ float navPidApply3( pid->last_input = measurement; } - if (pid->dTermLpfHz > 0) { + if (pid->dTermLpfHz > 0.0f) { newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt); } else { newDerivative = pid->param.kD * newDerivative; @@ -138,7 +144,7 @@ void navPidReset(pidController_t *pid) pid->output_constrained = 0.0f; } -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz) +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz) { pid->param.kP = _kP; pid->param.kI = _kI; @@ -159,5 +165,6 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF pid->param.kT = 0.0; } pid->dTermLpfHz = _dTermLpfHz; + pid->errorLpfHz = _errorLpfHz; navPidReset(pid); } \ No newline at end of file diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h index 17a43bd920..c45b2719dd 100644 --- a/src/main/common/fp_pid.h +++ b/src/main/common/fp_pid.h @@ -48,7 +48,9 @@ typedef enum { typedef struct { bool reset; pidControllerParam_t param; + pt1Filter_t error_filter_state; pt1Filter_t dterm_filter_state; // last derivative for low-pass filter + float errorLpfHz; float dTermLpfHz; // dTerm low pass filter cutoff frequency float integrator; // integrator value float last_input; // last input for derivative @@ -73,4 +75,4 @@ float navPidApply3( const float dTermScaler ); void navPidReset(pidController_t *pid); -void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz); \ No newline at end of file +void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ef1f445b6e..00bbbafeeb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1949,7 +1949,6 @@ groups: min: HEADING_HOLD_RATE_LIMIT_MIN max: HEADING_HOLD_RATE_LIMIT_MAX default_value: 90 - - name: nav_mc_pos_z_p description: "P gain of altitude PID controller (Multirotor)" field: bank_mc.pid[PID_POS_Z].P @@ -1970,7 +1969,6 @@ groups: default_value: 50 - name: nav_mc_vel_z_d description: "D gain of velocity PID controller" - default_value: 10 field: bank_mc.pid[PID_VEL_Z].D min: 0 max: 255 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9ebb2e2628..3144eb177f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1260,7 +1260,8 @@ void pidInit(void) (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f, 0.0f, 0.0f, - 2.0f + 2.0f, + 0.0f ); } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 01f3733869..e1e3d5d1fb 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3692,14 +3692,16 @@ void navigationUsePIDs(void) 0.0f, 0.0f, 0.0f, - NAV_DTERM_CUT_HZ + NAV_DTERM_CUT_HZ, + 0.0f ); navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f, (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f, (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f, (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f, - pidProfile()->navVelXyDTermLpfHz + pidProfile()->navVelXyDTermLpfHz, + 0.0f ); } @@ -3721,24 +3723,16 @@ void navigationUsePIDs(void) 0.0f, 0.0f, 0.0f, - NAV_DTERM_CUT_HZ + NAV_DTERM_CUT_HZ, + 0.0f ); navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f, (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f, (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f, 0.0f, - NAV_DTERM_CUT_HZ - ); - - #define DEFAULT_ACCEL_P 0.5f - #define DEFAULT_ACCEL_I 1.0f - - navPidInit(&posControl.pids.accel, DEFAULT_ACCEL_P, - DEFAULT_ACCEL_I, - 0.0f, - 0.0f, - NAV_DTERM_CUT_HZ + NAV_VEL_Z_DERIVATIVE_CUT_HZ, + NAV_VEL_Z_ERROR_CUT_HZ ); // Initialize surface tracking PID @@ -3746,7 +3740,8 @@ void navigationUsePIDs(void) 0.0f, 0.0f, 0.0f, - NAV_DTERM_CUT_HZ + NAV_DTERM_CUT_HZ, + 0.0f ); /** Airplane PIDs */ @@ -3755,21 +3750,24 @@ void navigationUsePIDs(void) (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f, 0.0f, - NAV_DTERM_CUT_HZ + NAV_DTERM_CUT_HZ, + 0.0f ); navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f, 0.0f, - NAV_DTERM_CUT_HZ + NAV_DTERM_CUT_HZ, + 0.0f ); navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f, 0.0f, - 2.0f + 2.0f, + 0.0f ); } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index ddecc57135..4e905e0284 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -374,9 +374,8 @@ typedef struct navDestinationPath_s { typedef struct navigationPIDControllers_s { /* Multicopter PIDs */ - pidController_t pos[XYZ_AXIS_COUNT]; + pidController_t pos[XYZ_AXIS_COUNT]; pidController_t vel[XYZ_AXIS_COUNT]; - pidController_t accel; pidController_t surface; /* Fixed-wing PIDs */ diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 9ccf1a7834..c1b7b5641b 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -61,7 +61,6 @@ static int16_t altHoldThrottleRCZero = 1500; static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; -static float accel_integrator_max = 500.0f; // Position to velocity controller for Z axis static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) @@ -74,14 +73,16 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) posControl.desiredState.pos.z = pos_desired_z; -/* // hard limit desired target velocity to max_climb_rate + float vel_max_z = 0.0f; + if (posControl.flags.isAdjustingAltitude) { - targetVel = constrainf(targetVel, -navConfig()->general.max_manual_climb_rate, navConfig()->general.max_manual_climb_rate); + vel_max_z = navConfig()->general.max_manual_climb_rate; } else { - targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate); + vel_max_z = navConfig()->general.max_auto_climb_rate; } -*/ + + targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); posControl.pids.pos[Z].output_constrained = targetVel; @@ -114,37 +115,14 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros) // Calculate min and max throttle boundaries (to compensate for integral windup) const int16_t thrAdjustmentMin = (int16_t)getThrottleIdleValue() - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; const int16_t thrAdjustmentMax = (int16_t)motorConfig()->maxthrottle - (int16_t)currentBatteryProfile->nav.mc.hover_throttle; - + float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrAdjustmentMin, thrAdjustmentMax, 0); - float accel_max_z = 0.0f; - - if (posControl.flags.isAdjustingAltitude) { - accel_max_z = navConfig()->general.max_manual_climb_rate; - } else { - accel_max_z = navConfig()->general.max_auto_climb_rate; - } - - velocity_controller = constrainf(velocity_controller, -accel_max_z, accel_max_z); - - const float thr_hover_converted = scaleRangef((float)currentBatteryProfile->nav.mc.hover_throttle, 1000.0f, 2000.0f, 200.0f, 800.0f); - - // ensure imax is always large enough to overpower hover throttle - if (posControl.pids.accel.integrator > accel_integrator_max) { - accel_integrator_max = posControl.pids.accel.integrator; - } - - if (thr_hover_converted > accel_integrator_max) { - accel_integrator_max = thr_hover_converted; - } + posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); - float acceleration_controller = navPidApply2(&posControl.pids.accel, velocity_controller, navGetCurrentActualPositionAndVelocity()->vel.z, - US2S(deltaMicros), -accel_integrator_max, accel_integrator_max, PID_LIMIT_INTEGRATOR); - - posControl.rcAdjustment[THROTTLE] = acceleration_controller; - - posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax); + + posControl.rcAdjustment[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); } bool adjustMulticopterAltitudeFromRCInput(void) @@ -228,9 +206,11 @@ void setupMulticopterAltitudeController(void) void resetMulticopterAltitudeController(void) { const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity(); + float nav_speed_up = 0.0f; + float nav_speed_down = 0.0f; + float nav_accel_z = 0.0f; navPidReset(&posControl.pids.vel[Z]); - navPidReset(&posControl.pids.accel); navPidReset(&posControl.pids.surface); posControl.rcAdjustment[THROTTLE] = 0; @@ -238,12 +218,21 @@ void resetMulticopterAltitudeController(void) posControl.desiredState.vel.z = posToUse->vel.z; // Gradually transition from current climb pt1FilterReset(&altholdThrottleFilterState, 0.0f); + pt1FilterReset(&posControl.pids.vel[Z].error_filter_state, 0.0f); pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f); - pt1FilterReset(&posControl.pids.accel.dterm_filter_state, 0.0f); - - sqrt_controller_set_limits(&alt_hold_sqrt_controller, -fabsf((float)navConfig()->general.max_manual_climb_rate), (float)navConfig()->general.max_manual_climb_rate, (float)navConfig()->general.max_auto_climb_rate); - accel_integrator_max = (float)currentBatteryProfile->nav.mc.hover_throttle - (float)rcCommand[THROTTLE]; + if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + const float maxSpeed = getActiveWaypointSpeed(); + nav_speed_up = maxSpeed; + nav_accel_z = maxSpeed; + nav_speed_down = navConfig()->general.max_auto_climb_rate; + } else { + nav_speed_up = navConfig()->general.max_manual_speed; + nav_accel_z = navConfig()->general.max_manual_speed; + nav_speed_down = navConfig()->general.max_manual_climb_rate; + } + + sqrt_controller_set_limits(&alt_hold_sqrt_controller, -fabsf(nav_speed_down), nav_speed_up, nav_accel_z); } static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) @@ -264,7 +253,6 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate; posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP); posControl.pids.vel[Z].integrator = -500.0f; - posControl.pids.accel.integrator = -500; pt1FilterReset(&altholdThrottleFilterState, -500.0f); prepareForTakeoffOnReset = false; } @@ -283,7 +271,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE]; // Save processed throttle for future use rcCommandAdjustedThrottle = rcCommand[THROTTLE]; @@ -822,7 +810,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) } // Update throttle controller - rcCommand[THROTTLE] = constrain((int16_t)currentBatteryProfile->nav.mc.hover_throttle + posControl.rcAdjustment[THROTTLE], getThrottleIdleValue(), motorConfig()->maxthrottle); + rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE]; } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 67497f5ec2..cc24e0d853 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -31,6 +31,8 @@ #define NAV_THROTTLE_CUTOFF_FREQENCY_HZ 4 // low-pass filter on throttle output #define NAV_FW_CONTROL_MONITORING_RATE 2 #define NAV_DTERM_CUT_HZ 10.0f +#define NAV_VEL_Z_DERIVATIVE_CUT_HZ 5.0f +#define NAV_VEL_Z_ERROR_CUT_HZ 5.0f #define NAV_ACCELERATION_XY_MAX 980.0f // cm/s/s // approx 45 deg lean angle #define INAV_SURFACE_MAX_DISTANCE 40 diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 8b284087eb..0283227f45 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -105,7 +105,8 @@ void programmingPidInit(void) programmingPids(i)->gains.I / 1000.0f, programmingPids(i)->gains.D / 1000.0f, programmingPids(i)->gains.FF / 1000.0f, - 5.0f + 5.0f, + 0.0f ); } }