AP_Camera: add set lens support

libraries/AP_Camera/AP_Camera.cpp

@@ -529,6 +529,30 @@ bool AP_Camera::set_tracking(uint8_t instance, TrackingType tracking_type, const
     return backend->set_tracking(tracking_type, p1, p2);
 }
 
+// set camera lens as a value from 0 to 5
+bool AP_Camera::set_lens(uint8_t lens)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    if (primary == nullptr) {
+        return false;
+    }
+    return primary->set_lens(lens);
+}
+
+bool AP_Camera::set_lens(uint8_t instance, uint8_t lens)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+
+    // call instance
+    return backend->set_lens(lens);
+}
+
 #if AP_CAMERA_SCRIPTING_ENABLED
 // accessor to allow scripting backend to retrieve state
 // returns true on success and cam_state is filled in

libraries/AP_Camera/AP_Camera.h

@@ -135,6 +135,10 @@ public:
     bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2);
     bool set_tracking(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2);
 
+    // set camera lens as a value from 0 to 5
+    bool set_lens(uint8_t lens);
+    bool set_lens(uint8_t instance, uint8_t lens);
+
     // set if vehicle is in AUTO mode
     void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; }
 

libraries/AP_Camera/AP_Camera_Backend.h

@@ -75,6 +75,9 @@ public:
     // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
     virtual bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) { return false; }
 
+    // set camera lens as a value from 0 to 5
+    virtual bool set_lens(uint8_t lens) { return false; }
+
     // handle MAVLink messages from the camera
     virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {}
 

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -60,6 +60,17 @@ bool AP_Camera_Mount::set_tracking(TrackingType tracking_type, const Vector2f& p
     return false;
 }
 
+
+// set camera lens as a value from 0 to 5
+bool AP_Camera_Mount::set_lens(uint8_t lens)
+{
+    AP_Mount* mount = AP::mount();
+    if (mount != nullptr) {
+        return mount->set_lens(0, lens);
+    }
+    return false;
+}
+
 // send camera information message to GCS
 void AP_Camera_Mount::send_camera_information(mavlink_channel_t chan) const
 {

libraries/AP_Camera/AP_Camera_Mount.h

@@ -51,6 +51,9 @@ public:
     // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
     bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) override;
 
+    // set camera lens as a value from 0 to 5
+    bool set_lens(uint8_t lens) override;
+
     // send camera information message to GCS
     void send_camera_information(mavlink_channel_t chan) const override;