AP_Mount: add set_camera_source support

libraries/AP_Mount/AP_Mount.cpp

@@ -833,6 +833,17 @@ bool AP_Mount::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
+// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+bool AP_Mount::set_camera_source(uint8_t instance, uint8_t primary_source, uint8_t secondary_source)
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    return backend->set_camera_source(primary_source, secondary_source);
+}
+
 // send camera information message to GCS
 void AP_Mount::send_camera_information(uint8_t instance, mavlink_channel_t chan) const
 {

libraries/AP_Mount/AP_Mount.h

@@ -250,6 +250,10 @@ class AP_Mount
     // set camera lens as a value from 0 to 5
     bool set_lens(uint8_t instance, uint8_t lens);
 
+    // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+    // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+    bool set_camera_source(uint8_t instance, uint8_t primary_source, uint8_t secondary_source);
+
     // send camera information message to GCS
     void send_camera_information(uint8_t instance, mavlink_channel_t chan) const;
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -182,6 +182,10 @@ class AP_Mount_Backend
     // 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
+    // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+    virtual bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) { return false; }
+
     // send camera information message to GCS
     virtual void send_camera_information(mavlink_channel_t chan) const {}
 

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -962,6 +962,64 @@ bool AP_Mount_Siyi::set_lens(uint8_t lens)
     return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type);
 }
 
+// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+bool AP_Mount_Siyi::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
+{
+    // only supported on ZT30.  sanity check lens values
+    if (_hardware_model != HardwareModel::ZT30) {
+        return false;
+    }
+
+    // maps primary and secondary source to siyi camera image type
+    CameraImageType cam_image_type;
+    switch (primary_source) {
+    case 0: // Default (RGB)
+        FALLTHROUGH;
+    case 1: // RGB
+        switch (secondary_source) {
+        case 0: // RGB + Default (None)
+            cam_image_type = CameraImageType::MAIN_ZOOM_SUB_THERMAL;                // 3
+            break;
+        case 2: // PIP RGB+IR
+            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE;  // 0
+            break;
+        case 3: // PIP RGB+RGB_WIDEANGLE
+            cam_image_type = CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL;  // 2
+            break;
+        default:
+            return false;
+        }
+        break;
+    case 2: // IR
+        switch (secondary_source) {
+        case 0: // IR + Default (None)
+            cam_image_type = CameraImageType::MAIN_THERMAL_SUB_ZOOM;                // 7
+            break;
+        default:
+            return false;
+        }
+        break;
+    case 3: // RGB_WIDEANGLE
+        switch (secondary_source) {
+        case 0: // RGB_WIDEANGLE + Default (None)
+            cam_image_type = CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL;           // 5
+            break;
+        case 2: // PIP RGB_WIDEANGLE+IR
+            cam_image_type = CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM;  // 1
+            break;
+        default:
+            return false;
+        }
+        break;
+    default:
+        return false;
+    }
+
+    // send desired image type to camera
+    return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type);
+}
+
 // send camera information message to GCS
 void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
 {

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -75,6 +75,10 @@ class AP_Mount_Siyi : public AP_Mount_Backend
     // set camera lens as a value from 0 to 8.  ZT30 only
     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
+    // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+    bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
+
     // send camera information message to GCS
     void send_camera_information(mavlink_channel_t chan) const override;
 

libraries/AP_Mount/AP_Mount_Viewpro.cpp

@@ -862,6 +862,47 @@ bool AP_Mount_Viewpro::set_lens(uint8_t lens)
     return send_camera_command(new_image_sensor, CameraCommand::NO_ACTION, 0);
 }
 
+// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
+// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+bool AP_Mount_Viewpro::set_camera_source(uint8_t primary_source, uint8_t secondary_source)
+{
+    // maps primary and secondary source to viewpro image sensor
+    ImageSensor new_image_sensor;
+    switch (primary_source) {
+    case 0: // Default (RGB)
+        FALLTHROUGH;
+    case 1: // RGB
+        switch (secondary_source) {
+        case 0: // RGB + Default (None)
+            new_image_sensor = ImageSensor::EO1;
+            break;
+        case 2: // PIP RGB+IR
+            new_image_sensor = ImageSensor::EO1_IR_PIP;
+            break;
+        default:
+            return false;
+        }
+        break;
+    case 2: // IR
+        switch (secondary_source) {
+        case 0: // IR + Default (None)
+            new_image_sensor = ImageSensor::IR;
+            break;
+        case 1: // PIP IR+RGB
+            new_image_sensor = ImageSensor::IR_EO1_PIP;
+            break;
+        default:
+            return false;
+        }
+        break;
+    default:
+        return false;
+    }
+
+    // send desired image type to camera
+    return send_camera_command(new_image_sensor, CameraCommand::NO_ACTION, 0);
+}
+
 // send camera information message to GCS
 void AP_Mount_Viewpro::send_camera_information(mavlink_channel_t chan) const
 {

libraries/AP_Mount/AP_Mount_Viewpro.h

@@ -78,6 +78,10 @@ class AP_Mount_Viewpro : public AP_Mount_Backend
     // 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
+    // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
+    bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
+
     // send camera information message to GCS
     void send_camera_information(mavlink_channel_t chan) const override;