Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 07:15:16 +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); 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 * Detects if thrust vector is facing downwards
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
@ -3036,7 +3028,14 @@ void navigationUsePIDs(void)
// Initialize position hold P-controller // Initialize position hold P-controller
for (int axis = 0; axis < 2; axis++) { 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, 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].I / 100.0f,
@ -3047,12 +3046,19 @@ void navigationUsePIDs(void)
} }
// Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z // 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, 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].I / 20.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.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 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, 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].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 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 NAV_DTERM_CUT_HZ
); );
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.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].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 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 NAV_DTERM_CUT_HZ
); );
} }

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

@ -266,14 +266,9 @@ typedef struct {
float output_constrained; // controller output constrained float output_constrained; // controller output constrained
} pidController_t; } pidController_t;
typedef struct {
pControllerParam_t param;
float output_constrained;
} pController_t;
typedef struct navigationPIDControllers_s { typedef struct navigationPIDControllers_s {
/* Multicopter PIDs */ /* Multicopter PIDs */
pController_t pos[XYZ_AXIS_COUNT]; pidController_t pos[XYZ_AXIS_COUNT];
pidController_t vel[XYZ_AXIS_COUNT]; pidController_t vel[XYZ_AXIS_COUNT];
pidController_t surface; 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); 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 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);
void navPInit(pController_t *p, float _kP);
bool isThrustFacingDownwards(void); bool isThrustFacingDownwards(void);
uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos); uint32_t calculateDistanceToDestination(const fpVector3_t * destinationPos);