From 9d0973904498d8d44375050e6ca70c0f430d0482 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 21 Aug 2023 21:29:28 +0900 Subject: [PATCH] AP_Mount: Viewpro supports get rangefinder distance --- libraries/AP_Mount/AP_Mount.cpp | 10 ++++++++++ libraries/AP_Mount/AP_Mount.h | 7 +++++++ libraries/AP_Mount/AP_Mount_Backend.cpp | 5 +++++ libraries/AP_Mount/AP_Mount_Backend.h | 7 +++++++ libraries/AP_Mount/AP_Mount_Viewpro.cpp | 16 ++++++++++++++++ libraries/AP_Mount/AP_Mount_Viewpro.h | 8 ++++++++ libraries/AP_Mount/LogStructure.h | 22 ++++++++++++---------- 7 files changed, 65 insertions(+), 10 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index dc388cbb0c..433e7b1e67 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -824,6 +824,16 @@ void AP_Mount::send_camera_settings(uint8_t instance, mavlink_channel_t chan) co backend->send_camera_settings(chan); } +// get rangefinder distance. Returns true on success +bool AP_Mount::get_rangefinder_distance(uint8_t instance, float& distance_m) const +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + return backend->get_rangefinder_distance(distance_m); +} + AP_Mount_Backend *AP_Mount::get_primary() const { return get_instance(_primary); diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 55306d5efb..2bd9101df9 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -247,6 +247,13 @@ public: // send camera settings message to GCS void send_camera_settings(uint8_t instance, mavlink_channel_t chan) const; + // + // rangefinder + // + + // get rangefinder distance. Returns true on success + bool get_rangefinder_distance(uint8_t instance, float& distance_m) const; + // parameter var table static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index be0bbb81e2..819e60f37e 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -387,6 +387,10 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) bool target_yaw_is_ef = false; IGNORE_RETURN(get_angle_target(target_roll, target_pitch, target_yaw, target_yaw_is_ef)); + // get rangefinder distance + float rangefinder_dist = nanf; + IGNORE_RETURN(get_rangefinder_distance(rangefinder_dist)); + const struct log_Mount pkt { LOG_PACKET_HEADER_INIT(static_cast(LOG_MOUNT_MSG)), time_us : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(), @@ -399,6 +403,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) actual_yaw_bf : yaw_bf, desired_yaw_ef: target_yaw_is_ef ? target_yaw : nanf, actual_yaw_ef : yaw_ef, + rangefinder_dist : rangefinder_dist, }; AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt)); } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 3481950fe6..af7965d2d6 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -178,6 +178,13 @@ public: // send camera settings message to GCS virtual void send_camera_settings(mavlink_channel_t chan) const {} + // + // rangefinder + // + + // get rangefinder distance. Returns true on success + virtual bool get_rangefinder_distance(float& distance_m) const { return false; } + protected: enum class MountTargetType { diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index b58ca0fcdf..32c3838edd 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -370,6 +370,9 @@ void AP_Mount_Viewpro::process_packet() // get optical zoom times _zoom_times = UINT16_VALUE(_msg_buff[_msg_buff_data_start+39], _msg_buff[_msg_buff_data_start+40]) * 0.1; + + // get laser rangefinder distance + _rangefinder_dist_m = UINT16_VALUE(_msg_buff[_msg_buff_data_start+33], _msg_buff[_msg_buff_data_start+34]) * 0.1; break; } @@ -915,4 +918,17 @@ void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown } +// get rangefinder distance. Returns true on success +bool AP_Mount_Viewpro::get_rangefinder_distance(float& distance_m) const +{ + // if not healthy or zero distance return false + // healthy() checks attitude timeout which is in same message as rangefinder distance + if (!healthy()) { + return false; + } + + distance_m = _rangefinder_dist_m; + return true; +} + #endif // HAL_MOUNT_VIEWPRO_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index ab9c89b7de..2e9fc3cdb9 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/libraries/AP_Mount/AP_Mount_Viewpro.h @@ -84,6 +84,13 @@ public: // send camera settings message to GCS void send_camera_settings(mavlink_channel_t chan) const override; + // + // rangefinder + // + + // get rangefinder distance. Returns true on success + bool get_rangefinder_distance(float& distance_m) const override; + protected: // get attitude as a quaternion. returns true on success @@ -384,6 +391,7 @@ private: bool _got_firmware_version; // true once we have received the firmware version uint8_t _model_name[11] {}; // model name received from gimbal bool _got_model_name; // true once we have received model name + float _rangefinder_dist_m; // latest rangefinder distance (in meters) }; #endif // HAL_MOUNT_VIEWPRO_ENABLED diff --git a/libraries/AP_Mount/LogStructure.h b/libraries/AP_Mount/LogStructure.h index ab66620cf7..369128bb21 100644 --- a/libraries/AP_Mount/LogStructure.h +++ b/libraries/AP_Mount/LogStructure.h @@ -6,17 +6,18 @@ LOG_MOUNT_MSG // @LoggerMessage: MNT -// @Description: Mount's actual and Target/Desired RPY information +// @Description: Mount's desired and actual roll, pitch and yaw angles // @Field: TimeUS: Time since system startup // @Field: I: Instance number -// @Field: DesRoll: mount's desired roll -// @Field: Roll: mount's actual roll -// @Field: DesPitch: mount's desired pitch -// @Field: Pitch: mount's actual pitch -// @Field: DesYawB: mount's desired yaw in body frame -// @Field: YawB: mount's actual yaw in body frame -// @Field: DesYawE: mount's desired yaw in earth frame -// @Field: YawE: mount's actual yaw in earth frame +// @Field: DRoll: Desired roll +// @Field: Roll: Actual roll +// @Field: DPitch: Desired pitch +// @Field: Pitch: Actual pitch +// @Field: DYawB: Desired yaw in body frame +// @Field: YawB: Actual yaw in body frame +// @Field: DYawE: Desired yaw in earth frame +// @Field: YawE: Actual yaw in earth frame +// @Field: Dist: Rangefinder distance struct PACKED log_Mount { LOG_PACKET_HEADER; @@ -30,9 +31,10 @@ struct PACKED log_Mount { float actual_yaw_bf; float desired_yaw_ef; float actual_yaw_ef; + float rangefinder_dist; }; #define LOG_STRUCTURE_FROM_MOUNT \ { LOG_MOUNT_MSG, sizeof(log_Mount), \ - "MNT", "QBffffffff","TimeUS,I,DesRoll,Roll,DesPitch,Pitch,DesYawB,YawB,DesYawE,YawE", "s#dddddddd", "F---------" }, + "MNT", "QBfffffffff","TimeUS,I,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#ddddddddm", "F---------0" },