Sub: Bug fix for camera_tilt_smooth() conflict with RC_CHANNELS_OVERRIDE

ArduSub/ArduSub.cpp

@@ -50,7 +50,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
     SCHED_TASK(gcs_send_deferred,     50,    550),
     SCHED_TASK(gcs_data_stream_send,  50,    550),
     SCHED_TASK(update_mount,          50,     75),
-    SCHED_TASK(camera_tilt_smooth,    50,     50),
     SCHED_TASK(update_trigger,        50,     75),
     SCHED_TASK(ten_hz_logging_loop,   10,    350),
     SCHED_TASK(twentyfive_hz_logging, 25,    110),
@@ -224,6 +223,7 @@ void Sub::fifty_hz_loop()
     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();
     }
 }

ArduSub/Sub.h

@@ -713,7 +713,6 @@ private:
     void init_joystick();
     void transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons);
     void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false);
-    void camera_tilt_smooth();
     JSButton* get_button(uint8_t index);
     void default_js_buttons(void);
     void set_throttle_zero_flag(int16_t throttle_control);

ArduSub/joystick.cpp

@@ -7,8 +7,6 @@
 namespace {
 int16_t mode_switch_pwm = 1100;
 float cam_tilt = 1500.0;
-float cam_tilt_goal = 1500.0;
-float cam_tilt_alpha = 0.97;
 int16_t lights1 = 1100;
 int16_t lights2 = 1100;
 int16_t rollTrim = 0;
@@ -87,7 +85,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
     channels[4] = mode_switch_pwm;                                       // for testing only
     channels[5] = constrain_int16((x+xTrim)*rpyScale+rpyCenter,1100,1900);           // forward for ROV
     channels[6] = constrain_int16((y+yTrim)*rpyScale+rpyCenter,1100,1900);           // lateral for ROV
-    channels[7] = 0xffff;                                     // camera tilt (sent in camera_tilt_smooth)
+    channels[7] = cam_tilt;                                   // camera tilt
     channels[8] = lights1;                                    // lights 1
     channels[9] = lights2;                                    // lights 2
     channels[10] = video_switch;                              // video switch
@@ -160,7 +158,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         toggle_mode = false;
         break;
     case JSButton::button_function_t::k_mount_center:
-        cam_tilt_goal = g.cam_tilt_center;
+        cam_tilt = g.cam_tilt_center;
         break;
     case JSButton::button_function_t::k_mount_tilt_up: {
         uint8_t i;
@@ -173,7 +171,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             uint16_t min = ch->get_output_min();
             uint16_t max = ch->get_output_max();
 
-            cam_tilt_goal = constrain_int16(cam_tilt_goal-g.cam_tilt_step,min,max);
+            cam_tilt = constrain_int16(cam_tilt-g.cam_tilt_step,min,max);
         }
     }
     break;
@@ -188,7 +186,7 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
             uint16_t min = ch->get_output_min();
             uint16_t max = ch->get_output_max();
 
-            cam_tilt_goal = constrain_int16(cam_tilt_goal+g.cam_tilt_step,min,max);
+            cam_tilt = constrain_int16(cam_tilt+g.cam_tilt_step,min,max);
         }
     }
     break;
@@ -424,23 +422,6 @@ JSButton* Sub::get_button(uint8_t index)
     }
 }
 
-void Sub::camera_tilt_smooth()
-{
-    if (failsafe.manual_control) {
-        return;
-    }
-    int16_t channels[11];
-
-    for (uint8_t i = 0 ; i < 11 ; i++) {
-        channels[i] = 0xffff;
-    }
-    // Camera tilt low pass filter
-    cam_tilt = cam_tilt*cam_tilt_alpha+cam_tilt_goal*(1-cam_tilt_alpha);
-    channels[7] = cam_tilt;
-
-    failsafe.rc_override_active = hal.rcin->set_overrides(channels, 10);
-}
-
 void Sub::default_js_buttons()
 {
     JSButton::button_function_t defaults[16][2] = {