GCS_Mavlink: Add function for sending POSITION_TARGET_LOCAL_NED message

This commit is contained in:
Rajat Singhal 2019-03-28 15:08:08 +05:30 committed by Peter Barker
parent b79993a2f2
commit b7a40f2bfe
2 changed files with 8 additions and 0 deletions

View File

@ -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);

View File

@ -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();