Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: use MAVLink POSITION_TARGET_TYPEMASK

This commit is contained in:
Pierre Kancir 2021-03-16 18:28:28 +01:00 committed by Andrew Tridgell
parent fda07e7dc8
commit e0d181a793
1 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/GCS_Mavlink.cpp
View File

@ -101,12 +101,15 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
if (!target.change_alt_frame(Location::AltFrame::ABSOLUTE)) {
return;
}
static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
static constexpr 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_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE;
mavlink_msg_position_target_global_int_send(
chan,
AP_HAL::millis(), // time_boot_ms
MAV_FRAME_GLOBAL, // targets are always global altitude
0xFFF8, // ignore everything except the x/y/z components
TYPE_MASK, // ignore everything except the x/y/z components
target.lat, // latitude as 1e7
target.lng, // longitude as 1e7
target.alt * 0.01f, // altitude is sent as a float
@ -137,16 +140,21 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
// we don't have a local target when in angle mode
return;
case ModeGuided::SubMode::WP:
type_mask = 0x0FF8; // ignore everything except position
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_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position
target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres
break;
case ModeGuided::SubMode::Velocity:
type_mask = 0x0FC7; // ignore everything except velocity
type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE |
POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity
target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s
break;
case ModeGuided::SubMode::TakeOff:
case ModeGuided::SubMode::PosVel:
type_mask = 0x0FC0; // ignore everything except position & velocity
type_mask = POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position & velocity
target_pos = copter.wp_nav->get_wp_destination() * 0.01f;
target_vel = copter.flightmode->get_desired_velocity() * 0.01f;
break;