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

Merge branch 'master' into new_state_machine

Browse Source 
compiling again
Conflicts:
	src/modules/fixedwing_att_control/fixedwing_att_control_att.c
	src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
	src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
	src/modules/mavlink/orb_listener.c
	src/modules/multirotor_att_control/multirotor_attitude_control.c
	src/modules/multirotor_att_control/multirotor_rate_control.c
	src/modules/systemlib/pid/pid.c
	src/modules/systemlib/pid/pid.h
	src/modules/uORB/objects_common.cpp
This commit is contained in:
Julian Oes
2013-07-08 10:31:32 +02:00
parent 76346bfe19 cf2dbdf9a1
commit 88389ea255
2165 changed files with 10906 additions and 485356 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/pid/pid.c
+34 -36
View File
@@ -1,9 +1,11 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 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>
* Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
* Author: Laurens Mackay <mackayl@student.ethz.ch>
* Tobias Naegeli <naegelit@student.ethz.ch>
* Martin Rutschmann <rutmarti@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,14 +38,21 @@
/**
* @file pid.c
* Implementation of generic PID control interface
*
* Implementation of generic PID control interface.
*
* @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>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "pid.h"
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
float limit, float diff_filter_factor, uint8_t mode)
float limit, uint8_t mode, float dt_min)
{
pid->kp = kp;
pid->ki = ki;
@@ -51,17 +60,15 @@ __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->diff_filter_factor = diff_filter_factor;
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_filtered = 0.0f;
pid->control_previous = 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, float diff_filter_factor)
__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
{
int ret = 0;
@@ -100,13 +107,6 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
ret = 1;
}
if (isfinite(diff_filter_factor)) {
pid->diff_filter_factor = diff_filter_factor;
} else {
ret = 1;
}
return ret;
}
@@ -144,20 +144,16 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
// Calculated current error value
float error = pid->sp - val;
float error_filtered;
// Calculate or measured current error derivative
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = error;
error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor;
d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt
pid->error_previous_filtered = error_filtered;
} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
pid->error_previous = -val;
error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor;
d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt
pid->error_previous_filtered = error_filtered;
} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
d = -val_dot;
@@ -165,6 +161,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);
@@ -175,7 +175,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;
@@ -184,20 +184,18 @@ __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 = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (output > pid->limit) output = pid->limit;
if (output < -pid->limit) output = -pid->limit;
if (isfinite(output)) {
if (output > pid->limit) {
output = pid->limit;
} else if (output < -pid->limit) {
output = -pid->limit;
}
pid->last_output = output;
}
pid->control_previous = pid->last_output;
// if (isfinite(error)) { // Why is this necessary? DEW
// pid->error_previous = error;
// }
*ctrl_p = (error * pid->kp);
*ctrl_i = (i * pid->ki);
*ctrl_d = (d * pid->kd);