Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 17:55:28 +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

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