Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Camera: add semaphore to allow multi-threaded access

This commit is contained in:
Randy Mackay 2023-04-12 09:11:26 +09:00 committed by Peter Barker
parent ffc3ad1c1a
commit de4b1890f5
6 changed files with 57 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AP_Camera/AP_Camera.cpp
View File

@ -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;

8
libraries/AP_Camera/AP_Camera.h
View File

@ -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

2
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -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)) {

4
libraries/AP_Camera/AP_Camera_Backend.h
View File

@ -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:

2
libraries/AP_Camera/AP_Camera_Scripting.cpp
View File

@ -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;

2
libraries/AP_Camera/AP_Camera_Scripting.h
View File

@ -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: