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

Merge pull request #4624 from iNavFlight/dzikuvx-nav-pids-to-pidff

Browse source 
Convert all NAV P-controllers to PIDFF
This commit is contained in:
Paweł Spychalski 2019-04-21 17:42:38 +02:00 • committed by GitHub
commit dbf51470a3
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 17 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

2
docs/Cli.md
View file

@ -318,9 +318,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag
| accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
| nav_mc_pos_z_p | 50 | P gain of altitude PID controller (Multirotor) |
| nav_fw_pos_z_p | 50 | P gain of altitude PID controller (Fixedwing) |
| nav_mc_pos_z_i | 0 | I gain of altitude PID controller (Multirotor) |
| nav_fw_pos_z_i | 0 | I gain of altitude PID controller (Fixedwing) |
| nav_mc_pos_z_d | 0 | D gain of altitude PID controller (Multirotor) |
| nav_fw_pos_z_d | 0 | D gain of altitude PID controller (Fixedwing) |
| nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller |

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

@ -1023,16 +1023,6 @@ groups:
condition: USE_NAV
min: 0
max: 255
- name: nav_mc_pos_z_i
field: bank_mc.pid[PID_POS_Z].I
condition: USE_NAV
min: 0
max: 255
- name: nav_mc_pos_z_d
field: bank_mc.pid[PID_POS_Z].D
condition: USE_NAV
min: 0
max: 255
- name: nav_mc_vel_z_p
field: bank_mc.pid[PID_VEL_Z].P
condition: USE_NAV

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

@ -1737,14 +1737,6 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
navPidReset(pid);
}
/*-----------------------------------------------------------
* Float point P-controller implementation
*-----------------------------------------------------------*/
void navPInit(pController_t *p, float _kP)
{
p->param.kP = _kP;
}
/*-----------------------------------------------------------
* Detects if thrust vector is facing downwards
*-----------------------------------------------------------*/
@ -3036,7 +3028,14 @@ void navigationUsePIDs(void)
// Initialize position hold P-controller
for (int axis = 0; axis < 2; axis++) {
navPInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f);
navPidInit(
&posControl.pids.pos[axis],
(float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f,
0.0f,
0.0f,
0.0f,
NAV_DTERM_CUT_HZ
);
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,
@ -3047,7 +3046,14 @@ void navigationUsePIDs(void)
}
// Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
navPInit(&posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f);
navPidInit(
&posControl.pids.pos[Z],
(float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f,
0.0f,
0.0f,
0.0f,
NAV_DTERM_CUT_HZ
);
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,

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

@ -266,14 +266,9 @@ typedef struct {
float output_constrained; // controller output constrained
} pidController_t;
typedef struct {
pControllerParam_t param;
float output_constrained;
} pController_t;
typedef struct navigationPIDControllers_s {
/* Multicopter PIDs */
pController_t pos[XYZ_AXIS_COUNT];
pidController_t pos[XYZ_AXIS_COUNT];
pidController_t vel[XYZ_AXIS_COUNT];
pidController_t surface;

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

@ -357,7 +357,6 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
void navPidReset(pidController_t *pid);
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
void navPInit(pController_t *p, float _kP);
bool isThrustFacingDownwards(void);
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);