5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-06 16:08:28 -04:00

Plane: fixed relaxing of attitude controller on transition

this fixes an issue found by Leonard where the attitude controller
could have residual control left over from a previous transition when
engaging the VTOL attitude controller
This commit is contained in:
Andrew Tridgell 2018-09-14 10:02:18 +10:00
parent 042265b4c4
commit 4e148959c9
2 changed files with 31 additions and 1 deletions

View File

@ -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
*/

View File

@ -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,