diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 781c784662..31cf56adef 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -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 diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 2b15a1daef..42d6854107 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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()) { diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index ee794d939b..b0406a7508 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -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;