Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

General robustness improvements in PID struct, numerically close to bullet-proof, error reporting needs improvements still.

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-23 01:20:41 +02:00
parent 8b951ec417
commit de530d6ba1
3 changed files with 48 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/mavlink/mavlink.c
+8 -8
View File
@@ -1415,10 +1415,10 @@ void handleMessage(mavlink_message_t *msg)
memset(&rc_hil, 0, sizeof(rc_hil)); memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0; static orb_advert_t rc_pub = 0;
rc_hil.chan[0].raw = 1510 + man.x * 500; rc_hil.chan[0].raw = 1510 + man.x / 2;
rc_hil.chan[1].raw = 1520 + man.y * 500; rc_hil.chan[1].raw = 1520 + man.y / 2;
rc_hil.chan[2].raw = 1590 + man.r * 500; rc_hil.chan[2].raw = 1590 + man.r / 2;
rc_hil.chan[3].raw = 1420 + man.z * 500; rc_hil.chan[3].raw = 1420 + man.z / 2;
rc_hil.chan[0].scaled = man.x; rc_hil.chan[0].scaled = man.x;
rc_hil.chan[1].scaled = man.y; rc_hil.chan[1].scaled = man.y;
@@ -1428,10 +1428,10 @@ void handleMessage(mavlink_message_t *msg)
struct manual_control_setpoint_s mc; struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0; static orb_advert_t mc_pub = 0;
mc.roll = man.x*1000; mc.roll = man.x / 1000.0f;
mc.pitch = man.y*1000; mc.pitch = man.y / 1000.0f;
mc.yaw = man.r*1000; mc.yaw = man.r / 1000.0f;
mc.roll = man.z*1000; mc.roll = man.z / 1000.0f;
/* fake RC channels with manual control input from simulator */ /* fake RC channels with manual control input from simulator */
apps/systemlib/pid/pid.c
+39 -14
View File
@@ -58,13 +58,36 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
pid->error_previous = 0; pid->error_previous = 0;
pid->integral = 0; pid->integral = 0;
} }
__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax) __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
{ {
pid->kp = kp; int ret = 0;
pid->ki = ki;
pid->kd = kd; if (isfinite(kp)) {
pid->intmax = intmax; pid->kp = kp;
} else {
ret = 1;
}
if (isfinite(ki)) {
pid->ki = ki;
} else {
ret = 1;
}
if (isfinite(kd)) {
pid->kd = kd;
} else {
ret = 1;
}
if (isfinite(intmax)) {
pid->intmax = intmax;
} else {
ret = 1;
}
// pid->limit = limit; // pid->limit = limit;
return ret;
} }
//void pid_set(PID_t *pid, float sp) //void pid_set(PID_t *pid, float sp)
@@ -133,19 +156,21 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
d = -val_dot; d = -val_dot;
} else { } else {
d = 0; d = 0.0f;
} }
if (pid->kd == 0) { if (pid->kd == 0.0f) {
d = 0; d = 0.0f;
} }
if (pid->ki == 0) { if (pid->ki == 0.0f) {
i = 0; i = 0;
} }
if (pid->kp == 0) { float p;
p = 0;
if (pid->kp == 0.0f) {
p = 0.0f;
} else { } else {
p = error; p = error;
} }
@@ -154,10 +179,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
pid->error_previous = error; pid->error_previous = error;
} }
float val = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
if (isfinite(val)) { if (isfinite(output)) {
last_output = val; pid->last_output = output;
} }
if (!isfinite(pid->integral)) { if (!isfinite(pid->integral)) {
apps/systemlib/pid/pid.h
+1 -1
View File
@@ -66,7 +66,7 @@ typedef struct {
} PID_t; } PID_t;
__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode); __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); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
//void pid_set(PID_t *pid, float sp); //void pid_set(PID_t *pid, float sp);
__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);