Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

maybe a final improvement

Browse source
This commit is contained in:
JuliooCesarMDM 2022-01-16 13:20:51 -03:00
parent 4956a7c86f
commit 4aa656e1ad
10 changed files with 65 additions and 69 deletions
Download patch file Download diff file Expand all files Collapse all files

2
docs/Settings.md
View file

@ -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 |
---

13
src/main/common/fp_pid.c
View file

@ -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);
}

4
src/main/common/fp_pid.h
View file

@ -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);
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz);

2
src/main/fc/settings.yaml
View file

@ -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

3
src/main/flight/pid.c
View file

@ -1260,7 +1260,8 @@ void pidInit(void)
(float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
0.0f,
0.0f,
2.0f
2.0f,
0.0f
);
}

34
src/main/navigation/navigation.c
View file

@ -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
);
}

3
src/main/navigation/navigation.h
View file

@ -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 */

68
src/main/navigation/navigation_multicopter.c
View file

@ -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];
}
/*-----------------------------------------------------------

2
src/main/navigation/navigation_private.h
View file

@ -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

3
src/main/programming/pid.c
View file

@ -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
);
}
}