AP_Camera: allow focus and zoom control with servos

Williangalvani · Feb 7, 2025 · f14fc28 · f14fc28
1 parent f62e65f
commit f14fc28
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 0 deletions.
diff --git a/libraries/AP_Camera/AP_Camera_Servo.cpp b/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,31 @@ 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) {
+        // set zoom rate
+        float current_zoom = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_zoom);
+        float new_zoom = current_zoom + zoom_value * 10;
+        SRV_Channels::set_output_scaled(SRV_Channel::k_cam_zoom, new_zoom);
+        return true;
+    }
+    return false;
+}
+
+// set focus specified as rate, percentage or auto
+// focus in = -1, focus hold = 0, focus out = 1
+SetFocusResult AP_Camera_Servo::set_focus(FocusType focus_type, float focus_value) {
+    if (focus_type == FocusType::RATE) {    
+        // set zoom rate
+        const float current_focus = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_focus);
+        const float new_focus = current_focus + focus_value * 10;
+        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)
 {

diff --git a/libraries/AP_Camera/AP_Camera_Servo.h b/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;
 

diff --git a/libraries/SRV_Channel/SRV_Channel.h b/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;