diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-24 02:32:29 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-24 02:32:29 -0700 |
commit | ac7caf1e2796cced2ea5a1d5344a6992be843cb6 (patch) | |
tree | 9f4e74a8e37a8b31f0c845a5c251c180e99f4ea1 | |
parent | 0761cad3c113f1dd5ffbe4d2382e8d308c626a9a (diff) | |
parent | 3b509aaee4ac910db7b49815eef1f78261b85782 (diff) | |
download | px4-firmware-ac7caf1e2796cced2ea5a1d5344a6992be843cb6.tar.gz px4-firmware-ac7caf1e2796cced2ea5a1d5344a6992be843cb6.tar.bz2 px4-firmware-ac7caf1e2796cced2ea5a1d5344a6992be843cb6.zip |
Merge pull request #1001 from PX4/pwm_limits
Pwm limits
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 9 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 1 | ||||
-rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.c | 14 |
3 files changed, 19 insertions, 5 deletions
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index fd69cf795..8a4bfa18c 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -648,11 +648,9 @@ PX4FMU::task_main() /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + /* last resort: catch NaN and INF */ + if ((i >= outputs.noutputs) || + !isfinite(outputs.output[i])) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -664,6 +662,7 @@ PX4FMU::task_main() uint16_t pwm_limited[num_outputs]; + /* the PWM limit call takes care of out of band errors and constrains */ pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index ebf4f3e8e..2f721bf1e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -213,6 +213,7 @@ mixer_tick(void) mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); in_mixer = false; + /* the pwm limit call takes care of out of band errors */ pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 190b315f1..ea5ba9e52 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -136,12 +136,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + + /* last line of defense against invalid inputs */ + if (effective_pwm[i] < ramp_min_pwm) { + effective_pwm[i] = ramp_min_pwm; + } else if (effective_pwm[i] > max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } } } break; case PWM_LIMIT_STATE_ON: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; + + /* last line of defense against invalid inputs */ + if (effective_pwm[i] < min_pwm[i]) { + effective_pwm[i] = min_pwm[i]; + } else if (effective_pwm[i] > max_pwm[i]) { + effective_pwm[i] = max_pwm[i]; + } } break; default: |