Browse Source

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
master
Rob Ratcliff 8 years ago committed by Francisco Ferreira
parent
commit
60c6f8ad60
  1. 4
      libraries/AP_GPS/AP_GPS.cpp
  2. 4
      libraries/AP_GPS/AP_GPS.h

4
libraries/AP_GPS/AP_GPS.cpp

@ -815,7 +815,7 @@ void AP_GPS::lock_port(uint8_t instance, bool lock) @@ -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) @@ -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);

4
libraries/AP_GPS/AP_GPS.h

@ -328,8 +328,8 @@ public: @@ -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);

Loading…
Cancel
Save