From c1959952b363859f764b09a16302229d3538e3d0 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Thu, 9 Mar 2017 14:29:01 -0500 Subject: [PATCH] Sub: Bug fix for camera_tilt_smooth() conflict with RC_CHANNELS_OVERRIDE --- ArduSub/ArduSub.cpp | 2 +- ArduSub/Sub.h | 1 - ArduSub/joystick.cpp | 27 ++++----------------------- 3 files changed, 5 insertions(+), 25 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index bd257bf331..c10526ef9f 100644 --- a/ArduSub/ArduSub.cpp +++ b/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(); } } diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index e4e1db986f..655eaade52 100644 --- a/ArduSub/Sub.h +++ b/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); diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index d8d6d814f9..501cc0188d 100644 --- a/ArduSub/joystick.cpp +++ b/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] = {