Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: RC targeting mode lock state from previous mode

Browse Source 
This leads to smoother transitions between RC and GCS control
This commit is contained in:
Randy Mackay 2024-03-19 19:32:45 +09:00
parent 0f4885e507
commit 0387a66f39
4 changed files with 34 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -91,6 +91,11 @@ void AP_Mount_Backend::set_angle_target(float roll_deg, float pitch_deg, float y
// set the mode to mavlink targeting
set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
// optionally set RC_TARGETING yaw lock state
if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
set_yaw_lock(yaw_is_earth_frame);
}
}
// sets rate target in deg/s
@ -106,6 +111,11 @@ void AP_Mount_Backend::set_rate_target(float roll_degs, float pitch_degs, float
// set the mode to mavlink targeting
set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING);
// optionally set RC_TARGETING yaw lock state
if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
set_yaw_lock(yaw_is_earth_frame);
}
}
// set_roi_target - sets target location that mount should attempt to point towards
@ -117,6 +127,11 @@ void AP_Mount_Backend::set_roi_target(const Location &target_loc)
// set the mode to GPS tracking mode
set_mode(MAV_MOUNT_MODE_GPS_POINT);
// optionally set RC_TARGETING yaw lock state
if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
set_yaw_lock(true);
}
}
// clear_roi_target - clears target location that mount should attempt to point towards
@ -139,6 +154,11 @@ void AP_Mount_Backend::set_target_sysid(uint8_t sysid)
// set the mode to sysid tracking mode
set_mode(MAV_MOUNT_MODE_SYSID_TARGET);
// optionally set RC_TARGETING yaw lock state
if (option_set(Options::RCTARGETING_LOCK_FROM_PREVMODE)) {
set_yaw_lock(true);
}
}
#if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED

6
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -228,6 +228,12 @@ protected:
void set(const Vector3f& rpy, bool yaw_is_ef_in);
};
// options parameter bitmask handling
enum class Options : uint8_t {
RCTARGETING_LOCK_FROM_PREVMODE = (1U << 0), // RC_TARGETING mode's lock/follow state maintained from previous mode
};
bool option_set(Options opt) const { return (_params.options.get() & (uint8_t)opt) != 0; }
// returns true if user has configured a valid yaw angle range
// allows user to disable yaw even on 3-axis gimbal
bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }

7
libraries/AP_Mount/AP_Mount_Params.cpp
View File

@ -165,6 +165,13 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
// @User: Advanced
AP_GROUPINFO_FLAGS("_DEVID", 15, AP_Mount_Params, dev_id, 0, AP_PARAM_FLAG_INTERNAL_USE_ONLY),
// @Param: _OPTIONS
// @DisplayName: Mount options
// @Description: Mount options bitmask
// @Bitmask: 0:RC lock state from previous mode
// @User: Standard
AP_GROUPINFO("_OPTIONS", 16, AP_Mount_Params, options, 0),
AP_GROUPEND
};

1
libraries/AP_Mount/AP_Mount_Params.h
View File

@ -31,4 +31,5 @@ public:
AP_Float pitch_stb_lead; // pitch lead control gain (only used by servo backend)
AP_Int8 sysid_default; // target sysid for mount to follow
AP_Int32 dev_id; // Device id taking into account bus
AP_Int8 options; // mount options bitmask
};