From 15658f15269a6194bf537cd9db86095343b94051 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Wed, 25 Oct 2017 21:44:54 -0400 Subject: [PATCH] Sub: rework camera tilt input and slew limiting --- ArduSub/ArduSub.cpp | 6 +----- ArduSub/Parameters.cpp | 20 ++------------------ ArduSub/Parameters.h | 3 --- ArduSub/joystick.cpp | 43 ++++++++++++------------------------------ ArduSub/radio.cpp | 2 +- 5 files changed, 16 insertions(+), 58 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 830172a4f4..360652cde3 100644 --- a/ArduSub/ArduSub.cpp +++ b/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(); } diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index bc0a1cd945..e397b89c48 100644 --- a/ArduSub/Parameters.cpp +++ b/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) diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index e1edad14d2..a4ec2156b1 100644 --- a/ArduSub/Parameters.h +++ b/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() : diff --git a/ArduSub/joystick.cpp b/ArduSub/joystick.cpp index 6cf0b8c1e4..90c10a6571 100644 --- a/ArduSub/joystick.cpp +++ b/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: diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp index 96f1c652b2..4f17cfba5d 100644 --- a/ArduSub/radio.cpp +++ b/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