SITL: add camera simulation object

Add a new SIM_Camera object to SITL, similar to existing Sprayer and
Gripper payload simulations.

The camera simulation can be enabled via SIM_CAM_ENABLE and supports
triggering image capture via:
 - servo output (TRIG_SRV + TRIG_PWM)
 - GPIO pin (GPIO)

When triggered, the camera increments an internal image counter and
sends a GCS message indicating capture.

Author: TharakaUJ

libraries/SITL/SIM_Aircraft.cpp

@@ -1106,6 +1106,19 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
 {
     external_payload_mass = 0;
 
+    #if AP_SIM_CAM_ENABLED && AP_SIM_MAX_CAMERAS > 0
+    // update camera
+    for (uint8_t i = 0; i < ARRAY_SIZE(cameras); i++) {
+        if (cameras[i] == nullptr) {
+            continue;
+        }
+        if (!cameras[i]->is_enabled()) {
+            continue;
+        }
+        cameras[i]->update(input);
+    }
+    #endif
+
     // update sprayer
     if (sprayer && sprayer->is_enabled()) {
         sprayer->update(input);

libraries/SITL/SIM_Aircraft.h

@@ -24,6 +24,7 @@
 
 #include "SITL.h"
 #include "SITL_Input.h"
+#include "SIM_Camera.h"
 #include "SIM_Sprayer.h"
 #include "SIM_Gripper_Servo.h"
 #include "SIM_Gripper_EPM.h"
@@ -43,6 +44,7 @@
 #include "SIM_GPIO_LED_2.h"
 #include "SIM_GPIO_LED_3.h"
 #include "SIM_GPIO_LED_RGB.h"
+#include <GCS_MAVLink/GCS.h>
 
 #define MAX_SIM_INSTANCES 16
 
@@ -154,6 +156,16 @@ class Aircraft {
     float get_home_yaw() const { return home_yaw; }
 
     void set_buzzer(Buzzer *_buzzer) { buzzer = _buzzer; }
+    void add_camera(Camera *_camera) {
+        for (uint8_t i = 0; i < ARRAY_SIZE(cameras); i++) {
+            if (cameras[i] != nullptr) {
+                continue;
+            }
+            cameras[i] = _camera;
+            return;
+        }
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "No free camera slots");
+    }
     void set_sprayer(Sprayer *_sprayer) { sprayer = _sprayer; }
     void set_parachute(Parachute *_parachute) { parachute = _parachute; }
     void set_richenpower(RichenPower *_richenpower) { richenpower = _richenpower; }
@@ -389,6 +401,7 @@ class Aircraft {
     } smoothing;
 
     Buzzer *buzzer;
+    Camera *cameras[AP_SIM_MAX_CAMERAS]{};
     Sprayer *sprayer;
     Gripper_Servo *gripper;
     Gripper_EPM *gripper_epm;

libraries/SITL/SIM_Camera.cpp

@@ -0,0 +1,85 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple camera simulator class
+*/
+
+#include "SIM_Camera.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <SITL/SITL.h>
+
+using namespace SITL;
+
+// table of user settable parameters
+const AP_Param::GroupInfo Camera::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Camera Sim enable/disable
+    // @Description: Allows you to enable (1) or disable (0) the Camera simulation
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO("ENABLE", 0, Camera, _enable, 0),
+
+    // @Param: TRIG_SRV
+    // @DisplayName: Camera trigger servo pin
+    // @Description: The pin number that the Camera trigger servo is connected to. (start at 1)
+    // @Range: 0 32
+    // @User: Advanced
+    AP_GROUPINFO("TRIG_SRV", 1, Camera, _trigger_servo_pin, -1),
+
+    // @Param: TRIG_GPIO
+    // @DisplayName: Camera trigger GPIO pin
+    // @Description: The GPIO pin number used to trigger Camera capture.
+    // @Range: -1 15
+    // @User: Advanced
+    AP_GROUPINFO("GPIO_PIN", 2, Camera, _trigger_gpio_pin, -1),
+
+    // @Param: TRIG_PWM
+    // @DisplayName: Camera trigger PWM threshold
+    // @Description: PWM threshold for considering the camera trigger servo active.
+    // @Range: 1000 2000
+    // @Units: PWM
+    // @User: Advanced
+    AP_GROUPINFO("TRIG_PWM", 3, Camera, _trigger_pwm, 1300),
+
+    AP_GROUPEND
+};
+
+/*
+  update camera state
+ */
+void Camera::update(const struct sitl_input &input)
+{
+    bool servo_triggered = false;
+    if (_trigger_servo_pin >= 1 && _trigger_servo_pin <= 32) {
+        const int16_t servo_pwm = input.servos[_trigger_servo_pin - 1];
+        servo_triggered = servo_pwm >= _trigger_pwm;
+    }
+
+    bool gpio_triggered = false;
+    const auto *sitl = AP::sitl();
+    if (sitl != nullptr && _trigger_gpio_pin >= 0 && _trigger_gpio_pin <= 15) {
+        gpio_triggered = (sitl->pin_mask.get() & (1U << _trigger_gpio_pin)) != 0;
+    }
+
+    const bool trigger_active = servo_triggered || gpio_triggered;
+    if (trigger_active && !_last_trigger_state) {
+        _image_count++;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SITL: Camera image %u", (unsigned)_image_count);
+    }
+
+    _last_trigger_state = trigger_active;
+}

libraries/SITL/SIM_Camera.h

@@ -0,0 +1,51 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  simple camera simulation class
+*/
+
+#pragma once
+
+#include "stdint.h"
+#include <AP_Param/AP_Param.h>
+#include "SITL_Input.h"
+
+namespace SITL {
+
+class Camera {
+public:
+    Camera() {
+        AP_Param::setup_object_defaults(this, var_info);
+    }
+
+    // update camera state
+    void update(const struct sitl_input &input);
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+    bool is_enabled() const { return static_cast<bool>(_enable); }
+    uint32_t image_count() const { return _image_count; }
+
+private:
+    AP_Int8 _enable;
+    AP_Int8 _trigger_servo_pin;
+    AP_Int8 _trigger_gpio_pin;
+    AP_Int16 _trigger_pwm;
+
+    bool _last_trigger_state;
+    uint32_t _image_count;
+};
+
+}

libraries/SITL/SIM_config.h

@@ -5,6 +5,14 @@
 
 #define AP_SIM_MAX_GPS_SENSORS 4
 
+#ifndef AP_SIM_CAM_ENABLED
+#define AP_SIM_CAM_ENABLED AP_SIM_ENABLED
+#endif
+
+#ifndef AP_SIM_MAX_CAMERAS
+#define AP_SIM_MAX_CAMERAS 2
+#endif
+
 #ifndef AP_SIM_ADSB_ENABLED
 #define AP_SIM_ADSB_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
 #endif  // AP_SIM_ADSB_ENABLED

libraries/SITL/SITL.cpp

@@ -655,6 +655,17 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     AP_SUBGROUPINFO(vicon, "VICON_", 56, SIM, ViconParms),
 #endif  // AP_SIM_VICON_ENABLED
 
+#if AP_SIM_CAM_ENABLED
+    // @Group: CAM_
+    // @Path: ./SIM_Camera.cpp
+    AP_SUBGROUPINFO(camera_sim[0], "CAM_", 57, SIM, Camera),
+#if AP_SIM_MAX_CAMERAS > 1
+    // @Group: CAM2_
+    // @Path: ./SIM_Camera.cpp
+    AP_SUBGROUPINFO(camera_sim[1], "CAM2_", 58, SIM, Camera),
+#endif
+#endif
+
 #ifdef SFML_JOYSTICK
     AP_SUBGROUPEXTENSION("",      63, SIM,  var_sfml_joystick),
 #endif // SFML_JOYSTICK

libraries/SITL/SITL.h

@@ -13,6 +13,7 @@
 #include <AP_InertialSensor/AP_InertialSensor.h>
 
 #include "SIM_Buzzer.h"
+#include "SIM_Camera.h"
 #include "SIM_Gripper_EPM.h"
 #include "SIM_Gripper_Servo.h"
 #include "SIM_I2C.h"
@@ -542,6 +543,9 @@ class SIM {
     }
 
     Sprayer sprayer_sim;
+#if AP_SIM_CAM_ENABLED && AP_SIM_MAX_CAMERAS > 0
+    Camera camera_sim[AP_SIM_MAX_CAMERAS];
+#endif
 
     Gripper_Servo gripper_sim;
     Gripper_EPM gripper_epm_sim;