diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-23 01:20:41 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-23 01:20:41 +0200 |
commit | de530d6ba1fcbcaf65fc78ac8cca3286fa52d624 (patch) | |
tree | b7de387487d99a24fc2913fa50475f66d8c8ce66 | |
parent | 8b951ec417454353d61d19b3379e52b6da5dd6b6 (diff) | |
download | px4-firmware-de530d6ba1fcbcaf65fc78ac8cca3286fa52d624.tar.gz px4-firmware-de530d6ba1fcbcaf65fc78ac8cca3286fa52d624.tar.bz2 px4-firmware-de530d6ba1fcbcaf65fc78ac8cca3286fa52d624.zip |
General robustness improvements in PID struct, numerically close to bullet-proof, error reporting needs improvements still.
-rw-r--r-- | apps/mavlink/mavlink.c | 16 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.c | 53 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.h | 2 |
3 files changed, 48 insertions, 23 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 1f745baac..bf0ed346e 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1415,10 +1415,10 @@ void handleMessage(mavlink_message_t *msg) memset(&rc_hil, 0, sizeof(rc_hil)); static orb_advert_t rc_pub = 0; - rc_hil.chan[0].raw = 1510 + man.x * 500; - rc_hil.chan[1].raw = 1520 + man.y * 500; - rc_hil.chan[2].raw = 1590 + man.r * 500; - rc_hil.chan[3].raw = 1420 + man.z * 500; + rc_hil.chan[0].raw = 1510 + man.x / 2; + rc_hil.chan[1].raw = 1520 + man.y / 2; + rc_hil.chan[2].raw = 1590 + man.r / 2; + rc_hil.chan[3].raw = 1420 + man.z / 2; rc_hil.chan[0].scaled = man.x; rc_hil.chan[1].scaled = man.y; @@ -1428,10 +1428,10 @@ void handleMessage(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; - mc.roll = man.x*1000; - mc.pitch = man.y*1000; - mc.yaw = man.r*1000; - mc.roll = man.z*1000; + mc.roll = man.x / 1000.0f; + mc.pitch = man.y / 1000.0f; + mc.yaw = man.r / 1000.0f; + mc.roll = man.z / 1000.0f; /* fake RC channels with manual control input from simulator */ diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index fbf3edc33..61dd5757f 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -58,13 +58,36 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->error_previous = 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; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; + int ret = 0; + + if (isfinite(kp)) { + 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; + return ret; } //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; } else { - d = 0; + d = 0.0f; } - if (pid->kd == 0) { - d = 0; + if (pid->kd == 0.0f) { + d = 0.0f; } - if (pid->ki == 0) { + if (pid->ki == 0.0f) { i = 0; } - if (pid->kp == 0) { - p = 0; + float p; + + if (pid->kp == 0.0f) { + p = 0.0f; } else { 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; } - 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)) { - last_output = val; + if (isfinite(output)) { + pid->last_output = output; } if (!isfinite(pid->integral)) { diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h index d4bbcaf31..098b2bef8 100644 --- a/apps/systemlib/pid/pid.h +++ b/apps/systemlib/pid/pid.h @@ -66,7 +66,7 @@ typedef struct { } PID_t; __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); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); |