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_attitude_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_attitude_control.c')
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 18 |
1 files changed, 11 insertions, 7 deletions
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index b98736cee..76dbb36d3 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -158,16 +158,18 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) { static uint64_t last_run = 0; static uint64_t last_input = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; last_run = hrt_absolute_time(); + if (last_input != att_sp->timestamp) { last_input = att_sp->timestamp; } + static int sensor_delay; sensor_delay = hrt_absolute_time() - att->timestamp; @@ -189,9 +191,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + 1000.0f, PID_MODE_DERIVATIV_SET); pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -207,7 +209,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } /* reset integral if on ground */ - if(att_sp->thrust < 0.1f) { + if (att_sp->thrust < 0.1f) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); } @@ -217,13 +219,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT); - if(control_yaw_position) { + if (control_yaw_position) { /* control yaw rate */ /* positive error: rotate to right, negative error, rotate to left (NED frame) */ @@ -233,12 +235,14 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (yaw_error > M_PI_F) { yaw_error -= M_TWOPI_F; + } else if (yaw_error < -M_PI_F) { yaw_error += M_TWOPI_F; } rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); } + rates_sp->thrust = att_sp->thrust; motor_skip_counter++; |