diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index c34c8611aa..765d022efe 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -760,8 +760,7 @@ void Plane::calc_nav_roll() float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f; - g2.guidedHeading.update_error(error); // push error into AC_PID , possible improvement is to use update_all instead.? - g2.guidedHeading.set_dt(delta); + g2.guidedHeading.update_error(error, delta); // push error into AC_PID , possible improvement is to use update_all instead.? float i = g2.guidedHeading.get_i(); // get integrator TODO if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) { diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3cda278dfa..808a145621 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -607,7 +607,6 @@ bool QuadPlane::setup(void) if (!enable || hal.util->get_soft_armed()) { return false; } - float loop_delta_t = 1.0 / plane.scheduler.get_loop_rate_hz(); /* cope with upgrade from old AP_Motors values for frame_class @@ -716,13 +715,13 @@ bool QuadPlane::setup(void) AP_BoardConfig::allocation_error("ahrs_view"); } - attitude_control = new AC_AttitudeControl_TS(*ahrs_view, aparm, *motors, loop_delta_t); + attitude_control = new AC_AttitudeControl_TS(*ahrs_view, aparm, *motors); if (!attitude_control) { AP_BoardConfig::allocation_error("attitude_control"); } AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); - pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, loop_delta_t); + pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); if (!pos_control) { AP_BoardConfig::allocation_error("pos_control"); } @@ -1986,6 +1985,9 @@ void QuadPlane::motors_output(bool run_rate_controller) // relax if have been inactive relax_attitude_control(); } + // run low level rate controllers that only require IMU data and set loop time + attitude_control->set_dt(AP::scheduler().get_last_loop_time_s()); + pos_control->set_dt(AP::scheduler().get_last_loop_time_s()); attitude_control->rate_controller_run(); last_att_control_ms = now; }