fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_attitude_control performance improved, tested in flight. PID library new functionality and bugfixes.

Browse Source
This commit is contained in:
Anton Babushkin
2013-06-20 19:25:37 +04:00
parent 85b5da8078
commit 5cb1f4662f
7 changed files with 70 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pid/pid.c
+29 -22
View File
@@ -1,9 +1,10 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -43,7 +44,7 @@
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
float limit, uint8_t mode)
float limit, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
@@ -51,13 +52,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
pid->intmax = intmax;
pid->limit = limit;
pid->mode = mode;
pid->count = 0;
pid->saturated = 0;
pid->last_output = 0;
pid->sp = 0;
pid->error_previous = 0;
pid->integral = 0;
pid->dt_min = dt_min;
pid->count = 0.0f;
pid->saturated = 0.0f;
pid->last_output = 0.0f;
pid->sp = 0.0f;
pid->error_previous = 0.0f;
pid->integral = 0.0f;
}
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
{
@@ -136,14 +137,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculated current error value
float error = pid->sp - val;
if (isfinite(error)) { // Why is this necessary? DEW
pid->error_previous = error;
}
// Calculate or measured current error derivative
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / dt;
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;
@@ -152,6 +153,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = 0.0f;
}
if (!isfinite(d)) {
d = 0.0f;
}
// Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
@@ -162,7 +167,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
} else {
if (!isfinite(i)) {
i = 0;
i = 0.0f;
}
pid->integral = i;
@@ -170,17 +175,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
}
// Calculate the output. Limit output magnitude to pid->limit
float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (output > pid->limit) output = pid->limit;
if (output < -pid->limit) output = -pid->limit;
float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (isfinite(output)) {
if (output > pid->limit) {
output = pid->limit;
} else if (output < -pid->limit) {
output = -pid->limit;
}
pid->last_output = output;
}
return pid->last_output;
}
src/modules/systemlib/pid/pid.h
+8 -3
View File
@@ -1,9 +1,10 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
* @author Martin Rutschmann <rutmarti@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -47,8 +48,11 @@
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
* val_dot in pid_calculate() will be ignored */
#define PID_MODE_DERIVATIV_CALC_NO_SP 1
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
#define PID_MODE_DERIVATIV_SET 1
#define PID_MODE_DERIVATIV_SET 2
// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
#define PID_MODE_DERIVATIV_NONE 9
@@ -62,12 +66,13 @@ typedef struct {
float error_previous;
float last_output;
float limit;
float dt_min;
uint8_t mode;
uint8_t count;
uint8_t saturated;
} PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);