|
|
|
@ -3928,22 +3928,26 @@ void GCS_MAVLINK::send_mount_status() const
@@ -3928,22 +3928,26 @@ void GCS_MAVLINK::send_mount_status() const
|
|
|
|
|
void GCS_MAVLINK::send_set_position_target_global_int(uint8_t target_system, uint8_t target_component, const Location& loc) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
uint16_t type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | \
|
|
|
|
|
POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | \
|
|
|
|
|
POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; |
|
|
|
|
const uint16_t type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | \
|
|
|
|
|
POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | \
|
|
|
|
|
POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; |
|
|
|
|
|
|
|
|
|
uint8_t mav_frame = loc.relative_alt ? MAV_FRAME_GLOBAL_RELATIVE_ALT_INT : MAV_FRAME_GLOBAL_INT; |
|
|
|
|
// convert altitude to relative to home
|
|
|
|
|
int32_t rel_alt; |
|
|
|
|
if (!loc.get_alt_cm(Location::ALT_FRAME_ABOVE_HOME, rel_alt)) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_msg_set_position_target_global_int_send( |
|
|
|
|
chan, |
|
|
|
|
AP_HAL::millis(), |
|
|
|
|
target_system, |
|
|
|
|
target_component, |
|
|
|
|
mav_frame, |
|
|
|
|
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, |
|
|
|
|
type_mask, |
|
|
|
|
loc.lat, |
|
|
|
|
loc.lng, |
|
|
|
|
loc.alt, |
|
|
|
|
rel_alt, |
|
|
|
|
0,0,0, // vx, vy, vz
|
|
|
|
|
0,0,0, // ax, ay, az
|
|
|
|
|
0,0); // yaw, yaw_rate
|
|
|
|
|