mirror of https://github.com/ArduPilot/ardupilot
Plane:Takeoff on rudder arm only after return to neutral
This commit is contained in:
parent
eb6c5aa7d3
commit
1415418d6e
|
@ -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: 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: 11: Disable suppression of fixed wing rate gains in ground mode
|
||||||
// @Bitmask: 12: Enable FBWB style loiter altitude control
|
// @Bitmask: 12: Enable FBWB style loiter altitude control
|
||||||
|
// @Bitmask: 13: Indicate takeoff waiting for neutral rudder with flight control surfaces
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
|
AP_GROUPINFO("FLIGHT_OPTIONS", 13, ParametersG2, flight_options, 0),
|
||||||
|
|
||||||
|
|
|
@ -402,11 +402,13 @@ private:
|
||||||
struct {
|
struct {
|
||||||
uint32_t last_tkoff_arm_time;
|
uint32_t last_tkoff_arm_time;
|
||||||
uint32_t last_check_ms;
|
uint32_t last_check_ms;
|
||||||
|
uint32_t rudder_takeoff_warn_ms;
|
||||||
uint32_t last_report_ms;
|
uint32_t last_report_ms;
|
||||||
bool launchTimerStarted;
|
bool launchTimerStarted;
|
||||||
uint8_t accel_event_counter;
|
uint8_t accel_event_counter;
|
||||||
uint32_t accel_event_ms;
|
uint32_t accel_event_ms;
|
||||||
uint32_t start_time_ms;
|
uint32_t start_time_ms;
|
||||||
|
bool waiting_for_rudder_neutral;
|
||||||
} takeoff_state;
|
} takeoff_state;
|
||||||
|
|
||||||
// ground steering controller 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,
|
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;
|
SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const;
|
||||||
void flaperon_update();
|
void flaperon_update();
|
||||||
|
void indicate_waiting_for_rud_neutral_to_takeoff(void);
|
||||||
|
|
||||||
// is_flying.cpp
|
// is_flying.cpp
|
||||||
void update_is_flying_5Hz(void);
|
void update_is_flying_5Hz(void);
|
||||||
|
|
|
@ -8,6 +8,8 @@
|
||||||
|
|
||||||
#define MIN_AIRSPEED_MIN 5 // m/s, used for arming check and speed scaling
|
#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
|
// failsafe
|
||||||
// ----------------------
|
// ----------------------
|
||||||
enum failsafe_state {
|
enum failsafe_state {
|
||||||
|
@ -161,7 +163,7 @@ enum FlightOptions {
|
||||||
CENTER_THROTTLE_TRIM = (1<<10),
|
CENTER_THROTTLE_TRIM = (1<<10),
|
||||||
DISABLE_GROUND_PID_SUPPRESSION = (1<<11),
|
DISABLE_GROUND_PID_SUPPRESSION = (1<<11),
|
||||||
ENABLE_LOITER_ALT_CONTROL = (1<<12),
|
ENABLE_LOITER_ALT_CONTROL = (1<<12),
|
||||||
|
INDICATE_WAITING_FOR_RUDDER_NEUTRAL = (1<<13),
|
||||||
};
|
};
|
||||||
|
|
||||||
enum CrowFlapOptions {
|
enum CrowFlapOptions {
|
||||||
|
|
|
@ -33,6 +33,9 @@ bool Mode::enter()
|
||||||
// cancel inverted flight
|
// cancel inverted flight
|
||||||
plane.auto_state.inverted_flight = false;
|
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
|
// don't cross-track when starting a mission
|
||||||
plane.auto_state.next_wp_crosstrack = false;
|
plane.auto_state.next_wp_crosstrack = false;
|
||||||
|
|
||||||
|
|
|
@ -2955,9 +2955,9 @@ void QuadPlane::takeoff_controller(void)
|
||||||
// start motor spinning if not spinning already so user sees it is armed
|
// start motor spinning if not spinning already so user sees it is armed
|
||||||
set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
||||||
takeoff_start_time_ms = now;
|
takeoff_start_time_ms = now;
|
||||||
if (now - rudder_takeoff_warn_ms > 3000) {
|
if (now - plane.takeoff_state.rudder_takeoff_warn_ms > TAKEOFF_RUDDER_WARNING_TIMEOUT) {
|
||||||
gcs().send_text(MAV_SEVERITY_WARNING, "takeoff wait rudder release");
|
gcs().send_text(MAV_SEVERITY_WARNING, "Takeoff waiting for rudder release");
|
||||||
rudder_takeoff_warn_ms = now;
|
plane.takeoff_state.rudder_takeoff_warn_ms = now;
|
||||||
}
|
}
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -580,7 +580,6 @@ private:
|
||||||
AP_Float maximum_takeoff_airspeed;
|
AP_Float maximum_takeoff_airspeed;
|
||||||
uint32_t takeoff_start_time_ms;
|
uint32_t takeoff_start_time_ms;
|
||||||
uint32_t takeoff_time_limit_ms;
|
uint32_t takeoff_time_limit_ms;
|
||||||
uint32_t rudder_takeoff_warn_ms;
|
|
||||||
|
|
||||||
float last_land_final_agl;
|
float last_land_final_agl;
|
||||||
|
|
||||||
|
|
|
@ -139,6 +139,7 @@ void Plane::rudder_arm_disarm_check()
|
||||||
arming.arm(AP_Arming::Method::RUDDER);
|
arming.arm(AP_Arming::Method::RUDDER);
|
||||||
rudder_arm_timer = 0;
|
rudder_arm_timer = 0;
|
||||||
seen_neutral_rudder = false;
|
seen_neutral_rudder = false;
|
||||||
|
takeoff_state.rudder_takeoff_warn_ms = now;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// not at full right rudder
|
// not at full right rudder
|
||||||
|
|
|
@ -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
|
run configured output mixer. This takes calculated servo_out values
|
||||||
for each channel and calculates PWM values, then pushes them to
|
for each channel and calculates PWM values, then pushes them to
|
||||||
|
@ -999,6 +1016,11 @@ void Plane::servos_output(void)
|
||||||
// set control surface servos to neutral
|
// set control surface servos to neutral
|
||||||
landing_neutral_control_surface_servos();
|
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
|
// support MANUAL_RCMASK
|
||||||
if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
|
if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
|
||||||
SRV_Channels::copy_radio_in_out_mask(uint32_t(g2.manual_rc_mask.get()));
|
SRV_Channels::copy_radio_in_out_mask(uint32_t(g2.manual_rc_mask.get()));
|
||||||
|
|
|
@ -30,6 +30,24 @@ bool Plane::auto_takeoff_check(void)
|
||||||
|
|
||||||
takeoff_state.last_check_ms = now;
|
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
|
// Check for bad GPS
|
||||||
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
|
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
|
||||||
// no auto takeoff without GPS lock
|
// no auto takeoff without GPS lock
|
||||||
|
|
Loading…
Reference in New Issue