diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 72dc05f7a5bcf..42a93071110d1 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -327,6 +327,8 @@ void AP_Mount_Backend::clear_roi_target() MAV_MOUNT_MODE default_mode = (MAV_MOUNT_MODE)_params.default_mode.get(); set_mode(default_mode); } + + clear_roi_pending = true; } // set_sys_target - sets system that mount should attempt to point towards @@ -942,6 +944,9 @@ uint16_t AP_Mount_Backend::get_gimbal_device_flags() const case MountTargetType::NEUTRAL: yaw_lock_state = false; // not locked onto the scenery break; + case MountTargetType::LOCATION: + yaw_lock_state = true; + break; } break; case MAV_MOUNT_MODE_RC_TARGETING: @@ -1045,9 +1050,7 @@ void AP_Mount_Backend::_update_mnt_target() case MAV_MOUNT_MODE_GPS_POINT: // point mount to a GPS point given by the mission planner - if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.target_type = MountTargetType::ANGLE; - } + mnt_target.target_type = MountTargetType::LOCATION; return; case MAV_MOUNT_MODE_HOME_LOCATION: @@ -1072,6 +1075,13 @@ void AP_Mount_Backend::_update_mnt_target() void AP_Mount_Backend::send_target_to_gimbal() { + // process any pending clear-roi-target + // it is assumed that we have already zeroed _roi_target + if (clear_roi_pending && natively_supports(MountTargetType::LOCATION)) { + clear_roi_pending = false; + send_target_location(_roi_target); + } + // the easy case, where the gimbal natively supports the MntTargetType: if (natively_supports(mnt_target.target_type)) { switch (mnt_target.target_type) { @@ -1087,6 +1097,9 @@ void AP_Mount_Backend::send_target_to_gimbal() case MountTargetType::NEUTRAL: send_target_neutral(); return; + case MountTargetType::LOCATION: + send_target_location(_roi_target); + return; } return; // should not reach this as all cases return } @@ -1127,6 +1140,14 @@ void AP_Mount_Backend::send_target_to_gimbal() return; } break; + case MountTargetType::LOCATION: + if (natively_supports(MountTargetType::ANGLE)) { + if (get_angle_target_to_roi(mnt_target.angle_rad)) { + send_target_angles(mnt_target.angle_rad); + } + return; + } + break; } send_warning_to_GCS("Failed to convert mount target to command gimbal"); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index bfcd8d96cd9c3..90abb08d494cd 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -243,6 +243,7 @@ class AP_Mount_Backend RATE = 1, RETRACTED = 2, NEUTRAL = 3, + LOCATION = 4, }; // class for a single angle or rate target @@ -306,6 +307,7 @@ class AP_Mount_Backend virtual void send_target_rates(const MountRateTarget &rate_rads) { } virtual void send_target_retracted() { } virtual void send_target_neutral() { } + virtual void send_target_location(const Location &roi_loc) { } // options parameter bitmask handling enum class Options : uint8_t { @@ -398,7 +400,8 @@ class AP_Mount_Backend // RP earth frame locks accessible by backend bool _pitch_lock = true; // pitch_lock used in RC_TARGETING mode. True if the gimbal's tilt target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame bool _roll_lock = true; // roll_lock used in RC_TARGETING mode. True if the gimbal's roll target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame - + + bool clear_roi_pending; // True if there is a pending clear ROI request private: diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 909979eae082b..618bb7ffcfc4b 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/libraries/AP_Mount/AP_Mount_MAVLink.cpp @@ -136,6 +136,9 @@ void AP_Mount_MAVLink::handle_gimbal_device_information(const mavlink_message_t const uint8_t fw_ver_revision = (info.firmware_version & 0x00FF0000) >> 16; const uint8_t fw_ver_build = (info.firmware_version & 0xFF000000) >> 24; + strncpy(vendor_name, info.vendor_name, ARRAY_SIZE(vendor_name)); + strncpy(model_name, info.model_name, ARRAY_SIZE(model_name)); + // display gimbal info to user GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Mount: %s %s fw:%u.%u.%u.%u", info.vendor_name, @@ -277,4 +280,28 @@ void AP_Mount_MAVLink::send_target_angles(const MountAngleTarget &angle_rad) _link->send_message(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char*)&pkt); } +// Send MAV_CMD_DO_SET_ROI_LOCATION to gimbal +void AP_Mount_MAVLink::send_target_location(const Location &roi_loc) +{ + if (_link == nullptr) { + return; + } + + mavlink_command_int_t pkt {}; + pkt.target_system = _sysid; + pkt.target_component = _compid; + + if (roi_loc.initialised()) { + pkt.command = MAV_CMD_DO_SET_ROI_LOCATION; + pkt.x = roi_loc.lat, // param5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 + pkt.y = roi_loc.lng, // param6 / local: y position in meters * 1e4, global: longitude in degrees * 10^7 + pkt.z = roi_loc.alt * 0.01f; // param7 / z position: global: altitude in meters (relative or absolute, depending on frame). + pkt.frame = (uint8_t)roi_loc.get_alt_frame(); + } else { + pkt.command = MAV_CMD_DO_SET_ROI_NONE; + } + + _link->send_message(MAVLINK_MSG_ID_COMMAND_INT, (const char*)&pkt); +} + #endif // HAL_MOUNT_MAVLINK_ENABLED diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index c9d636db41088..f300d3f827484 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -40,11 +40,19 @@ class AP_Mount_MAVLink : public AP_Mount_Backend // MVAVLink can send either rates or angles, and can also be // directly commanded to move to a retracted state uint8_t natively_supported_mount_target_types() const override { - return ( + + uint8_t supported_target = ( (1U<