Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: set new slew limit and inherit slew limt for flaps

This commit is contained in:
Iampete1 2022-01-07 01:34:34 +00:00 committed by Andrew Tridgell
parent f25c1ef954
commit f725f7bb7e
7 changed files with 21 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/Log.cpp
View File

@ -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(),
};

2
ArduPlane/Plane.h
View File

@ -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);

4
ArduPlane/afs_plane.cpp
View File

@ -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);

2
ArduPlane/failsafe.cpp
View File

@ -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();

4
ArduPlane/radio.cpp
View File

@ -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);

25
ArduPlane/servos.cpp
View File

@ -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

2
ArduPlane/tiltrotor.cpp
View File

@ -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);
}
/*