AP_Camera_servo: allow focus and zoom control

libraries/AP_Camera/AP_Camera_Servo.cpp

@@ -6,6 +6,12 @@
 
 extern const AP_HAL::HAL& hal;
 
+
+void AP_Camera_Servo::init() {
+    SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, 500);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, 500);
+}
+
 // update - should be called at 50hz
 void AP_Camera_Servo::update()
 {
@@ -43,6 +49,28 @@ bool AP_Camera_Servo::trigger_pic()
     return true;
 }
 
+
+bool AP_Camera_Servo::set_zoom(ZoomType zoom_type, float zoom_value) {
+    if (zoom_type == ZoomType::RATE) {
+        float current_zoom = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_zoom);
+        float new_zoom = constrain_float(current_zoom + zoom_value * 10, 0, 1000);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, new_zoom);
+        return true;
+    }
+    return false;
+}
+
+// set focus specified as rate
+SetFocusResult AP_Camera_Servo::set_focus(FocusType focus_type, float focus_value) {
+    if (focus_type == FocusType::RATE) {
+        const float current_focus = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_focus);
+        const float new_focus = constrain_float(current_focus + focus_value * 10, 0, 1000);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, new_focus);
+        return SetFocusResult::ACCEPTED;
+    }
+    return SetFocusResult::UNSUPPORTED;
+}
+
 // configure camera
 void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
 {

libraries/AP_Camera/AP_Camera_Servo.h

@@ -29,12 +29,20 @@ class AP_Camera_Servo : public AP_Camera_Backend
     // Constructor
     using AP_Camera_Backend::AP_Camera_Backend;
 
+    void init() override;
     /* Do not allow copies */
     CLASS_NO_COPY(AP_Camera_Servo);
 
     // update - should be called at 50hz
     void update() override;
 
+    // set zoom specified as a rate or percentage
+    bool set_zoom(ZoomType zoom_type, float zoom_value) override;
+
+    // set focus specified as rate, percentage or auto
+    // focus in = -1, focus hold = 0, focus out = 1
+    SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
+
     // entry point to actually take a picture.  returns true on success
     bool trigger_pic() override;
 

libraries/SRV_Channel/SRV_Channel.h

@@ -209,6 +209,7 @@ class SRV_Channel {
         k_motor30               = 177,
         k_motor31               = 178,
         k_motor32               = 179,
+        k_cam_zoom              = 180,
         k_nr_aux_servo_functions         ///< This must be the last enum value (only add new values _before_ this one)
     } Function;
 

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -200,6 +200,10 @@ void SRV_Channel::aux_servo_function_setup(void)
         // fixed wing throttle
         set_range(100);
         break;
+    case k_cam_zoom:
+    case k_cam_focus:
+        set_range(1000);
+        break;
     default:
         break;
     }