From b61ee34b6bfbe71bcff8dfa32d42b64eb94f6b17 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Fri, 2 Oct 2020 19:52:56 -0500 Subject: [PATCH] ArduPlane: add RC option for landing flare --- ArduPlane/Attitude.cpp | 6 ++++++ ArduPlane/Plane.h | 3 +++ ArduPlane/RC_Channel.cpp | 22 ++++++++++++++++++++++ ArduPlane/RC_Channel.h | 3 +++ ArduPlane/servos.cpp | 34 +++++++++++++++++++++++++++++++--- 5 files changed, 65 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 2833488db9..2c4b6f529c 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -123,6 +123,12 @@ void Plane::stabilize_pitch(float speed_scaler) if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) { disable_integrator = true; } + + // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD + if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && channel_throttle->in_trim_dz() && !auto_throttle_mode && flare_switch_active) { + demanded_pitch = landing.get_pitch_cd(); + } + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator)); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 4df6005759..d07734e1cf 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1050,6 +1050,7 @@ private: void servos_output(void); void servos_auto_trim(void); void servos_twin_engine_mix(); + void force_flare(); void throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle); void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func); @@ -1128,6 +1129,8 @@ private: CrowMode crow_mode = CrowMode::NORMAL; + bool flare_switch_active; + public: void failsafe_check(void); bool set_target_location(const Location& target_loc) override; diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp index 41fa56e696..6bd9cac49f 100644 --- a/ArduPlane/RC_Channel.cpp +++ b/ArduPlane/RC_Channel.cpp @@ -116,6 +116,23 @@ void RC_Channel_Plane::do_aux_function_soaring_3pos(AuxSwitchPos ch_flag) plane.g2.soaring_controller.set_pilot_desired_state(desired_state); #endif } + +void RC_Channel_Plane::do_aux_function_flare(AuxSwitchPos ch_flag) +{ + switch(ch_flag) { + case AuxSwitchPos::HIGH: + plane.flare_switch_active = true; + plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_DISABLED); + break; + case AuxSwitchPos::MIDDLE: + break; + case AuxSwitchPos::LOW: + plane.quadplane.set_q_assist_state(plane.quadplane.Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED); + plane.flare_switch_active = false; + break; + } +} + void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, const RC_Channel::AuxSwitchPos ch_flag) { @@ -131,6 +148,7 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::aux_func_t ch_option, case AUX_FUNC::RTL: case AUX_FUNC::TAKEOFF: case AUX_FUNC::FWD_THR: + case AUX_FUNC::LANDING_FLARE: break; case AUX_FUNC::Q_ASSIST: @@ -257,6 +275,10 @@ void RC_Channel_Plane::do_aux_function(const aux_func_t ch_option, const AuxSwit } break; + case AUX_FUNC::LANDING_FLARE: + do_aux_function_flare(ch_flag); + break; + default: RC_Channel::do_aux_function(ch_option, ch_flag); break; diff --git a/ArduPlane/RC_Channel.h b/ArduPlane/RC_Channel.h index 6bba22252f..fd45da29e2 100644 --- a/ArduPlane/RC_Channel.h +++ b/ArduPlane/RC_Channel.h @@ -23,6 +23,9 @@ private: void do_aux_function_crow_mode(AuxSwitchPos ch_flag); void do_aux_function_soaring_3pos(AuxSwitchPos ch_flag); + + void do_aux_function_flare(AuxSwitchPos ch_flag); + }; class RC_Channels_Plane : public RC_Channels diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index c2b6862f12..3c101d24ee 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -727,9 +727,35 @@ void Plane::servos_twin_engine_mix(void) } } - /* - Set the flight control servos based on the current calculated values + Set throttle,attitude(in Attitude.cpp), and tilt servos for forced flare by RCx_OPTION switch for landing in FW mode + For Fixed Wind modes with manual throttle control only. Forces tilts up and throttle to THR_MIN. + Throttle stick must be in idle deadzone. This allows non-momentary switch to be used and quick bailouts + for go-arounds. Also helps prevent propstrike after landing with switch release on ground. +*/ +void Plane::force_flare(void) +{ + if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && !auto_throttle_mode && channel_throttle->in_trim_dz() && flare_switch_active) { + int32_t tilt = -SERVO_MAX; //this is tilts up for a normal tiltrotor + if (quadplane.tilt.tilt_type == QuadPlane::TILT_TYPE_BICOPTER) { + tilt = 0; // this is tilts up for a Bicopter + } + if (quadplane.is_tailsitter()) { + tilt = SERVO_MAX; //this is tilts up for a tailsitter + } + SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, tilt); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt); + SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt); + float throttle_min = MAX(aparm.throttle_min.get(),0); //allows ICE to run if used but accounts for reverse thrust setups + if (arming.is_armed()) { //prevent running motors if unarmed + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle_min); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_min); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_min); + } + } +} + +/* Set the flight control servos based on the current calculated values This function operates by first building up output values for channels using set_servo() and set_radio_out(). Using @@ -885,7 +911,6 @@ void Plane::set_servos(void) servos_output(); } - /* run configured output mixer. This takes calculated servo_out values for each channel and calculates PWM values, then pushes them to @@ -902,6 +927,9 @@ void Plane::servos_output(void) quadplane.tailsitter_output(); quadplane.tiltrotor_bicopter(); + // support forced flare option + force_flare(); + // run vtail and elevon mixers servo_output_mixers();