Sub: Add camera pan functionality

Fix bluerobotics/ardusub#134

Signed-off-by: Patrick José Pereira <patrickelectric@gmail.com>

ArduSub/GCS_Mavlink.cpp

@@ -358,6 +358,13 @@ bool NOINLINE Sub::send_info(mavlink_channel_t chan)
             "CamTilt",
             1 - (SRV_Channels::get_output_norm(SRV_Channel::k_mount_tilt) / 2.0f + 0.5f));
 
+    CHECK_PAYLOAD_SIZE2(NAMED_VALUE_FLOAT);
+    mavlink_msg_named_value_float_send(
+            chan,
+            AP_HAL::millis(),
+            "CamPan",
+            1 - (SRV_Channels::get_output_norm(SRV_Channel::k_mount_pan) / 2.0f + 0.5f));
+
     CHECK_PAYLOAD_SIZE2(NAMED_VALUE_FLOAT);
     mavlink_msg_named_value_float_send(
             chan,

ArduSub/joystick.cpp

@@ -6,6 +6,7 @@
 // Anonymous namespace to hold variables used only in this file
 namespace {
 float cam_tilt = 1500.0;
+float cam_pan = 1500.0;
 int16_t lights1 = 1100;
 int16_t lights2 = 1100;
 int16_t rollTrim = 0;
@@ -61,8 +62,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
+    // Neutralize camera tilt and pan speed setpoint
     cam_tilt = 1500;
+    cam_pan = 1500;
 
     // Detect if any shift button is pressed
     for (uint8_t i = 0 ; i < 16 ; i++) {
@@ -108,7 +110,7 @@ void Sub::transform_manual_control_to_rc_override(int16_t x, int16_t y, int16_t
         channels[5] = constrain_int16(yTrim*rpyScale+rpyCenter,1100,1900); // lateral for ROV
     }
 
-    channels[6] = 0;             // Unused
+    channels[6] = cam_pan;       // camera pan
     channels[7] = cam_tilt;      // camera tilt
     channels[8] = lights1;       // lights 1
     channels[9] = lights2;       // lights 2
@@ -192,10 +194,10 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
         }
         break;
     case JSButton::button_function_t::k_mount_pan_right:
-        // Not implemented
+        cam_pan = 1900;
         break;
     case JSButton::button_function_t::k_mount_pan_left:
-        // Not implemented
+        cam_pan = 1100;
         break;
     case JSButton::button_function_t::k_lights1_cycle:
         if (!held) {
@@ -661,8 +663,8 @@ void Sub::default_js_buttons()
         {JSButton::button_function_t::k_mount_center,           JSButton::button_function_t::k_none},
 
         {JSButton::button_function_t::k_input_hold_set,         JSButton::button_function_t::k_none},
-        {JSButton::button_function_t::k_mount_tilt_down,        JSButton::button_function_t::k_none},
-        {JSButton::button_function_t::k_mount_tilt_up,          JSButton::button_function_t::k_none},
+        {JSButton::button_function_t::k_mount_tilt_down,        JSButton::button_function_t::k_mount_pan_left},
+        {JSButton::button_function_t::k_mount_tilt_up,          JSButton::button_function_t::k_mount_pan_right},
         {JSButton::button_function_t::k_gain_inc,               JSButton::button_function_t::k_trim_pitch_dec},
 
         {JSButton::button_function_t::k_gain_dec,               JSButton::button_function_t::k_trim_pitch_inc},

ArduSub/radio.cpp

@@ -31,6 +31,7 @@ void Sub::init_rc_in()
         hal.rcin->set_override(i, 1500);
     }
 
+    hal.rcin->set_override(6, 1500); // camera pan channel
     hal.rcin->set_override(7, 1500); // camera tilt channel
 
     RC_Channel* chan = RC_Channels::rc_channel(8);