AP_Camera: support set-camera-source mavlink command

libraries/AP_Camera/AP_Camera.cpp

@@ -324,6 +324,29 @@ MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet)
             break;
         }
         return MAV_RESULT_DENIED;
+
+#if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
+    case MAV_CMD_SET_CAMERA_SOURCE:
+        // sanity check instance
+        if (is_negative(packet.param1) || packet.param1 > AP_CAMERA_MAX_INSTANCES) {
+            return MAV_RESULT_DENIED;
+        }
+        if (is_zero(packet.param1)) {
+            // set camera source for all backends
+            bool accepted = false;
+            for (uint8_t i = 0; i < ARRAY_SIZE(_backends); i++) {
+                if (_backends[i] != nullptr) {
+                    accepted |= set_camera_source(i, (AP_Camera::CameraSource)packet.param2, (AP_Camera::CameraSource)packet.param3);
+                }
+            }
+            return accepted ? MAV_RESULT_ACCEPTED : MAV_RESULT_DENIED;
+        }
+        if (set_camera_source(packet.param1-1, (AP_Camera::CameraSource)packet.param2, (AP_Camera::CameraSource)packet.param3)) {
+            return MAV_RESULT_ACCEPTED;
+        }
+        return MAV_RESULT_DENIED;
+#endif
+
     case MAV_CMD_IMAGE_START_CAPTURE:
         // param1 : camera id
         // param2 : interval (in seconds)
@@ -666,6 +689,7 @@ bool AP_Camera::set_tracking(uint8_t instance, TrackingType tracking_type, const
     return backend->set_tracking(tracking_type, p1, p2);
 }
 
+#if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
 // set camera lens as a value from 0 to 5
 bool AP_Camera::set_lens(uint8_t lens)
 {
@@ -690,6 +714,21 @@ bool AP_Camera::set_lens(uint8_t instance, uint8_t lens)
     return backend->set_lens(lens);
 }
 
+// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+bool AP_Camera::set_camera_source(uint8_t instance, CameraSource primary_source, CameraSource secondary_source)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+
+    // call instance
+    return backend->set_camera_source(primary_source, secondary_source);
+}
+#endif // AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
+
 #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

@@ -156,10 +156,23 @@ 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
+#if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
+    // set camera lens as a value from 0 to 5, instance starts from 0
     bool set_lens(uint8_t lens);
     bool set_lens(uint8_t instance, uint8_t lens);
 
+    // camera source handling enum.  This is a one-to-one mapping with the CAMERA_SOURCE mavlink enum
+    // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+    enum class CameraSource {
+        DEFAULT = 0,
+        RGB = 1,
+        IR = 2,
+        NDVI = 3,
+        RGB_WIDEANGLE = 4,
+    };
+    bool set_camera_source(uint8_t instance, CameraSource primary_source, CameraSource secondary_source);
+#endif
+
     // 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

@@ -82,9 +82,14 @@ 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; }
 
+#if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
     // set camera lens as a value from 0 to 5
     virtual bool set_lens(uint8_t lens) { return false; }
 
+    // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+    virtual bool set_camera_source(AP_Camera::CameraSource primary_source, AP_Camera::CameraSource secondary_source) { return false; }
+#endif
+
     // get camera image horizontal or vertical field of view in degrees.  returns 0 if unknown
     float horizontal_fov() const { return MAX(0, _params.hfov); }
     float vertical_fov() const { return MAX(0, _params.vfov); }

libraries/AP_Camera/AP_Camera_Mount.cpp

@@ -70,6 +70,18 @@ bool AP_Camera_Mount::set_lens(uint8_t lens)
     return false;
 }
 
+// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+bool AP_Camera_Mount::set_camera_source(AP_Camera::CameraSource primary_source, AP_Camera::CameraSource secondary_source)
+{
+#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
+    AP_Mount* mount = AP::mount();
+    if (mount != nullptr) {
+        return mount->set_camera_source(get_mount_instance(), (uint8_t)primary_source, (uint8_t)secondary_source);
+    }
+#endif
+    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

@@ -54,6 +54,9 @@ public:
     // set camera lens as a value from 0 to 5
     bool set_lens(uint8_t lens) override;
 
+    // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+    bool set_camera_source(AP_Camera::CameraSource primary_source, AP_Camera::CameraSource secondary_source) override;
+
     // send camera information message to GCS
     void send_camera_information(mavlink_channel_t chan) const override;
 

libraries/AP_Camera/AP_Camera_config.h

@@ -9,10 +9,6 @@
 #define AP_CAMERA_ENABLED 1
 #endif
 
-#ifndef AP_CAMERA_SEND_FOV_STATUS_ENABLED
-#define AP_CAMERA_SEND_FOV_STATUS_ENABLED AP_MOUNT_POI_TO_LATLONALT_ENABLED
-#endif
-
 #ifndef AP_CAMERA_BACKEND_DEFAULT_ENABLED
 #define AP_CAMERA_BACKEND_DEFAULT_ENABLED AP_CAMERA_ENABLED
 #endif
@@ -44,3 +40,12 @@
 #ifndef AP_CAMERA_SCRIPTING_ENABLED
 #define AP_CAMERA_SCRIPTING_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED
 #endif
+
+#ifndef AP_CAMERA_SEND_FOV_STATUS_ENABLED
+#define AP_CAMERA_SEND_FOV_STATUS_ENABLED AP_MOUNT_POI_TO_LATLONALT_ENABLED
+#endif
+
+// set camera source is supported on cameras that may have more than one lens which is curently only cameras within gimbals/mounts
+#ifndef AP_CAMERA_SET_CAMERA_SOURCE_ENABLED
+#define AP_CAMERA_SET_CAMERA_SOURCE_ENABLED HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
+#endif