Sub: rework camera tilt input and slew limiting

ArduSub/ArduSub.cpp

@@ -209,12 +209,8 @@ void Sub::fifty_hz_loop()
 
     failsafe_sensors_check();
 
-    // Update servo output
+    // Update rc input/output
     RC_Channels::set_pwm_all();
-    // wait for outputs to initialize: TODO find a better way to do this
-    if (millis() > 10000) {
-        SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f);
-    }
     SRV_Channels::output_ch_all();
 }
 

ArduSub/Parameters.cpp

@@ -302,14 +302,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @Range: 1 10
     GSCALAR(numGainSettings, "JS_GAIN_STEPS", 4),
 
-    // @Param: JS_CAM_TILT_STEP
-    // @DisplayName: Camera tilt step size
-    // @Description: Size of PWM increment in microseconds on camera tilt servo
-    // @User: Standard
-    // @Range: 30 400
-    // @Units: PWM
-    GSCALAR(cam_tilt_step, "JS_CAM_TILT_STEP", 50),
-
     // @Param: JS_LIGHTS_STEP
     // @DisplayName: Lights step size
     // @Description: Size of PWM increment in microseconds on lights servo
@@ -325,14 +317,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @Range: 0.5 4.0
     GSCALAR(throttle_gain, "JS_THR_GAIN", 1.0f),
 
-    // @Param: CAM_CENTER
-    // @DisplayName: Camera tilt mount center
-    // @Description: Servo PWM in microseconds at camera center position
-    // @User: Standard
-    // @Range: 1000 2000
-    // @Units: PWM
-    GSCALAR(cam_tilt_center, "CAM_CENTER", 1500),
-
     // @Param: FRAME_CONFIG
     // @DisplayName: Frame configuration
     // @Description: Set this parameter according to your vehicle/motor configuration
@@ -727,8 +711,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(terrain_follow, "TERRAIN_FOLLOW", 0),
 
-    GSCALAR(cam_slew_limit, "CAM_SLEW_LIMIT", 30.0),
-
     // @Group:
     // @Path: Parameters.cpp
     GOBJECT(g2, "",  ParametersG2),
@@ -817,6 +799,8 @@ void Sub::load_parameters(void)
     AP_Param::set_default_by_name("RC3_TRIM", 1100);
     AP_Param::set_default_by_name("COMPASS_OFFS_MAX", 1000);
     AP_Param::set_default_by_name("INS_GYR_CAL", 0);
+    AP_Param::set_default_by_name("MNT_DEFLT_MODE", MAV_MOUNT_MODE_RC_TARGETING);
+    AP_Param::set_default_by_name("MNT_JSTICK_SPD", 100);
 }
 
 void Sub::convert_old_parameters(void)

ArduSub/Parameters.h

@@ -272,9 +272,7 @@ public:
     AP_Float        minGain;
     AP_Int8         numGainSettings;
     AP_Float        throttle_gain;
-    AP_Int16        cam_tilt_center;
 
-    AP_Int16        cam_tilt_step;
     AP_Int16        lights_step;
 
     // Joystick button parameters
@@ -315,7 +313,6 @@ 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() :

ArduSub/joystick.cpp

@@ -60,6 +60,9 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
 
     bool shift = false;
 
+    // Neutralize camera tilt speed setpoint
+    cam_tilt = 1500;
+
     // Detect if any shift button is pressed
     for (uint8_t i = 0 ; i < 16 ; i++) {
         if ((buttons & (1 << i)) && get_button(i)->function() == JSButton::button_function_t::k_shift) {
@@ -158,38 +161,16 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         break;
 
     case JSButton::button_function_t::k_mount_center:
-        cam_tilt = g.cam_tilt_center;
+        camera_mount.set_angle_targets(0, 0, 0);
+        // for some reason the call to set_angle_targets changes the mode to mavlink targeting!
+        camera_mount.set_mode(MAV_MOUNT_MODE_RC_TARGETING);
+        break;
+    case JSButton::button_function_t::k_mount_tilt_up:
+        cam_tilt = 1900;
+        break;
+    case JSButton::button_function_t::k_mount_tilt_down:
+        cam_tilt = 1100;
         break;
-    case JSButton::button_function_t::k_mount_tilt_up: {
-        uint8_t i;
-
-        // Find the first aux channel configured as mount tilt, if any
-        if (SRV_Channels::find_channel(SRV_Channel::k_mount_tilt, i)) {
-
-            // Get the channel output limits
-            SRV_Channel *ch = SRV_Channels::srv_channel(i);
-            uint16_t min = ch->get_output_min();
-            uint16_t max = ch->get_output_max();
-
-            cam_tilt = constrain_int16(cam_tilt-g.cam_tilt_step,min,max);
-        }
-    }
-    break;
-    case JSButton::button_function_t::k_mount_tilt_down: {
-        uint8_t i;
-
-        // Find the first aux channel configured as mount tilt, if any
-        if (SRV_Channels::find_channel(SRV_Channel::k_mount_tilt, i)) {
-
-            // Get the channel output limits
-            SRV_Channel *ch = SRV_Channels::srv_channel(i);
-            uint16_t min = ch->get_output_min();
-            uint16_t max = ch->get_output_max();
-
-            cam_tilt = constrain_int16(cam_tilt+g.cam_tilt_step,min,max);
-        }
-    }
-    break;
     case JSButton::button_function_t::k_camera_trigger:
         break;
     case JSButton::button_function_t::k_camera_source_toggle:

ArduSub/radio.cpp

@@ -31,7 +31,7 @@ void Sub::init_rc_in()
         hal.rcin->set_override(i, 1500);
     }
 
-    hal.rcin->set_override(7, g.cam_tilt_center); // camera tilt channel
+    hal.rcin->set_override(7, 1500); // camera tilt channel
     hal.rcin->set_override(8, 1100); // lights 1 channel
     hal.rcin->set_override(9, 1100); // lights 2 channel
     hal.rcin->set_override(10, 1100); // video switch