diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-28 08:14:13 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-28 08:14:13 +0200 |
| commit | 5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe (patch) | |
| tree | 5f024477d370d92edacead7412f60f6757cbf082 /src/modules/multirotor_att_control/multirotor_attitude_control.c | |
| parent | e44d134c6c64535f67e26f9633206aba50a10613 (diff) | |
| parent | 66c61fbe96e11ee7099431a8370d84f862543810 (diff) | |
| download | px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.tar.gz px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.tar.bz2 px4-firmware-5fe3c49ba0e7e3f64f5b6c9b64ced675c01b14fe.zip | |
Merged multirotor branch
Diffstat (limited to 'src/modules/multirotor_att_control/multirotor_attitude_control.c')
| -rw-r--r-- | src/modules/multirotor_att_control/multirotor_attitude_control.c | 11 |
1 files changed, 7 insertions, 4 deletions
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4b..c78232f11 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -166,7 +166,7 @@ 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, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); @@ -246,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } rates_sp->thrust = att_sp->thrust; + //need to update the timestamp now that we've touched rates_sp + rates_sp->timestamp = hrt_absolute_time(); motor_skip_counter++; } |