From dc1e8c15c23ee03388311fc3035eeb85d5e0745c Mon Sep 17 00:00:00 2001 From: Mikel Graham Date: Wed, 23 Sep 2020 12:43:34 -0400 Subject: [PATCH 1/2] This allows user to set the surface positions for the aeroterminate functionality. --- ArduPlane/afs_plane.cpp | 6 ++--- .../AP_AdvancedFailsafe.cpp | 24 +++++++++++++++++++ .../AP_AdvancedFailsafe/AP_AdvancedFailsafe.h | 3 +++ 3 files changed, 30 insertions(+), 3 deletions(-) diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 04c12c2ab152d..3745e471eedfc 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -30,9 +30,9 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) // aerodynamic termination is the default approach to termination SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100); - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX * _terminate_pos_ail/100); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX * _terminate_pos_rud/100); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX * _terminate_pos_ele/100); if (plane.have_reverse_thrust()) { // configured for reverse thrust, use TRIM SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM); diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp index 9af4f5c478194..64972a3aac74e 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp @@ -158,6 +158,30 @@ const AP_Param::GroupInfo AP_AdvancedFailsafe::var_info[] = { // @Units: km AP_GROUPINFO("MAX_RANGE", 20, AP_AdvancedFailsafe, _max_range_km, 0), + // @Param: TERM_POS_AIL + // @DisplayName: Terminate Aileron Position + // @Description: This is the position the aileron will go to during an aerotermination. 0 means neutral deflection, default is 100. + // @User: Advanced + // @Units: % + // @Range: -100 100 + AP_GROUPINFO("TERM_POS_AIL", 21, AP_AdvancedFailsafe, _terminate_pos_ail, 100), + + // @Param: TERM_POS_RUD + // @DisplayName: Terminate Rudder Position + // @Description: This is the position the rudder will go to during an aerotermination. 0 means neutral deflection, default is 100. + // @User: Advanced + // @Units: % + // @Range: -100 100 + AP_GROUPINFO("TERM_POS_RUD", 22, AP_AdvancedFailsafe, _terminate_pos_rud, 100), + + // @Param: TERM_POS_ELE + // @DisplayName: Terminate Elevator Position + // @Description: This is the position the elevator will go to during an aerotermination. 0 means neutral deflection, default is 100. + // @User: Advanced + // @Units: % + // @Range: -100 100 + AP_GROUPINFO("TERM_POS_ELE", 23, AP_AdvancedFailsafe, _terminate_pos_ele, 100), + AP_GROUPEND }; diff --git a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h index 282c1e2bd4a5a..ea0e4d2c5ca39 100644 --- a/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h +++ b/libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.h @@ -131,6 +131,9 @@ class AP_AdvancedFailsafe AP_Int8 _rc_term_manual_only; AP_Int8 _enable_dual_loss; AP_Int16 _max_range_km; + AP_Int8 _terminate_pos_ail; + AP_Int8 _terminate_pos_rud; + AP_Int8 _terminate_pos_ele; bool _heartbeat_pin_value; From 32bac74c5dfc83ca1943774e29094b55b020edd3 Mon Sep 17 00:00:00 2001 From: Mikel Graham Date: Fri, 2 Oct 2020 14:24:55 -0400 Subject: [PATCH 2/2] Changed 100 to 100.0 to avoid truncation when value is less than 1. --- ArduPlane/afs_plane.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduPlane/afs_plane.cpp b/ArduPlane/afs_plane.cpp index 3745e471eedfc..6aa7e33f87706 100644 --- a/ArduPlane/afs_plane.cpp +++ b/ArduPlane/afs_plane.cpp @@ -30,9 +30,9 @@ void AP_AdvancedFailsafe_Plane::terminate_vehicle(void) // aerodynamic termination is the default approach to termination SRV_Channels::set_output_scaled(SRV_Channel::k_flap_auto, 100); SRV_Channels::set_output_scaled(SRV_Channel::k_flap, 100); - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX * _terminate_pos_ail/100); - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX * _terminate_pos_rud/100); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX * _terminate_pos_ele/100); + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, SERVO_MAX * _terminate_pos_ail/100.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, SERVO_MAX * _terminate_pos_rud/100.0); + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, SERVO_MAX * _terminate_pos_ele/100.0); if (plane.have_reverse_thrust()) { // configured for reverse thrust, use TRIM SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::TRIM);