From f725f7bb7e2b00a7539b652c44ccd123a23c71d2 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 7 Jan 2022 01:34:34 +0000 Subject: [PATCH] Plane: set new slew limit and inherit slew limt for flaps --- ArduPlane/Log.cpp | 2 +- ArduPlane/Plane.h | 2 +- ArduPlane/afs_plane.cpp | 4 ++++ ArduPlane/failsafe.cpp | 2 +- ArduPlane/radio.cpp | 4 ++-- ArduPlane/servos.cpp | 25 +++++++++++-------------- ArduPlane/tiltrotor.cpp | 2 +- 7 files changed, 21 insertions(+), 20 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index c2b9ec5370..2f07592790 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -249,7 +249,7 @@ void Plane::Log_Write_AETR() ,elevator : SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) ,throttle : SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) ,rudder : SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) - ,flap : SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) + ,flap : SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) ,speed_scaler : get_speed_scaler(), }; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 72b61d5831..a94cc19f70 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1092,7 +1092,7 @@ private: void update_throttle_hover(); 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(int8_t flap_percent); + void flaperon_update(); // is_flying.cpp void update_is_flying_5Hz(void); diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 019d867376..893d1208b3 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -24,6 +24,10 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) if (_terminate_action == TERMINATE_ACTION_LAND) { plane.landing.terminate(); } else { + // remove flap slew limiting + SRV_Channels::set_slew_rate(SRV_Channel::k_flap_auto, 0.0, 100, plane.G_Dt); + SRV_Channels::set_slew_rate(SRV_Channel::k_flap, 0.0, 100, plane.G_Dt); + // aerodynamic termination is the default approach to termination SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100.0); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100.0); diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index d121c6cc30..ac8ff68558 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -99,7 +99,7 @@ void Plane::failsafe_check(void) SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 0.0); // setup flaperons - flaperon_update(0); + flaperon_update(); servos_output(); diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 6207389acb..d15bc3afd9 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -348,8 +348,8 @@ void Plane::trim_radio() SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_dspoilerRight2); } - if (is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto)) && - is_zero(SRV_Channels::get_output_scaled(SRV_Channel::k_flap))) { + if (is_zero(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto)) && + is_zero(SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap))) { // trim flaperons if no flap input SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_left); SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::k_flaperon_right); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 5e895cdfd5..d8df5e4f2d 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -32,6 +32,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) if (!do_throttle_slew) { // only do throttle slew limiting in modes where throttle control is automatic + SRV_Channels::set_slew_rate(func, 0.0, 100, G_Dt); return; } @@ -54,10 +55,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func) slewrate = g.takeoff_throttle_slewrate; } #endif - // if slew limit rate is set to zero then do not slew limit - if (slewrate) { - SRV_Channels::limit_slew_rate(func, slewrate, G_Dt); - } + SRV_Channels::set_slew_rate(func, slewrate, 100, G_Dt); } /* We want to suppress the throttle if we think we are on the ground and in an autopilot controlled throttle mode. @@ -186,7 +184,7 @@ void Plane::channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, S /* setup flaperon output channels */ -void Plane::flaperon_update(int8_t flap_percent) +void Plane::flaperon_update() { /* flaperons are implemented as a mixer between aileron and a @@ -194,6 +192,7 @@ void Plane::flaperon_update(int8_t flap_percent) or from auto flaps. */ float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); + float flap_percent = SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto); float flaperon_left = constrain_float(aileron + flap_percent * 45, -4500, 4500); float flaperon_right = constrain_float(aileron - flap_percent * 45, -4500, 4500); SRV_Channels::set_output_scaled(SRV_Channel::k_flaperon_left, flaperon_left); @@ -275,11 +274,11 @@ void Plane::dspoiler_update(void) spoilers to both wings. Get flap percentage from k_flap_auto, which is set in set_servos_flaps() as the maximum of manual and auto flap control */ - const int16_t flap_percent = SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto); + const float flap_percent = SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto); - if (flap_percent > 0) { - float inner_flap_scaled = (float)flap_percent; - float outer_flap_scaled = (float)flap_percent; + if (is_positive(flap_percent)) { + float inner_flap_scaled = flap_percent; + float outer_flap_scaled = flap_percent; if (progressive_crow) { // apply 0 - full inner from 0 to 50% flap then add in outer above 50% inner_flap_scaled = constrain_float(inner_flap_scaled * 2, 0,100); @@ -671,13 +670,11 @@ void Plane::set_servos_flaps(void) SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, auto_flap_percent); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, manual_flap_percent); - if (g.flap_slewrate) { - SRV_Channels::limit_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, G_Dt); - SRV_Channels::limit_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, G_Dt); - } + SRV_Channels::set_slew_rate(SRV_Channel::k_flap_auto, g.flap_slewrate, 100, G_Dt); + SRV_Channels::set_slew_rate(SRV_Channel::k_flap, g.flap_slewrate, 100, G_Dt); // output to flaperons, if any - flaperon_update(auto_flap_percent); + flaperon_update(); } #if LANDING_GEAR_ENABLED == ENABLED diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp index a8132abb16..af44aad72a 100644 --- a/ArduPlane/tiltrotor.cpp +++ b/ArduPlane/tiltrotor.cpp @@ -202,7 +202,7 @@ float Tiltrotor::get_fully_forward_tilt() const // return the target tilt value for forward flight float Tiltrotor::get_forward_flight_tilt() const { - return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_output_scaled(SRV_Channel::k_flap_auto) * 0.01); + return 1.0 - ((flap_angle_deg / 90.0) * SRV_Channels::get_slew_limited_output_scaled(SRV_Channel::k_flap_auto) * 0.01); } /*