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:
parent
042265b4c4
commit
4e148959c9
ArduPlane
@ -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
|
||||
*/
|
||||
|
@ -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,
|
||||
|
Loading…
Reference in New Issue
Block a user