Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Quadplane: SLT: enforce TECS pitch limits to beat race

This commit is contained in:
Iampete1 2022-10-05 01:21:23 +01:00 committed by Randy Mackay
parent 854061fdbe
commit 46fae47a06
2 changed files with 35 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduPlane/quadplane.cpp
View File

@ -1529,18 +1529,7 @@ void SLT_Transition::update()
transition_start_ms = 0; transition_start_ms = 0;
transition_low_airspeed_ms = 0; transition_low_airspeed_ms = 0;
} }
if (transition_state < TRANSITION_TIMER) {
// set a single loop pitch limit in TECS
if (plane.ahrs.groundspeed() < 3) {
// until we have some ground speed limit to zero pitch
plane.TECS_controller.set_pitch_max_limit(0);
} else {
plane.TECS_controller.set_pitch_max_limit(quadplane.transition_pitch_max);
}
} else if (transition_state < TRANSITION_DONE) {
plane.TECS_controller.set_pitch_max_limit((quadplane.transition_pitch_max+1)*2);
}
if (transition_state < TRANSITION_DONE) { if (transition_state < TRANSITION_DONE) {
// during transition we ask TECS to use a synthetic // during transition we ask TECS to use a synthetic
// airspeed. Otherwise the pitch limits will throw off the // airspeed. Otherwise the pitch limits will throw off the
@ -4236,6 +4225,38 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
return MAV_VTOL_STATE_UNDEFINED; return MAV_VTOL_STATE_UNDEFINED;
} }
// Set FW roll and pitch limits and keep TECS informed
void SLT_Transition::set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing)
{
if (quadplane.in_vtol_mode() || quadplane.in_vtol_airbrake()) {
// not in FW flight
return;
}
if (transition_state == TRANSITION_DONE) {
// transition complete, nothing to do
return;
}
float max_pitch;
if (transition_state < TRANSITION_TIMER) {
if (plane.ahrs.groundspeed() < 3.0) {
// until we have some ground speed limit to zero pitch
max_pitch = 0.0;
} else {
max_pitch = quadplane.transition_pitch_max;
}
} else {
max_pitch = (quadplane.transition_pitch_max+1.0)*2.0;
}
// set a single loop pitch limit in TECS
plane.TECS_controller.set_pitch_max_limit(max_pitch);
// ensure pitch is constrained to limit
nav_pitch_cd = constrain_int32(nav_pitch_cd, -max_pitch*100.0, max_pitch*100.0);
}
/* /*
see if we are in a VTOL takeoff see if we are in a VTOL takeoff
*/ */

2
ArduPlane/transition.h
View File

@ -87,6 +87,8 @@ public:
bool show_vtol_view() const override; bool show_vtol_view() const override;
void set_FW_roll_pitch(int32_t& nav_pitch_cd, int32_t& nav_roll_cd, bool& allow_stick_mixing) override;
bool set_FW_roll_limit(int32_t& roll_limit_cd) override; bool set_FW_roll_limit(int32_t& roll_limit_cd) override;
bool allow_update_throttle_mix() const override; bool allow_update_throttle_mix() const override;