SRV_Channel: initialize focus and zoom with 1000 range

ArduPilot · Feb 7, 2025 · 216094d · 216094d
1 parent f14fc28
commit 216094d
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 7 deletions.
diff --git a/libraries/AP_Camera/AP_Camera_Servo.cpp b/libraries/AP_Camera/AP_Camera_Servo.cpp
@@ -52,22 +52,19 @@ bool AP_Camera_Servo::trigger_pic()
 
 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;
+        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, percentage or auto
-// focus in = -1, focus hold = 0, focus out = 1
+// set focus specified as rate
 SetFocusResult AP_Camera_Servo::set_focus(FocusType focus_type, float focus_value) {
-    if (focus_type == FocusType::RATE) {    
-        // set zoom rate
+    if (focus_type == FocusType::RATE) {
         const float current_focus = SRV_Channels::get_output_scaled(SRV_Channel::k_cam_focus);
-        const float new_focus = current_focus + focus_value * 10;
+        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;
     }

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