diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:45:24 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-01-11 07:45:24 +0100 |
commit | cded2787f0cd0794a73cf58ea4ecd993c5e304d6 (patch) | |
tree | dc59963441075c1b0543098a93bbbe3f88bc8c11 /apps/multirotor_att_control/multirotor_rate_control.c | |
parent | cf563eda8648534475705b6211bf4040ef9e193f (diff) | |
download | px4-firmware-cded2787f0cd0794a73cf58ea4ecd993c5e304d6.tar.gz px4-firmware-cded2787f0cd0794a73cf58ea4ecd993c5e304d6.tar.bz2 px4-firmware-cded2787f0cd0794a73cf58ea4ecd993c5e304d6.zip |
Fixed code style for multirotor_att_control app
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 10 |
1 files changed, 8 insertions, 2 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 93f7085ae..deba1ac03 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -148,7 +148,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators) { static float roll_control_last = 0; static float pitch_control_last = 0; @@ -157,6 +157,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static uint64_t last_input = 0; float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; + if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; } @@ -186,12 +187,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, } /* calculate current control outputs */ - + /* control pitch (forward) output */ float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; + } else { pitch_control = 0.0f; warnx("rej. NaN ctrl pitch"); @@ -199,9 +202,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control roll (left/right) output */ float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); + /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; + } else { roll_control = 0.0f; warnx("rej. NaN ctrl roll"); @@ -209,6 +214,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control yaw rate */ float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); + /* increase resilience to faulty control inputs */ if (!isfinite(yaw_rate_control)) { yaw_rate_control = 0.0f; |