Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Convert all NAV P-controllers to PIDFF #4624

Merged
merged 3 commits into from
Apr 21, 2019
Merged

Convert all NAV P-controllers to PIDFF #4624

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fa4ae8f
Remove unused settings
DzikuVx Apr 19, 2019
32eeefa
Convert all NAV P-controllers to PIDFF without the possibility to cha…
DzikuVx Apr 19, 2019
b8ba10b
Review changes
DzikuVx Apr 20, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions docs/Cli.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand Down
10 changes: 0 additions & 10 deletions src/main/fc/settings.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
26 changes: 16 additions & 10 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*-----------------------------------------------------------*/
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand Down
7 changes: 1 addition & 6 deletions src/main/navigation/navigation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
1 change: 0 additions & 1 deletion src/main/navigation/navigation_private.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down