diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 83c1924e42..ada4a28ed1 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1092,6 +1092,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Bitmask: 10: Adjust mid-throttle to be TRIM_THROTTLE in non-auto throttle modes except MANUAL // @Bitmask: 11: Disable suppression of fixed wing rate gains in ground mode // @Bitmask: 12: Enable FBWB style loiter altitude control + // @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 89ed040993..48f6ca5f24 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -402,11 +402,13 @@ private: struct { uint32_t last_tkoff_arm_time; uint32_t last_check_ms; + uint32_t rudder_takeoff_warn_ms; uint32_t last_report_ms; bool launchTimerStarted; uint8_t accel_event_counter; uint32_t accel_event_ms; uint32_t start_time_ms; + bool waiting_for_rudder_neutral; } takeoff_state; // ground steering controller state @@ -1103,6 +1105,7 @@ private: void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in, SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const; void flaperon_update(); + void indicate_waiting_for_rud_neutral_to_takeoff(void); // is_flying.cpp void update_is_flying_5Hz(void); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index e880eef494..c0bfb174a7 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -8,6 +8,8 @@ #define MIN_AIRSPEED_MIN 5 // m/s, used for arming check and speed scaling +#define TAKEOFF_RUDDER_WARNING_TIMEOUT 3000 //ms that GCS warning about not returning arming rudder to neutral repeats + // failsafe // ---------------------- enum failsafe_state { @@ -161,7 +163,7 @@ enum FlightOptions { CENTER_THROTTLE_TRIM = (1<<10), DISABLE_GROUND_PID_SUPPRESSION = (1<<11), ENABLE_LOITER_ALT_CONTROL = (1<<12), - + INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13), }; enum CrowFlapOptions { diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 63559a50dd..c87e596285 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -32,6 +32,9 @@ bool Mode::enter() // cancel inverted flight plane.auto_state.inverted_flight = false; + + // cancel waiting for rudder neutral + plane.takeoff_state.waiting_for_rudder_neutral = false; // don't cross-track when starting a mission plane.auto_state.next_wp_crosstrack = false; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 440fe2b11c..9a550d0176 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2955,9 +2955,9 @@ void QuadPlane::takeoff_controller(void) // start motor spinning if not spinning already so user sees it is armed set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); takeoff_start_time_ms = now; - if (now - rudder_takeoff_warn_ms > 3000) { - gcs().send_text(MAV_SEVERITY_WARNING, "takeoff wait rudder release"); - rudder_takeoff_warn_ms = now; + if (now - plane.takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) { + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release"); + plane.takeoff_state.rudder_takeoff_warn_ms = now; } return; } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 6861acc956..932bb545c7 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -580,7 +580,6 @@ private: AP_Float maximum_takeoff_airspeed; uint32_t takeoff_start_time_ms; uint32_t takeoff_time_limit_ms; - uint32_t rudder_takeoff_warn_ms; float last_land_final_agl; diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 02fa2f79e8..8e48b0ea4c 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -139,6 +139,7 @@ void Plane::rudder_arm_disarm_check() arming.arm(AP_Arming::Method::RUDDER); rudder_arm_timer = 0; seen_neutral_rudder = false; + takeoff_state.rudder_takeoff_warn_ms = now; } } else { // not at full right rudder diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 11f50fabd8..b924febd23 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -968,6 +968,23 @@ void Plane::landing_neutral_control_surface_servos(void) } +/* + sets rudder/vtail , and elevon to indicator positions that we are in a rudder arming waiting for neutral stick state +*/ +void Plane::indicate_waiting_for_rud_neutral_to_takeoff(void) +{ + if (takeoff_state.waiting_for_rudder_neutral) { + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); + channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left); + if (!SRV_Channels::function_assigned(SRV_Channel::k_rudder) && !SRV_Channels::function_assigned(SRV_Channel::k_vtail_left)) { + // if no rudder indication possible, neutral elevons during wait becuase on takeoff stance they are usually both full up + SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, 0); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, 0); + } + } +} + + /* run configured output mixer. This takes calculated servo_out values for each channel and calculates PWM values, then pushes them to @@ -998,6 +1015,11 @@ void Plane::servos_output(void) // set control surface servos to neutral landing_neutral_control_surface_servos(); + + // set rudder arm waiting for neutral control throws (rudder neutral, aileron/rt vtail/rt elevon to full right) + if (g2.flight_options & FlightOptions::INDICATE_WAITING_FOR_RUDDER_NEUTRAL) { + indicate_waiting_for_rud_neutral_to_takeoff(); + } // support MANUAL_RCMASK if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) { diff --git a/ArduPlane/takeoff.cpp b/ArduPlane/takeoff.cpp index 31aeab0229..a298b870e1 100644 --- a/ArduPlane/takeoff.cpp +++ b/ArduPlane/takeoff.cpp @@ -29,6 +29,24 @@ bool Plane::auto_takeoff_check(void) } takeoff_state.last_check_ms = now; + + //check if waiting for rudder neutral after rudder arm + if (plane.arming.last_arm_method() == AP_Arming::Method::RUDDER && + !seen_neutral_rudder) { + // we were armed with rudder but have not seen rudder neutral yet + takeoff_state.waiting_for_rudder_neutral = true; + // warn if we have been waiting a long time + if (now - takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) { + gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release"); + takeoff_state.rudder_takeoff_warn_ms = now; + } + // since we are still waiting, dont takeoff + return false; + } else { + // we did not arm by rudder or rudder has returned to neutral + // make sure we dont indicate we are in the waiting state with servo position indicator + takeoff_state.waiting_for_rudder_neutral = false; + } // Check for bad GPS if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {