mirror of https://github.com/ArduPilot/ardupilot
AC_WPNav: bug fix for loiter init in Hybrid
AC_PosControl::init_xy_controller() has been added to PosControl and is called by init_loiter_target. Hybrid is currently using set_loiter_target function to init the loiter controller. So we have to call init_xy_controller() by set_loiter_target function. What happens otherwise? In AC_PosControl::update_xy_controller, we update "now" with now = hal.scheduler->millis(); and, as _last_update_xy_ms has not been updated previously by init_xy_controller(), we just call init_xy_controller(). So, _dt_xy will be negative and used anyways in all the functions and PID called by update_xy_controller. That will avoid at least _accel_target.x/y to be set to 0 but I'm not sure for the high values, probably an I_term that is not reset and reached very high value. Or maybe a cast error somewhere... no clue at all
This commit is contained in:
parent
8c5226a17a
commit
aed5787c1b
|
@ -122,6 +122,9 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
|
|||
_pos_control.keep_xy_I_terms();
|
||||
}
|
||||
|
||||
// initialise position controller
|
||||
_pos_control.init_xy_controller();
|
||||
|
||||
// set target position
|
||||
_pos_control.set_pos_target(position);
|
||||
|
||||
|
@ -135,6 +138,10 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
|
|||
_loiter_accel_cms = _loiter_speed_cms/2.0f;
|
||||
_pos_control.set_accel_xy(_loiter_accel_cms);
|
||||
|
||||
// initialise desired accel and add fake wind
|
||||
_loiter_desired_accel.x = 0;
|
||||
_loiter_desired_accel.y = 0;
|
||||
|
||||
// initialise pilot input
|
||||
_pilot_accel_fwd_cms = 0;
|
||||
_pilot_accel_rgt_cms = 0;
|
||||
|
|
Loading…
Reference in New Issue