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

WIP on HIL

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-22 20:55:44 +02:00
parent a9b21886f3
commit 8b951ec417
6 changed files with 78 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/systemlib/pid/pid.c
+38 -18
View File
@@ -40,18 +40,19 @@
*/
#include "pid.h"
#include <math.h>
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
uint8_t mode, uint8_t plot_i)
uint8_t mode)
{
pid->kp = kp;
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
pid->mode = mode;
pid->plot_i = plot_i;
pid->count = 0;
pid->saturated = 0;
pid->last_output = 0;
pid->sp = 0;
pid->error_previous = 0;
@@ -63,11 +64,7 @@ __EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float
pid->ki = ki;
pid->kd = kd;
pid->intmax = intmax;
// pid->mode = mode;
// pid->sp = 0;
// pid->error_previous = 0;
// pid->integral = 0;
// pid->limit = limit;
}
//void pid_set(PID_t *pid, float sp)
@@ -95,6 +92,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
goto start
*/
if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt))
{
return pid->last_output;
}
float i, d;
pid->sp = sp;
float error = pid->sp - val;
@@ -111,7 +113,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
}
// Anti-Windup. Needed if we don't use the saturation above.
if (pid->intmax != 0.0) {
if (pid->intmax != 0.0f) {
if (i > pid->intmax) {
pid->integral = pid->intmax;
@@ -122,14 +124,6 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
} else {
pid->integral = i;
}
//Send Controller integrals
// Disabled because of new possibilities with debug_vect.
// Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
// if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
// {
// mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
// }
}
if (pid->mode == PID_MODE_DERIVATIV_CALC) {
@@ -142,7 +136,33 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = 0;
}
pid->error_previous = error;
if (pid->kd == 0) {
d = 0;
}
return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (pid->ki == 0) {
i = 0;
}
if (pid->kp == 0) {
p = 0;
} else {
p = error;
}
if (isfinite(error)) {
pid->error_previous = error;
}
float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (isfinite(val)) {
last_output = val;
}
if (!isfinite(pid->integral)) {
pid->integral = 0;
}
return pid->last_output;
}
apps/systemlib/pid/pid.h
+3 -2
View File
@@ -58,13 +58,14 @@ typedef struct {
float sp;
float integral;
float error_previous;
float last_output;
float limit;
uint8_t mode;
uint8_t plot_i;
uint8_t count;
uint8_t saturated;
} PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode);
__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
//void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);