diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 38ab91b5c0..a224b64603 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -263,8 +263,6 @@ protected: void handle_radio_status(mavlink_message_t *msg, DataFlash_Class &dataflash, bool log_radio); void handle_serial_control(mavlink_message_t *msg, AP_GPS &gps); - void handle_gps_inject(const mavlink_message_t *msg, AP_GPS &gps); - void handle_common_message(mavlink_message_t *msg); void handle_setup_signing(const mavlink_message_t *msg); uint8_t handle_preflight_reboot(const mavlink_command_long_t &packet, bool disable_overrides); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a298297be2..2158d84b6d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -755,17 +755,6 @@ mission_ack: return mission_is_complete; } -void -GCS_MAVLINK::handle_gps_inject(const mavlink_message_t *msg, AP_GPS &gps) -{ - mavlink_gps_inject_data_t packet; - mavlink_msg_gps_inject_data_decode(msg, &packet); - //TODO: check target - - gps.inject_data(packet.data, packet.len); - -} - // send a message using mavlink, handling message queueing void GCS_MAVLINK::send_message(enum ap_message id) { @@ -1816,6 +1805,8 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_GPS_INPUT: /* fall through */ case MAVLINK_MSG_ID_HIL_GPS: + /* fall through */ + case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_common_gps_message(msg); break;