From 47977b163582aa1546bfedc79713f88e9318b4c1 Mon Sep 17 00:00:00 2001 From: Asif Khan Date: Wed, 2 Aug 2023 00:15:51 +0530 Subject: [PATCH] AP_Camera: add parameter CAMx_MNT_INST for associating camera with corresponding mount --- libraries/AP_Camera/AP_Camera_Backend.cpp | 10 ++++++++++ libraries/AP_Camera/AP_Camera_Backend.h | 3 +++ libraries/AP_Camera/AP_Camera_Logging.cpp | 3 +-- libraries/AP_Camera/AP_Camera_Mount.cpp | 16 ++++++++-------- libraries/AP_Camera/AP_Camera_Params.cpp | 6 ++++++ libraries/AP_Camera/AP_Camera_Params.h | 1 + 6 files changed, 29 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index 0dba97f0d2..a91c6fda45 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -93,6 +93,16 @@ void AP_Camera_Backend::update() take_picture(); } +// get corresponding mount instance for the camera +uint8_t AP_Camera_Backend::get_mount_instance() const +{ + // instance 0 means default + if (_params.mount_instance.get() == 0) { + return _instance; + } + return _params.mount_instance.get() - 1; +} + // take a picture. returns true on success bool AP_Camera_Backend::take_picture() { diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index f917085a54..568402e44d 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -144,6 +144,9 @@ protected: void Write_Trigger(); void Write_CameraInfo(enum LogMessages msg, uint64_t timestamp_us=0); + // get corresponding mount instance for the camera + uint8_t get_mount_instance() const; + // internal members uint8_t _instance; // this instance's number bool timer_installed; // true if feedback pin change detected using timer diff --git a/libraries/AP_Camera/AP_Camera_Logging.cpp b/libraries/AP_Camera/AP_Camera_Logging.cpp index bad765067e..903f42a8a8 100644 --- a/libraries/AP_Camera/AP_Camera_Logging.cpp +++ b/libraries/AP_Camera/AP_Camera_Logging.cpp @@ -68,8 +68,7 @@ void AP_Camera_Backend::Write_CameraInfo(enum LogMessages msg, uint64_t timestam #if HAL_MOUNT_ENABLED auto *mount = AP_Mount::get_singleton(); if (mount!= nullptr) { - // assuming camera instance matches mount instance - mount->write_log(_instance, timestamp_us); + mount->write_log(get_mount_instance(), timestamp_us); } #endif } diff --git a/libraries/AP_Camera/AP_Camera_Mount.cpp b/libraries/AP_Camera/AP_Camera_Mount.cpp index 75294d16ef..f85e646a9b 100644 --- a/libraries/AP_Camera/AP_Camera_Mount.cpp +++ b/libraries/AP_Camera/AP_Camera_Mount.cpp @@ -10,7 +10,7 @@ bool AP_Camera_Mount::trigger_pic() { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - mount->take_picture(0); + mount->take_picture(get_mount_instance()); return true; } return false; @@ -22,7 +22,7 @@ bool AP_Camera_Mount::record_video(bool start_recording) { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->record_video(0, start_recording); + return mount->record_video(get_mount_instance(), start_recording); } return false; } @@ -32,7 +32,7 @@ bool AP_Camera_Mount::set_zoom(ZoomType zoom_type, float zoom_value) { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_zoom(0, zoom_type, zoom_value); + return mount->set_zoom(get_mount_instance(), zoom_type, zoom_value); } return false; } @@ -43,7 +43,7 @@ SetFocusResult AP_Camera_Mount::set_focus(FocusType focus_type, float focus_valu { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_focus(0, focus_type, focus_value); + return mount->set_focus(get_mount_instance(), focus_type, focus_value); } return SetFocusResult::FAILED; } @@ -55,7 +55,7 @@ bool AP_Camera_Mount::set_tracking(TrackingType tracking_type, const Vector2f& p { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_tracking(0, tracking_type, p1, p2); + return mount->set_tracking(get_mount_instance(), tracking_type, p1, p2); } return false; } @@ -66,7 +66,7 @@ bool AP_Camera_Mount::set_lens(uint8_t lens) { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->set_lens(0, lens); + return mount->set_lens(get_mount_instance(), lens); } return false; } @@ -76,7 +76,7 @@ void AP_Camera_Mount::send_camera_information(mavlink_channel_t chan) const { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->send_camera_information(0, chan); + return mount->send_camera_information(get_mount_instance(), chan); } } @@ -85,7 +85,7 @@ void AP_Camera_Mount::send_camera_settings(mavlink_channel_t chan) const { AP_Mount* mount = AP::mount(); if (mount != nullptr) { - return mount->send_camera_settings(0, chan); + return mount->send_camera_settings(get_mount_instance(), chan); } } diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index ca7e34ae3e..ee3f779659 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -81,6 +81,12 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @User: Standard AP_GROUPINFO("_OPTIONS", 10, AP_Camera_Params, options, 0), + // @Param: _MNT_INST + // @DisplayName: Camera Mount instance + // @Description: Mount instance camera is associated with. 0 means camera and mount have identical instance numbers e.g. camera1 and mount1 + // @User: Standard + AP_GROUPINFO("_MNT_INST", 11, AP_Camera_Params, mount_instance, 0), + AP_GROUPEND }; diff --git a/libraries/AP_Camera/AP_Camera_Params.h b/libraries/AP_Camera/AP_Camera_Params.h index 904252b69f..ca54550fe1 100644 --- a/libraries/AP_Camera/AP_Camera_Params.h +++ b/libraries/AP_Camera/AP_Camera_Params.h @@ -22,6 +22,7 @@ public: AP_Int8 relay_on; // relay value to trigger camera AP_Float interval_min; // minimum time (in seconds) between shots required by camera AP_Int8 options; // whether to start recording when armed and stop when disarmed + AP_Int8 mount_instance; // mount instance to which camera is associated with // pin number for accurate camera feedback messages AP_Int8 feedback_pin;