aboutsummaryrefslogtreecommitdiff
path: root/apps/multirotor_att_control/multirotor_rate_control.c
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2013-01-11 07:45:24 +0100
committerLorenz Meier <lm@inf.ethz.ch>2013-01-11 07:45:24 +0100
commitcded2787f0cd0794a73cf58ea4ecd993c5e304d6 (patch)
treedc59963441075c1b0543098a93bbbe3f88bc8c11 /apps/multirotor_att_control/multirotor_rate_control.c
parentcf563eda8648534475705b6211bf4040ef9e193f (diff)
downloadpx4-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.c10
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;