diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index a26dfa3fff..8552cce0b6 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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(