Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Add a failure timer for quadplane forward transitions

Browse Source 
Falls over to QLAND if we fail to transition
This commit is contained in:
Michael du Breuil 2018-12-14 19:40:43 -07:00 committed by Andrew Tridgell
parent b4c5041aad
commit d6cac4b52a
3 changed files with 46 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/defines.h
View File

@ -85,7 +85,8 @@ enum mode_reason_t {
MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING,
MODE_REASON_SOARING_THERMAL_DETECTED,
MODE_REASON_SOARING_IN_THERMAL,
MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED
MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED,
MODE_REASON_VTOL_FAILED_TRANSITION,
};
// type of stick mixing enabled

46
ArduPlane/quadplane.cpp
View File

@ -393,6 +393,15 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
// @User: Advanced
AP_GROUPINFO("FW_LND_APR_RAD", 7, QuadPlane, fw_land_approach_radius, 0),
// @Param: TRANS_FAIL
// @DisplayName: Quadplane transition failure time
// @Description: Maximum time allowed for forward transitions, exceeding this time will cancel the transition and the aircraft will immediately change to QLAND. 0 for no limit.
// @Units: s
// @Range: 0 20
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("TRANS_FAIL", 8, QuadPlane, transition_failure, 0),
AP_GROUPEND
};
@ -1241,10 +1250,25 @@ void QuadPlane::update_transition(void)
motors->output();
}
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
assisted_flight = false;
return;
}
const uint32_t now = millis();
if (!hal.util->get_soft_armed()) {
// reset the failure timer if we haven't started transitioning
transition_start_ms = now;
} else if ((transition_state != TRANSITION_DONE) &&
(transition_start_ms != 0) &&
(transition_failure > 0) &&
((now - transition_start_ms) > ((uint32_t)transition_failure * 1000))) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed, exceeded time limit");
plane.set_mode(QLAND, MODE_REASON_VTOL_FAILED_TRANSITION);
}
float aspeed;
bool have_airspeed = ahrs.airspeed_estimate(&aspeed);
@ -1268,7 +1292,9 @@ void QuadPlane::update_transition(void)
gcs().send_text(MAV_SEVERITY_INFO, "Transition started airspeed %.1f", (double)aspeed);
}
transition_state = TRANSITION_AIRSPEED_WAIT;
transition_start_ms = millis();
if (transition_start_ms == 0) {
transition_start_ms = now;
}
assisted_flight = true;
} else {
assisted_flight = false;
@ -1279,6 +1305,8 @@ void QuadPlane::update_transition(void)
tailsitter_transition_fw_complete()) {
gcs().send_text(MAV_SEVERITY_INFO, "Transition FW done");
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
}
}
@ -1287,6 +1315,8 @@ void QuadPlane::update_transition(void)
// the tilt will decrease rapidly)
if (tiltrotor_fully_fwd() && transition_state != TRANSITION_AIRSPEED_WAIT) {
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
}
if (transition_state < TRANSITION_TIMER) {
@ -1313,11 +1343,11 @@ void QuadPlane::update_transition(void)
// we hold in hover until the required airspeed is reached
if (transition_start_ms == 0) {
gcs().send_text(MAV_SEVERITY_INFO, "Transition airspeed wait");
transition_start_ms = millis();
transition_start_ms = now;
}
transition_low_airspeed_ms = now;
if (have_airspeed && aspeed > plane.aparm.airspeed_min && !assisted_flight) {
transition_start_ms = millis();
transition_state = TRANSITION_TIMER;
gcs().send_text(MAV_SEVERITY_INFO, "Transition airspeed reached %.1f", (double)aspeed);
}
@ -1355,12 +1385,15 @@ void QuadPlane::update_transition(void)
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// after airspeed is reached we degrade throttle over the
// transition time, but continue to stabilize
if (millis() - transition_start_ms > (unsigned)transition_time_ms) {
const uint32_t transition_timer_ms = now - transition_low_airspeed_ms;
if (transition_timer_ms > (unsigned)transition_time_ms) {
transition_state = TRANSITION_DONE;
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
gcs().send_text(MAV_SEVERITY_INFO, "Transition done");
}
float trans_time_ms = (float)transition_time_ms.get();
float transition_scale = (trans_time_ms - (millis() - transition_start_ms)) / trans_time_ms;
float transition_scale = (trans_time_ms - transition_timer_ms) / trans_time_ms;
float throttle_scaled = last_throttle * transition_scale;
// set zero throttle mix, to give full authority to
@ -1392,7 +1425,7 @@ void QuadPlane::update_transition(void)
// millisecond. Assume we want to get to the transition angle
// in half the transition time
float transition_rate = tailsitter.transition_angle / float(transition_time_ms/2);
uint32_t dt = AP_HAL::millis() - transition_start_ms;
uint32_t dt = now - transition_low_airspeed_ms;
plane.nav_pitch_cd = constrain_float((-transition_rate * dt)*100, -8500, 0);
plane.nav_roll_cd = 0;
check_attitude_relax();
@ -1492,6 +1525,7 @@ void QuadPlane::update(void)
setup the transition state appropriately for next time we go into a non-VTOL mode
*/
transition_start_ms = 0;
transition_low_airspeed_ms = 0;
if (throttle_wait && !plane.is_flying()) {
transition_state = TRANSITION_DONE;
} else if (is_tailsitter()) {

4
ArduPlane/quadplane.h
View File

@ -234,6 +234,9 @@ private:
// transition deceleration, m/s/s
AP_Float transition_decel;
// transition failure milliseconds
AP_Int16 transition_failure;
// Quadplane trim, degrees
AP_Float ahrs_trim_pitch;
@ -307,6 +310,7 @@ private:
// timer start for transition
uint32_t transition_start_ms;
uint32_t transition_low_airspeed_ms;
Location last_auto_target;