|
|
|
@ -130,18 +130,26 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
@@ -130,18 +130,26 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
|
|
|
|
|
const ModeGuided::SubMode guided_mode = copter.mode_guided.submode(); |
|
|
|
|
Vector3f target_pos; |
|
|
|
|
Vector3f target_vel; |
|
|
|
|
uint16_t type_mask; |
|
|
|
|
uint16_t type_mask = 0; |
|
|
|
|
|
|
|
|
|
if (guided_mode == ModeGuided::SubMode::WP) { |
|
|
|
|
switch (guided_mode) { |
|
|
|
|
case ModeGuided::SubMode::Angle: |
|
|
|
|
// we don't have a local target when in angle mode
|
|
|
|
|
return; |
|
|
|
|
case ModeGuided::SubMode::WP: |
|
|
|
|
type_mask = 0x0FF8; // ignore everything except position
|
|
|
|
|
target_pos = copter.wp_nav->get_wp_destination() * 0.01f; // convert to metres
|
|
|
|
|
} else if (guided_mode == ModeGuided::SubMode::Velocity) { |
|
|
|
|
break; |
|
|
|
|
case ModeGuided::SubMode::Velocity: |
|
|
|
|
type_mask = 0x0FC7; // ignore everything except velocity
|
|
|
|
|
target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s
|
|
|
|
|
} else { |
|
|
|
|
break; |
|
|
|
|
case ModeGuided::SubMode::TakeOff: |
|
|
|
|
case ModeGuided::SubMode::PosVel: |
|
|
|
|
type_mask = 0x0FC0; // 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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
mavlink_msg_position_target_local_ned_send( |
|
|
|
|