diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fcb494ae5c..997ef8f2ec 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -90,6 +90,7 @@ enum ap_message : uint8_t { MSG_WHEEL_DISTANCE, MSG_MISSION_ITEM_REACHED, MSG_POSITION_TARGET_GLOBAL_INT, + MSG_POSITION_TARGET_LOCAL_NED, MSG_ADSB_VEHICLE, MSG_BATTERY_STATUS, MSG_AOA_SSA, @@ -227,6 +228,7 @@ public: void send_home_position() const; void send_gps_global_origin() const; virtual void send_position_target_global_int() { }; + virtual void send_position_target_local_ned() { }; void send_servo_output_raw(); static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour); void send_accelcal_vehicle_position(uint32_t position); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 1bbde284f3..f6fb46801f 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -950,6 +950,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_RPM, MSG_RPM}, { MAVLINK_MSG_ID_MISSION_ITEM_REACHED, MSG_MISSION_ITEM_REACHED}, { MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, MSG_POSITION_TARGET_GLOBAL_INT}, + { MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, MSG_POSITION_TARGET_LOCAL_NED}, { MAVLINK_MSG_ID_ADSB_VEHICLE, MSG_ADSB_VEHICLE}, { MAVLINK_MSG_ID_BATTERY_STATUS, MSG_BATTERY_STATUS}, { MAVLINK_MSG_ID_AOA_SSA, MSG_AOA_SSA}, @@ -4186,6 +4187,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) send_position_target_global_int(); break; + case MSG_POSITION_TARGET_LOCAL_NED: + CHECK_PAYLOAD_SIZE(POSITION_TARGET_LOCAL_NED); + send_position_target_local_ned(); + break; + case MSG_POWER_STATUS: CHECK_PAYLOAD_SIZE(POWER_STATUS); send_power_status();