aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: cope with fwd thrust loss in Q approach

Browse Source 
if throttle is saturated and descending and low airspeed then declare
thrust loss
This commit is contained in:
Andrew Tridgell 2021-06-01 18:10:53 +10:00
parent fbf5083e6a
commit 0232fa7456
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/quadplane.cpp
View File

@ -2568,6 +2568,7 @@ void QuadPlane::poscontrol_init_approach(void)
poscontrol.start_closing_vel,
poscontrol.start_dist);
poscontrol.set_state(QPOS_APPROACH);
poscontrol.thrust_loss_start_ms = 0;
}
}
@ -2675,6 +2676,24 @@ void QuadPlane::vtol_position_controller(void)
vel_forward.last_ms = AP_HAL::millis();
}
bool throttle_saturated = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) >= plane.aparm.throttle_max;
if (throttle_saturated &&
motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED &&
plane.auto_state.sink_rate > 0.2 && aspeed < aspeed_threshold+4) {
const uint32_t now = AP_HAL::millis();
if (poscontrol.thrust_loss_start_ms == 0) {
poscontrol.thrust_loss_start_ms = now;
}
if (now - poscontrol.thrust_loss_start_ms > 5000) {
gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 thrust loss as=%.1f at=%.1f",
aspeed, aspeed_threshold);
poscontrol.set_state(QPOS_POSITION1);
}
} else {
poscontrol.thrust_loss_start_ms = 0;
}
// handle loss of forward thrust in approach
if (poscontrol.get_state() == QPOS_APPROACH && aspeed < aspeed_threshold &&
motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {

1
ArduPlane/quadplane.h
View File

@ -489,6 +489,7 @@ private:
bool pilot_correction_done;
float start_closing_vel;
float start_dist;
uint32_t thrust_loss_start_ms;
private:
uint32_t last_state_change_ms;
enum position_control_state state;