From fae6c084ac2ef38489e24d284072bc73d32aa43f Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Tue, 21 Mar 2017 15:40:59 -0400 Subject: [PATCH] Sub: Add camera slew rate parameter --- ArduSub/ArduSub.cpp | 10 ++++------ ArduSub/Parameters.cpp | 2 ++ ArduSub/Parameters.h | 3 +++ 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 2b9e9912a3..f3623a9d46 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -219,12 +219,10 @@ void Sub::fifty_hz_loop() // check pilot input failsafe failsafe_manual_control_check(); - if (hal.rcin->new_input()) { - // Update servo output - RC_Channels::set_pwm_all(); - SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, 20.0f, 0.02f); - SRV_Channels::output_ch_all(); - } + // Update servo output + RC_Channels::set_pwm_all(); + SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f); + SRV_Channels::output_ch_all(); } // update_mount - update camera mount position diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index d94ec9356a..624052c4bf 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -799,6 +799,8 @@ const AP_Param::Info Sub::var_info[] = { // @User: Standard GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0), + GSCALAR(cam_slew_limit, "CAM_SLEW_LIMIT", 45.0), + // @Group: // @Path: Parameters.cpp GOBJECT(g2, "", ParametersG2), diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index 03cd8389ed..3c0dd7c7fe 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -227,6 +227,8 @@ public: k_param_radio_tuning_high, // Disabled k_param_radio_tuning_low, // Disabled + k_param_cam_slew_limit, + }; AP_Int16 format_version; @@ -352,6 +354,7 @@ public: AP_Float surface_depth; AP_Int8 frame_configuration; + AP_Float cam_slew_limit; // Note: keep initializers here in the same order as they are declared // above. Parameters() :