mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-11 02:13:57 -04:00
Plane: zero integrators and set throttle mix
this zeros fixed wing integrators during TRANSITION_AIRSPEED_WAIT and uses a zero throttle mix during TRANSITION_TIMER. This should allow the fixed wing controller more time to adapt to forward flight
This commit is contained in:
parent
e5ed20d6f7
commit
4ff5bf5af5
@ -1118,6 +1118,15 @@ void QuadPlane::update_transition(void)
|
|||||||
run_rate_controller();
|
run_rate_controller();
|
||||||
motors_output();
|
motors_output();
|
||||||
last_throttle = motors->get_throttle();
|
last_throttle = motors->get_throttle();
|
||||||
|
|
||||||
|
// reset integrators while we are below target airspeed as we
|
||||||
|
// may build up too much while still primarily under
|
||||||
|
// multicopter control
|
||||||
|
plane.pitchController.reset_I();
|
||||||
|
plane.rollController.reset_I();
|
||||||
|
|
||||||
|
// give full authority to attitude control
|
||||||
|
attitude_control->set_throttle_mix_max();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1130,7 +1139,14 @@ void QuadPlane::update_transition(void)
|
|||||||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done");
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Transition done");
|
||||||
}
|
}
|
||||||
float trans_time_ms = (float)transition_time_ms.get();
|
float trans_time_ms = (float)transition_time_ms.get();
|
||||||
float throttle_scaled = last_throttle * (trans_time_ms - (millis() - transition_start_ms)) / trans_time_ms;
|
float transition_scale = (trans_time_ms - (millis() - transition_start_ms)) / trans_time_ms;
|
||||||
|
float throttle_scaled = last_throttle * transition_scale;
|
||||||
|
|
||||||
|
// set zero throttle mix, to give full authority to
|
||||||
|
// throttle. This ensures that the fixed wing controllers get
|
||||||
|
// a chance to learn the right integrators during the transition
|
||||||
|
attitude_control->set_throttle_mix_value(0.5*transition_scale);
|
||||||
|
|
||||||
if (throttle_scaled < 0.01) {
|
if (throttle_scaled < 0.01) {
|
||||||
// ensure we don't drop all the way to zero or the motors
|
// ensure we don't drop all the way to zero or the motors
|
||||||
// will stop stabilizing
|
// will stop stabilizing
|
||||||
@ -1170,7 +1186,6 @@ void QuadPlane::update_transition(void)
|
|||||||
*/
|
*/
|
||||||
void QuadPlane::run_rate_controller(void)
|
void QuadPlane::run_rate_controller(void)
|
||||||
{
|
{
|
||||||
attitude_control->set_throttle_mix_max();
|
|
||||||
attitude_control->rate_controller_run();
|
attitude_control->rate_controller_run();
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1193,12 +1208,15 @@ void QuadPlane::update(void)
|
|||||||
motor_test_output();
|
motor_test_output();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!in_vtol_mode()) {
|
if (!in_vtol_mode()) {
|
||||||
update_transition();
|
update_transition();
|
||||||
} else {
|
} else {
|
||||||
assisted_flight = false;
|
assisted_flight = false;
|
||||||
|
|
||||||
|
// give full authority to attitude control
|
||||||
|
attitude_control->set_throttle_mix_max();
|
||||||
|
|
||||||
// run low level rate controllers
|
// run low level rate controllers
|
||||||
run_rate_controller();
|
run_rate_controller();
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user