diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 04c211365d..e75bd4b96f 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -693,6 +693,8 @@ void QuadPlane::init_stabilize(void) */ void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds) { + check_attitude_relax(); + if (in_vtol_mode() || is_tailsitter()) { // use euler angle attitude control attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, @@ -769,6 +771,22 @@ void QuadPlane::run_z_controller(void) pos_control->update_z_controller(); } +/* + check if we should relax the attitude controllers + + We relax them whenever we will be using them after a period of + inactivity + */ +void QuadPlane::check_attitude_relax(void) +{ + uint32_t now = AP_HAL::millis(); + if (now - last_att_control_ms > 500) { + attitude_control->relax_attitude_controllers(); + attitude_control->reset_rate_controller_I_terms(); + } + last_att_control_ms = now; +} + // init quadplane hover mode void QuadPlane::init_hover(void) { @@ -946,6 +964,7 @@ void QuadPlane::control_loiter() return; } + check_attitude_relax(); if (should_relax()) { loiter_nav->soften_for_landing(); @@ -1342,7 +1361,8 @@ void QuadPlane::update_transition(void) uint32_t dt = AP_HAL::millis() - transition_start_ms; plane.nav_pitch_cd = constrain_float((-transition_rate * dt)*100, -8500, 0); plane.nav_roll_cd = 0; - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, + check_attitude_relax(); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd, plane.nav_pitch_cd, 0); attitude_control->set_throttle_out(motors->get_throttle_hover(), true, 0); @@ -1746,6 +1766,8 @@ void QuadPlane::vtol_position_controller(void) float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + check_attitude_relax(); + // horizontal position control switch (poscontrol.state) { @@ -1971,6 +1993,8 @@ void QuadPlane::takeoff_controller(void) float ekfGndSpdLimit, ekfNavVelGainScaler; ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + check_attitude_relax(); + setup_target_position(); // set position controller desired velocity and acceleration to zero @@ -2000,6 +2024,8 @@ void QuadPlane::waypoint_controller(void) { setup_target_position(); + check_attitude_relax(); + /* this is full copter control of auto flight */ diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 90535400b3..9bdc5df710 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -183,6 +183,7 @@ private: void init_stabilize(void); void control_stabilize(void); + void check_attitude_relax(void); void init_hover(void); void control_hover(void); void run_rate_controller(void); @@ -302,6 +303,9 @@ private: // pitch when we enter loiter mode int32_t loiter_initial_pitch_cd; + // when did we last run the attitude controller? + uint32_t last_att_control_ms; + // true if we have reached the airspeed threshold for transition enum { TRANSITION_AIRSPEED_WAIT,