AP_Camera: allow focus and zoom control with servos

libraries/AP_Camera/AP_Camera_Servo.cpp

@@ -6,6 +6,14 @@
 
 extern const AP_HAL::HAL& hal;
 
+// initialize the AP_Camera_Servo driver
+void AP_Camera_Servo::init()
+{
+    // set the zoom and focus to the trim point
+    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()
 {
@@ -22,6 +30,13 @@ void AP_Camera_Servo::update()
     } else {
         SRV_Channels::set_output_pwm(SRV_Channel::k_cam_iso, _params.servo_off_pwm);
     }
+    float current_zoom = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_zoom);
+    float new_zoom = constrain_float(current_zoom + zoom_current_rate, 0, 1000);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, new_zoom);
+
+    float current_focus = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_focus);
+    float new_focus = constrain_float(current_focus + focus_current_rate, 0, 1000);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_cam_focus, new_focus);
 
     // call parent update
     AP_Camera_Backend::update();
@@ -43,6 +58,26 @@ 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) {
+        zoom_current_rate = zoom_value;
+        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) {
+        focus_current_rate = focus_value;
+        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

@@ -35,14 +35,25 @@ public:
     // 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;
 
     // configure camera
     void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time) override;
 
-private:
+    // initialize the AP_Camera_Servo driver
+    void init() override;
 
+private:
+    float zoom_current_rate;    // current zoom rate of change
+    float focus_current_rate;   // current focus rate of change
     uint16_t trigger_counter;   // count of number of cycles shutter should be held open
     uint16_t iso_counter;       // count of number of cycles iso output should be held open
 };

libraries/SRV_Channel/SRV_Channel.h

@@ -188,6 +188,7 @@ public:
         k_rcin14_mapped         = 153,
         k_rcin15_mapped         = 154,
         k_rcin16_mapped         = 155,
+        k_cam_zoom              = 180,   // 180 matches the value on master
         k_nr_aux_servo_functions         ///< This must be the last enum value (only add new values _before_ this one)
     } Aux_servo_function_t;