From 60c6f8ad6088a6de974ff96d4e766824f5d1ad6f Mon Sep 17 00:00:00 2001 From: Rob Ratcliff Date: Mon, 26 Jun 2017 17:01:50 -0500 Subject: [PATCH] AP_GPS: fix bug when injecting messages bigger than 255 bytes Commit 5261654 changed the used injection method so change this method signature to use uint16 for message length like previous injection method did --- libraries/AP_GPS/AP_GPS.cpp | 4 ++-- libraries/AP_GPS/AP_GPS.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index a91248c93e..26ae91d0d6 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -815,7 +815,7 @@ void AP_GPS::lock_port(uint8_t instance, bool lock) } // Inject a packet of raw binary to a GPS -void AP_GPS::inject_data(uint8_t *data, uint8_t len) +void AP_GPS::inject_data(uint8_t *data, uint16_t len) { //Support broadcasting to all GPSes. if (_inject_to == GPS_RTK_INJECT_TO_ALL) { @@ -827,7 +827,7 @@ void AP_GPS::inject_data(uint8_t *data, uint8_t len) } } -void AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint8_t len) +void AP_GPS::inject_data(uint8_t instance, uint8_t *data, uint16_t len) { if (instance < GPS_MAX_RECEIVERS && drivers[instance] != nullptr) { drivers[instance]->inject_data(data, len); diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 953d186459..1faffb14d7 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -328,8 +328,8 @@ public: void lock_port(uint8_t instance, bool locked); //Inject a packet of raw binary to a GPS - void inject_data(uint8_t *data, uint8_t len); - void inject_data(uint8_t instance, uint8_t *data, uint8_t len); + void inject_data(uint8_t *data, uint16_t len); + void inject_data(uint8_t instance, uint8_t *data, uint16_t len); //MAVLink Status Sending void send_mavlink_gps_raw(mavlink_channel_t chan);