From eec2cfb88583bcce76724972ad87b17e1a0abc09 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Sat, 17 Jan 2026 07:45:46 -0600 Subject: [PATCH] AP_Mount: add mount POI lock Aux Func/Switch --- libraries/AP_Mount/AP_Mount.cpp | 38 ++++++++++++++- libraries/AP_Mount/AP_Mount.h | 10 ++++ libraries/AP_Mount/AP_Mount_Backend.cpp | 63 ++++++++++++++++++++++++- libraries/AP_Mount/AP_Mount_Backend.h | 22 ++++++++- libraries/AP_Mount/AP_Mount_config.h | 6 ++- libraries/AP_Mount/LogStructure.h | 4 +- 6 files changed, 138 insertions(+), 5 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index dc2e2fe4747..4a837b7b780 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -668,6 +668,42 @@ bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Locati } #endif +#if AP_MOUNT_POI_LOCK_ENABLED +// lock currently viewed GPS point and switch to GPS Targeting mode +void AP_Mount::set_poi_lock(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's set_poi_lock + backend->set_poi_lock(); +} + +void AP_Mount::clear_poi_lock(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's clear_poi_lock + backend->clear_poi_lock(); +} + +void AP_Mount::suspend_poi_lock(uint8_t instance) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + + // call backend's suspend_poi_lock + backend->suspend_poi_lock(); +} +#endif // AP_MOUNT_POI_LOCK_ENABLED + // get attitude as a quaternion. returns true on success. // att_quat will be an earth-frame quaternion rotated such that // yaw is in body-frame. @@ -797,7 +833,7 @@ void AP_Mount::set_target_sysid(uint8_t instance, uint8_t sysid) if (backend == nullptr) { return; } - // call instance's set_roi_cmd + // call instance's set target SYSID cmd backend->set_target_sysid(sysid); } diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 8c1b3f4d3cd..5a12716cc5f 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -177,6 +177,16 @@ public: // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); } void set_yaw_lock(uint8_t instance, bool yaw_lock); + +#if AP_MOUNT_POI_LOCK_ENABLED + // controls POI lock which locks gimbal to GPS point currently in view and switches to GPS Targeting mode, suspends tracking poi or clears it + void set_poi_lock() { set_poi_lock(_primary); } + void set_poi_lock(uint8_t instance); + void clear_poi_lock() { clear_poi_lock(_primary); } + void clear_poi_lock(uint8_t instance); + void suspend_poi_lock() { suspend_poi_lock(_primary); } + void suspend_poi_lock(uint8_t instance); +#endif // set angle target in degrees // roll and pitch are in earth-frame diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index bf96344f3a5..72dc05f7a5b 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -45,6 +45,10 @@ void AP_Mount_Backend::update() const bool mount_open = (_mode == MAV_MOUNT_MODE_RETRACT); SRV_Channels::move_servo(_open_idx, mount_open, 0, 1); +#if AP_MOUNT_POI_LOCK_ENABLED + update_poi_lock_target(); + #endif // AP_MOUNT_POI_LOCK_ENABLED + // location exists for mode Location current_loc; switch (_mode) { @@ -129,7 +133,7 @@ void AP_Mount_Backend::update_mnt_target_from_rc_target() // yaw angle mnt_target.angle_rad.yaw = radians(((yaw_in + 1.0f) * 0.5f * (_params.yaw_angle_max - _params.yaw_angle_min) + _params.yaw_angle_min)); - + // if in yaw ef lock, we use the captured and adjusted yaw_lock_heading rad to // adjust the yaw so that any RC yaw changes are reflected in locked heading if (mnt_target.angle_rad.yaw_is_ef) { @@ -237,6 +241,62 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc) } } +#if AP_MOUNT_POI_LOCK_ENABLED +// set poi_lock - switch to GPS Targeting mode using current gimbal view's GPS point or save poi location as target +void AP_Mount_Backend::set_poi_lock() +{ + saved_mount_mode = get_mode(); //save current mount mode for the suspend_poi_lock + if (!roi_is_set()) { + mnt_target.poi_start_ms = AP_HAL::millis(); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking r=%.1f p=%.1f y=%.1f", degrees(mnt_target.angle_rad.roll), degrees(mnt_target.angle_rad.pitch), degrees(mnt_target.angle_rad.yaw)); + } else { // there is a poi target, just turn POI tracking back on + set_mode(MAV_MOUNT_MODE_GPS_POINT); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: tracking"); + mnt_target.poi_start_ms = 0; + } +} + +// clear poi_lock - clear POI location and revert to default mode +void AP_Mount_Backend::clear_poi_lock() +{ + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Cleared"); + clear_roi_target(); +} + +// suspend_poi_lock - revert to saved targeting mode, if it exists and POI target exists, otherwise do nothing +void AP_Mount_Backend::suspend_poi_lock() +{ + if (roi_is_set() && saved_mount_mode != MAV_MOUNT_MODE_ENUM_END) { + set_mode(saved_mount_mode); // set back to mode before GPS_POINT if its been set by switch going HIGH + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Revert mode,target saved");; + } +} + +// update_poi_lock_target - tries to obtain POI location and start tracking,caller only needs to set poi_start_ms to current time to execute this +void AP_Mount_Backend::update_poi_lock_target() +{ + if (mnt_target.poi_start_ms == 0) { + return; + } + // POI calculation is running and but will silently give up after 3 seconds normally if it does not succeed + // try to resolve a AuxFunc POI command to a lat/lng/alt using get_poi + // set up variables for get_poi call + Quaternion quat; + Location vehicle_location; + Location target_location; + // if poi available, use it and start tracking, otherwise give warning, stop poi retrieval attempts + if (get_poi(_instance, quat, vehicle_location, target_location)) { + set_roi_target(target_location); + mnt_target.poi_start_ms = 0; + } else if (AP_HAL::millis() - mnt_target.poi_start_ms > 5000) { + // timeout + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "POI: Failed to find poi"); + mnt_target.poi_start_ms = 0; + } +} + +#endif // AP_MOUNT_POI_LOCK_ENABLED + // set yaw lock - sets the _yaw_lock variable and captures current earth frame heading of mount for targeting in RC Targeting mode void AP_Mount_Backend::set_yaw_lock(bool yaw_lock) { @@ -540,6 +600,7 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) LOG_PACKET_HEADER_INIT(static_cast(LOG_MOUNT_MSG)), time_us : (timestamp_us > 0) ? timestamp_us : AP_HAL::micros64(), instance : _instance, + mode : static_cast(get_mode()), desired_roll : target_roll, actual_roll : roll, desired_pitch : target_pitch, diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 379fc12ca65..bfcd8d96cd9 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -93,6 +93,17 @@ public: // If false (aka "follow") the gimbal's tilt is maintained in body-frame meaning it will roll with the vehicle void set_roll_lock(bool roll_lock) { _roll_lock = roll_lock; } +#if AP_MOUNT_POI_LOCK_ENABLED + // set poi_lock to switch to GPS Targeting mode using current GPS point in gimbal's view or current saved poi and save entry mode for suspend function + void set_poi_lock(); + // clears poi_lock and reverts to default targeting mode + void clear_poi_lock(); + // reverts to saved poi entry mode but maintains poi location if set_poi_lock is called again without clearing it + void suspend_poi_lock(); + // check that poi_target has been set + bool roi_is_set() { return !_roi_target.is_zero(); } +#endif // AP_MOUNT_POI_LOCK_ENABLED + // set angle target in degrees // roll and pitch are in earth-frame // yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame @@ -381,6 +392,7 @@ protected: MountAngleTarget angle_rad; // angle target in radians MountRateTarget rate_rads; // rate target in rad/s uint32_t last_rate_request_ms; + uint32_t poi_start_ms; // time we started trying to find the gimbal POI for an AuxFunc::MOUNT_POI_LOCK } mnt_target; // RP earth frame locks accessible by backend @@ -403,7 +415,7 @@ private: #endif bool _yaw_lock; // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame - + float _yaw_lock_heading_rad; // mount earth frame direction captured upon calling set_yaw_lock #if AP_MOUNT_POI_TO_LATLONALT_ENABLED @@ -419,6 +431,14 @@ private: Location _roi_target; // roi target location +#if AP_MOUNT_POI_LOCK_ENABLED + void update_poi_lock_target(); + + // mount mode saved here entering poi lock for + // switching poi lock back to previous mode with aux function middle position + MAV_MOUNT_MODE saved_mount_mode = MAV_MOUNT_MODE_ENUM_END; +#endif // AP_MOUNT_POI_LOCK_ENABLED + uint8_t _target_sysid; // sysid to track Location _target_sysid_location;// sysid target location diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h index 5519a624684..377b3e30400 100644 --- a/libraries/AP_Mount/AP_Mount_config.h +++ b/libraries/AP_Mount/AP_Mount_config.h @@ -62,8 +62,12 @@ #define HAL_MOUNT_XFROBOT_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 #endif +#ifndef AP_MOUNT_POI_LOCK_ENABLED +#define AP_MOUNT_POI_LOCK_ENABLED 0 +#endif + #ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED -#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && HAL_PROGRAM_SIZE_LIMIT_KB > 1024 +#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && (HAL_PROGRAM_SIZE_LIMIT_KB > 1024 || AP_MOUNT_POI_LOCK_ENABLED) #endif #ifndef HAL_MOUNT_TOPOTEK_ENABLED diff --git a/libraries/AP_Mount/LogStructure.h b/libraries/AP_Mount/LogStructure.h index 74289460611..c27bf2ee301 100644 --- a/libraries/AP_Mount/LogStructure.h +++ b/libraries/AP_Mount/LogStructure.h @@ -10,6 +10,7 @@ // @Description: Mount's desired and actual roll, pitch and yaw angles // @Field: TimeUS: Time since system startup // @Field: I: Instance number +// @Field: Mode: Mount mode // @Field: DRoll: Desired roll // @Field: Roll: Actual roll // @Field: DPitch: Desired pitch @@ -24,6 +25,7 @@ struct PACKED log_Mount { LOG_PACKET_HEADER; uint64_t time_us; uint8_t instance; + uint8_t mode; float desired_roll; float actual_roll; float desired_pitch; @@ -38,7 +40,7 @@ struct PACKED log_Mount { #if HAL_MOUNT_ENABLED #define LOG_STRUCTURE_FROM_MOUNT \ { LOG_MOUNT_MSG, sizeof(log_Mount), \ - "MNT", "QBfffffffff","TimeUS,I,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#ddddddddm", "F---------0" }, + "MNT", "QBBfffffffff","TimeUS,I,Mode,DRoll,Roll,DPitch,Pitch,DYawB,YawB,DYawE,YawE,Dist", "s#-ddddddddm", "F----------0" }, #else #define LOG_STRUCTURE_FROM_MOUNT #endif