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

AP_Mount: get-gimbal-device-flags reports lock state

Browse Source
This commit is contained in:
Randy Mackay 2024-03-20 09:53:11 +09:00 committed by Andrew Tridgell
parent c392089ae7
commit affc1b4eb4
1 changed files with 35 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -817,10 +817,44 @@ void AP_Mount_Backend::update_angle_target_from_rate(const MountTarget& rate_rad
// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
uint16_t AP_Mount_Backend::get_gimbal_device_flags() const
{
// get yaw lock state by mode
bool yaw_lock_state = false;
switch (_mode) {
case MAV_MOUNT_MODE_RETRACT:
case MAV_MOUNT_MODE_NEUTRAL:
// these modes always use body-frame yaw (aka follow)
yaw_lock_state = false;
break;
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
switch (mnt_target.target_type) {
case MountTargetType::RATE:
yaw_lock_state = mnt_target.rate_rads.yaw_is_ef;
break;
case MountTargetType::ANGLE:
yaw_lock_state = mnt_target.angle_rad.yaw_is_ef;
break;
}
break;
case MAV_MOUNT_MODE_RC_TARGETING:
yaw_lock_state = _yaw_lock;
break;
case MAV_MOUNT_MODE_GPS_POINT:
case MAV_MOUNT_MODE_SYSID_TARGET:
case MAV_MOUNT_MODE_HOME_LOCATION:
// these modes always use earth-frame yaw (aka lock)
yaw_lock_state = true;
break;
case MAV_MOUNT_MODE_ENUM_END:
// unsupported
yaw_lock_state = false;
break;
}
const uint16_t flags = (get_mode() == MAV_MOUNT_MODE_RETRACT ? GIMBAL_DEVICE_FLAGS_RETRACT : 0) |
(get_mode() == MAV_MOUNT_MODE_NEUTRAL ? GIMBAL_DEVICE_FLAGS_NEUTRAL : 0) |
GIMBAL_DEVICE_FLAGS_ROLL_LOCK | // roll angle is always earth-frame
GIMBAL_DEVICE_FLAGS_PITCH_LOCK; // pitch angle is always earth-frame, yaw_angle is always body-frame
GIMBAL_DEVICE_FLAGS_PITCH_LOCK| // pitch angle is always earth-frame, yaw_angle is always body-frame
(yaw_lock_state ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0);
return flags;
}