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

Merge branch 'mpc_fix' into vector_control2

Browse Source
This commit is contained in:
Anton Babushkin 2013-12-13 17:26:41 +04:00
commit 18eefa9dca
7 changed files with 128 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/multirotor_att_control/multirotor_attitude_control.c
View File

@ -194,8 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
pid_init(&pitch_controller, PID_MODE_DERIVATIV_SET, 0.0f);
pid_init(&roll_controller, PID_MODE_DERIVATIV_SET, 0.0f);
initialized = true;
}

6
src/modules/multirotor_att_control/multirotor_rate_control.c
View File

@ -161,9 +161,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
parameters_update(&h, &p);
initialized = true;
pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&pitch_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&roll_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
pid_init(&yaw_rate_controller, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
}
/* load new parameters with lower rate */

39
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -244,39 +244,41 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
parameters_init(&params_h);
parameters_update(&params_h, &params);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
pid_init(&(xy_pos_pids[i]), PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
pid_init(&z_pos_pid, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, 0.02f);
bool param_updated = true;
while (!thread_should_exit) {
bool param_updated;
orb_check(param_sub, &param_updated);
if (!param_updated)
orb_check(param_sub, &param_updated);
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update params */
parameters_update(&params_h, &params);
/* integral_limit * ki = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i > 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i > 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 0.0f, 0.0f);
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
@ -471,6 +473,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;

105
src/modules/multirotor_pos_control/thrust_pid.c
View File

@ -43,22 +43,22 @@
#include "thrust_pid.h"
#include <math.h>
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
#define COS_TILT_MAX 0.7f
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->limit_min = limit_min;
pid->limit_max = limit_max;
pid->mode = mode;
pid->dt_min = dt_min;
pid->last_output = 0.0f;
pid->sp = 0.0f;
pid->error_previous = 0.0f;
pid->kp = 0.0f;
pid->ki = 0.0f;
pid->kd = 0.0f;
pid->integral = 0.0f;
pid->output_min = 0.0f;
pid->output_max = 0.0f;
pid->error_previous = 0.0f;
pid->last_output = 0.0f;
}
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max)
{
int ret = 0;
@ -83,15 +83,15 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl
ret = 1;
}
if (isfinite(limit_min)) {
pid->limit_min = limit_min;
if (isfinite(output_min)) {
pid->output_min = output_min;
} else {
ret = 1;
}
if (isfinite(limit_max)) {
pid->limit_max = limit_max;
if (isfinite(output_max)) {
pid->output_max = output_max;
} else {
ret = 1;
@ -102,40 +102,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
{
/* Alternative integral component calculation
*
* start:
* error = setpoint - current_value
* integral = integral + (Ki * error * dt)
* derivative = (error - previous_error) / dt
* previous_error = error
* output = (Kp * error) + integral + (Kd * derivative)
* wait(dt)
* goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
pid->sp = sp;
// Calculated current error value
float error = pid->sp - val;
/* error value */
float error = sp - val;
// Calculate or measured current error derivative
if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
} else {
d = 0.0f;
}
/* error derivative */
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
if (!isfinite(d)) {
d = 0.0f;
@ -145,36 +123,41 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa
i = pid->integral + (pid->ki * error * dt);
/* attitude-thrust compensation
* r22 is (2, 2) componet of rotation matrix for current attitude */
* r22 is (2, 2) component of rotation matrix for current attitude */
float att_comp;
if (r22 > 0.8f)
if (r22 > COS_TILT_MAX) {
att_comp = 1.0f / r22;
else if (r22 > 0.0f)
att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
else
att_comp = 1.0f;
/* calculate output */
float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
/* check for saturation */
if (output < pid->limit_min || output > pid->limit_max) {
/* saturated, recalculate output with old integral */
output = (error * pid->kp) + pid->integral + (d * pid->kd);
} else if (r22 > 0.0f) {
att_comp = ((1.0f / COS_TILT_MAX - 1.0f) / COS_TILT_MAX) * r22 + 1.0f;
} else {
if (isfinite(i)) {
att_comp = 1.0f;
}
/* calculate PD output */
float output = ((error * pid->kp) + (d * pid->kd)) * att_comp;
/* check for saturation */
if (isfinite(i)) {
float i_comp = i * att_comp;
if ((output + i_comp) >= pid->output_min || (output + i_comp) <= pid->output_max) {
/* not saturated, use new integral value */
pid->integral = i;
}
}
if (isfinite(output)) {
if (output > pid->limit_max) {
output = pid->limit_max;
/* add I component to output */
output += pid->integral * att_comp;
} else if (output < pid->limit_min) {
output = pid->limit_min;
/* limit output */
if (isfinite(output)) {
if (output > pid->output_max) {
output = pid->output_max;
} else if (output < pid->output_min) {
output = pid->output_min;
}
pid->last_output = output;

17
src/modules/multirotor_pos_control/thrust_pid.h
View File

@ -47,27 +47,20 @@
__BEGIN_DECLS
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
#define THRUST_PID_MODE_DERIVATIV_CALC 0
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1
typedef struct {
float dt_min;
float kp;
float ki;
float kd;
float sp;
float integral;
float output_min;
float output_max;
float error_previous;
float last_output;
float limit_min;
float limit_max;
float dt_min;
uint8_t mode;
} thrust_pid_t;
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
__EXPORT void thrust_pid_init(thrust_pid_t *pid, float dt_min);
__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float output_min, float output_max);
__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);

105
src/modules/systemlib/pid/pid.c
View File

@ -39,7 +39,7 @@
/**
* @file pid.c
*
* Implementation of generic PID control interface.
* Implementation of generic PID controller.
*
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
@ -53,24 +53,21 @@
#define SIGMA 0.000001f
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
float limit, uint8_t mode, float dt_min)
__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
pid->limit = limit;
pid->mode = mode;
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->kp = 0.0f;
pid->ki = 0.0f;
pid->kd = 0.0f;
pid->integral = 0.0f;
pid->integral_limit = 0.0f;
pid->output_limit = 0.0f;
pid->error_previous = 0.0f;
pid->last_output = 0.0f;
}
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
{
int ret = 0;
@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
ret = 1;
}
if (isfinite(intmax)) {
pid->intmax = intmax;
if (isfinite(integral_limit)) {
pid->integral_limit = integral_limit;
} else {
ret = 1;
}
if (isfinite(limit)) {
pid->limit = limit;
if (isfinite(output_limit)) {
pid->output_limit = output_limit;
} else {
ret = 1;
@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
return ret;
}
//void pid_set(PID_t *pid, float sp)
//{
// pid->sp = sp;
// pid->error_previous = 0;
// pid->integral = 0;
//}
/**
*
* @param pid
* @param val
* @param dt
* @return
*/
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
{
/* error = setpoint - actual_position
integral = integral + (error*dt)
derivative = (error - previous_error)/dt
output = (Kp*error) + (Ki*integral) + (Kd*derivative)
previous_error = error
wait(dt)
goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) {
return pid->last_output;
}
float i, d;
pid->sp = sp;
// Calculated current error value
float error = pid->sp - val;
/* current error value */
float error = sp - val;
// Calculate or measured current error derivative
/* current error derivative */
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
@ -167,39 +140,29 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = 0.0f;
}
if (pid->ki > 0.0f) {
// Calculate the error integral and check for saturation
i = pid->integral + (error * dt);
if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
fabsf(i) > pid->intmax) {
i = pid->integral; // If saturated then do not update integral value
pid->saturated = 1;
} else {
if (!isfinite(i)) {
i = 0.0f;
}
/* calculate PD output */
float output = (error * pid->kp) + (d * pid->kd);
/* check for saturation */
if (isfinite(i)) {
if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
fabsf(i) <= pid->integral_limit) {
/* not saturated, use new integral value */
pid->integral = i;
pid->saturated = 0;
}
} else {
i = 0.0f;
pid->saturated = 0;
}
// Calculate the output. Limit output magnitude to pid->limit
float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
/* add I component to output */
output += pid->integral * pid-> ki;
/* limit output */
if (isfinite(output)) {
if (pid->limit > SIGMA) {
if (output > pid->limit) {
output = pid->limit;
if (pid->output_limit > SIGMA) {
if (output > pid->output_limit) {
output = pid->output_limit;
} else if (output < -pid->limit) {
output = -pid->limit;
} else if (output < -pid->output_limit) {
output = -pid->output_limit;
}
}
@ -212,5 +175,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
__EXPORT void pid_reset_integral(PID_t *pid)
{
pid->integral = 0;
pid->integral = 0.0f;
}

41
src/modules/systemlib/pid/pid.h
View File

@ -39,7 +39,7 @@
/**
* @file pid.h
*
* Definition of generic PID control interface.
* Definition of generic PID controller.
*
* @author Laurens Mackay <mackayl@student.ethz.ch>
* @author Tobias Naegeli <naegelit@student.ethz.ch>
@ -55,38 +55,35 @@
__BEGIN_DECLS
/* 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 2
// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
#define PID_MODE_DERIVATIV_NONE 9
typedef enum PID_MODE {
/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
PID_MODE_DERIVATIV_NONE = 0,
/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
* val_dot in pid_calculate() will be ignored */
PID_MODE_DERIVATIV_CALC,
/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
* setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
PID_MODE_DERIVATIV_CALC_NO_SP,
/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
PID_MODE_DERIVATIV_SET
} pid_mode_t;
typedef struct {
pid_mode_t mode;
float dt_min;
float kp;
float ki;
float kd;
float intmax;
float sp;
float integral;
float integral_limit;
float output_limit;
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, 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 void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
__EXPORT void pid_reset_integral(PID_t *pid);
__END_DECLS