diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index b6e7543ec01ce..6170ce0096e72 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -249,7 +249,7 @@ void AP_Mount::set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode) backend->set_mode(mode); } -// set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) +// set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle void AP_Mount::set_yaw_lock(uint8_t instance, bool yaw_lock) { @@ -358,6 +358,12 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com return MAV_RESULT_ACCEPTED; } + // if neither angles nor rates were provided set the RC_TARGETING yaw lock state + if (isnan(pitch_angle_deg) && isnan(yaw_angle_deg) && isnan(pitch_rate_degs) && isnan(yaw_rate_degs)) { + backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return MAV_RESULT_ACCEPTED; + } + return MAV_RESULT_FAILED; } @@ -498,6 +504,12 @@ void AP_Mount::handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg) backend->set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); return; } + + // if neither angles nor rates were provided set the RC_TARGETING yaw lock state + if (isnan(packet.pitch) && isnan(packet.yaw) && isnan(packet.pitch_rate) && isnan(packet.yaw_rate)) { + backend->set_yaw_lock(flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK); + return; + } } MAV_RESULT AP_Mount::handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 3763a4f822118..7d761f53f8e53 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -151,7 +151,7 @@ class AP_Mount void set_mode_to_default() { set_mode_to_default(_primary); } void set_mode_to_default(uint8_t instance); - // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) + // set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // 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); diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 9e15e719abee3..75eb1bba185ab 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -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 @@ -797,10 +817,45 @@ 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 + GIMBAL_DEVICE_FLAGS_YAW_IN_VEHICLE_FRAME | // yaw angle is always in vehicle-frame + (yaw_lock_state ? GIMBAL_DEVICE_FLAGS_YAW_LOCK : 0); return flags; } diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index f75ea29da5289..f64bde9ec62e5 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -79,7 +79,7 @@ class AP_Mount_Backend // set mount's mode bool set_mode(enum MAV_MOUNT_MODE mode); - // set yaw_lock. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) + // set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North) // 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) { _yaw_lock = yaw_lock; } @@ -228,6 +228,12 @@ class AP_Mount_Backend 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); } @@ -283,7 +289,7 @@ class AP_Mount_Backend uint8_t _instance; // this instance's number MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum) - bool _yaw_lock; // True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame + 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 // structure for MAVLink Targeting angle and rate targets struct { diff --git a/libraries/AP_Mount/AP_Mount_Params.cpp b/libraries/AP_Mount/AP_Mount_Params.cpp index c811afde8418d..57257eb5e4e2b 100644 --- a/libraries/AP_Mount/AP_Mount_Params.cpp +++ b/libraries/AP_Mount/AP_Mount_Params.cpp @@ -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 }; diff --git a/libraries/AP_Mount/AP_Mount_Params.h b/libraries/AP_Mount/AP_Mount_Params.h index a3d7e12b01b6e..ccf42c1fe050c 100644 --- a/libraries/AP_Mount/AP_Mount_Params.h +++ b/libraries/AP_Mount/AP_Mount_Params.h @@ -31,4 +31,5 @@ class AP_Mount_Params { 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 };