AP_Camera: add semaphore to allow multi-threaded access

libraries/AP_Camera/AP_Camera.cpp

@@ -61,6 +61,8 @@ AP_Camera::AP_Camera(uint32_t _log_camera_bit) :
 // set camera trigger distance in a mission
 void AP_Camera::set_trigger_distance(float distance_m)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -70,6 +72,8 @@ void AP_Camera::set_trigger_distance(float distance_m)
 // momentary switch to change camera between picture and video modes
 void AP_Camera::cam_mode_toggle()
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -79,6 +83,8 @@ void AP_Camera::cam_mode_toggle()
 // take a picture
 void AP_Camera::take_picture()
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -89,6 +95,8 @@ void AP_Camera::take_picture()
 // start_recording should be true to start recording, false to stop recording
 bool AP_Camera::record_video(bool start_recording)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -99,6 +107,8 @@ bool AP_Camera::record_video(bool start_recording)
 // zoom out = -1, hold = 0, zoom in = 1
 bool AP_Camera::set_zoom_step(int8_t zoom_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -109,6 +119,8 @@ bool AP_Camera::set_zoom_step(int8_t zoom_step)
 // focus in = -1, focus hold = 0, focus out = 1
 bool AP_Camera::set_manual_focus_step(int8_t focus_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -118,6 +130,8 @@ bool AP_Camera::set_manual_focus_step(int8_t focus_step)
 // auto focus
 bool AP_Camera::set_auto_focus()
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return false;
     }
@@ -199,6 +213,8 @@ void AP_Camera::init()
 // handle incoming mavlink messages
 void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (msg.msgid == MAVLINK_MSG_ID_DIGICAM_CONTROL) {
         // decode deprecated MavLink message that controls camera.
         __mavlink_digicam_control_t packet;
@@ -287,6 +303,8 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
 // set camera trigger distance in a mission
 void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -299,6 +317,8 @@ void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m)
 // momentary switch to change camera between picture and video modes
 void AP_Camera::cam_mode_toggle(uint8_t instance)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -311,6 +331,8 @@ void AP_Camera::cam_mode_toggle(uint8_t instance)
 // configure camera
 void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -319,6 +341,8 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu
 
 void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -331,6 +355,8 @@ void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_s
 // handle camera control
 void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
 {
+    WITH_SEMAPHORE(_rsem);
+
     if (primary == nullptr) {
         return;
     }
@@ -339,6 +365,8 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
 
 void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -351,8 +379,10 @@ void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float z
 /*
   Send camera feedback to the GCS
  */
-void AP_Camera::send_feedback(mavlink_channel_t chan) const
+void AP_Camera::send_feedback(mavlink_channel_t chan)
 {
+    WITH_SEMAPHORE(_rsem);
+
     // call each instance
     for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
@@ -366,6 +396,8 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) const
 */
 void AP_Camera::update()
 {
+    WITH_SEMAPHORE(_rsem);
+
     // call each instance
     for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
         if (_backends[instance] != nullptr) {
@@ -377,6 +409,8 @@ void AP_Camera::update()
 // take_picture - take a picture
 void AP_Camera::take_picture(uint8_t instance)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return;
@@ -390,6 +424,8 @@ void AP_Camera::take_picture(uint8_t instance)
 // start_recording should be true to start recording, false to stop recording
 bool AP_Camera::record_video(uint8_t instance, bool start_recording)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -403,6 +439,8 @@ bool AP_Camera::record_video(uint8_t instance, bool start_recording)
 // zoom out = -1, hold = 0, zoom in = 1
 bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -416,6 +454,8 @@ bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step)
 // focus in = -1, focus hold = 0, focus out = 1
 bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -428,6 +468,8 @@ bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step)
 // auto focus.  returns true on success
 bool AP_Camera::set_auto_focus(uint8_t instance)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;
@@ -440,8 +482,10 @@ bool AP_Camera::set_auto_focus(uint8_t instance)
 #if AP_CAMERA_SCRIPTING_ENABLED
 // accessor to allow scripting backend to retrieve state
 // returns true on success and cam_state is filled in
-bool AP_Camera::get_state(uint8_t instance, camera_state_t& cam_state) const
+bool AP_Camera::get_state(uint8_t instance, camera_state_t& cam_state)
 {
+    WITH_SEMAPHORE(_rsem);
+
     auto *backend = get_instance(instance);
     if (backend == nullptr) {
         return false;

libraries/AP_Camera/AP_Camera.h

@@ -96,7 +96,7 @@ public:
     // MAVLink methods
     void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
     MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
-    void send_feedback(mavlink_channel_t chan) const;
+    void send_feedback(mavlink_channel_t chan);
 
     // configure camera
     void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
@@ -143,9 +143,12 @@ public:
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in
-    bool get_state(uint8_t instance, camera_state_t& cam_state) const;
+    bool get_state(uint8_t instance, camera_state_t& cam_state);
 #endif
 
+    // allow threads to lock against AHRS update
+    HAL_Semaphore &get_semaphore() { return _rsem; }
+
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -177,6 +180,7 @@ private:
     // perform any required parameter conversion
     void convert_params();
 
+    HAL_Semaphore _rsem;                // semaphore for multi-thread access
     AP_Camera_Backend *primary;         // primary camera backed
     bool _is_in_auto_mode;              // true if in AUTO mode
     uint32_t log_camera_bit;            // logging bit (from LOG_BITMASK) to enable camera logging

libraries/AP_Camera/AP_Camera_Backend.cpp

@@ -107,7 +107,7 @@ void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step,
 }
 
 // send camera feedback message to GCS
-void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan) const
+void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan)
 {
     int32_t altitude = 0;
     if (camera_feedback.location.initialised() && !camera_feedback.location.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude)) {

libraries/AP_Camera/AP_Camera_Backend.h

@@ -80,12 +80,12 @@ public:
     void set_trigger_distance(float distance_m) { _params.trigg_dist.set(distance_m); }
 
     // send camera feedback message to GCS
-    void send_camera_feedback(mavlink_channel_t chan) const;
+    void send_camera_feedback(mavlink_channel_t chan);
 
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in
-    virtual bool get_state(camera_state_t& cam_state) const { return false; }
+    virtual bool get_state(camera_state_t& cam_state) { return false; }
 #endif
 
 protected:

libraries/AP_Camera/AP_Camera_Scripting.cpp

@@ -47,7 +47,7 @@ bool AP_Camera_Scripting::set_auto_focus()
 
 // access for scripting backend to retrieve state
 // returns true on success and cam_state is filled in
-bool AP_Camera_Scripting::get_state(camera_state_t& cam_state) const
+bool AP_Camera_Scripting::get_state(camera_state_t& cam_state)
 {
     cam_state = _cam_state;
     return true;

libraries/AP_Camera/AP_Camera_Scripting.h

@@ -51,7 +51,7 @@ public:
     bool set_auto_focus() override;
 
     // returns true on success and cam_state is filled in
-    bool get_state(camera_state_t& cam_state) const override;
+    bool get_state(camera_state_t& cam_state) override;
 
 private: