Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Convert all NAV P-controllers to PIDFF without the possibility to change settings

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-04-19 11:10:07 +02:00
parent fa4ae8f328
commit 32eeefad1b
3 changed files with 20 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

32
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,
(float)pidProfile()->bank_mc.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_POS_XY].D / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_POS_XY].FF / 100.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,12 +3046,19 @@ 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,
(float)pidProfile()->bank_mc.pid[PID_POS_Z].I / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_POS_Z].D / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_POS_Z].FF / 100.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,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
0.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].FF / 100.0f,
NAV_DTERM_CUT_HZ
);
@ -3069,14 +3075,14 @@ void navigationUsePIDs(void)
navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
0.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].FF / 100.0f,
NAV_DTERM_CUT_HZ
);
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,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
NAV_DTERM_CUT_HZ
);
}