diff options
Diffstat (limited to 'apps/multirotor_att_control/multirotor_rate_control.c')
| -rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 9 |
1 files changed, 7 insertions, 2 deletions
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 372b378d1..7532dffa2 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -144,6 +144,8 @@ 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) { + static float roll_control_last=0; + static float pitch_control_last=0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -172,10 +174,13 @@ 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 * deltaT * (rate_sp->pitch-rates[1]); + + float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last); + pitch_control_last=pitch_control; /* control roll (left/right) output */ - float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0] ); + float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last); + roll_control_last=roll_control; /* control yaw rate */ float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] ); |