Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: support multiple AP_GPS_Auto drivers 

move all static variables into a dynamically allocated structure in
the AUTO driver which gets freed when we have found a GPS type
This commit is contained in:
Andrew Tridgell 2014-03-28 14:50:44 +11:00
parent 3c4389180e
commit 2dd92832dc
17 changed files with 243 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_GPS/AP_GPS.h
View File

@ -5,7 +5,6 @@
#include "AP_GPS_NMEA.h"
#include "AP_GPS_SIRF.h"
#include "AP_GPS_406.h"
#include "AP_GPS_UBLOX.h"
#include "AP_GPS_MTK.h"
#include "AP_GPS_MTK19.h"

90
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -12,21 +12,25 @@ extern const AP_HAL::HAL& hal;
static const uint32_t baudrates[] PROGMEM = {38400U, 57600U, 9600U, 4800U};
const prog_char AP_GPS_Auto::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
const prog_char AP_GPS_Auto::_mtk_set_binary[] PROGMEM = MTK_SET_BINARY;
const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY;
AP_GPS_Auto::AP_GPS_Auto(GPS **gps) :
GPS(),
_gps(gps)
_gps(gps),
state(NULL)
{
}
// Do nothing at init time - it may be too early to try detecting the GPS
//
void
AP_GPS_Auto::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
{
if (state == NULL) {
state = (struct detect_state *)calloc(1, sizeof(*state));
}
_port = s;
_nav_setting = nav_setting;
_DataFlash = DataFlash;
@ -41,26 +45,27 @@ AP_GPS_Auto::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, Da
bool
AP_GPS_Auto::read(void)
{
static uint32_t last_baud_change_ms;
static uint8_t last_baud;
if (state == NULL) {
return false;
}
GPS *gps;
uint32_t now = hal.scheduler->millis();
if (now - last_baud_change_ms > 1200) {
if (now - state->last_baud_change_ms > 1200) {
// its been more than 1.2 seconds without detection on this
// GPS - switch to another baud rate
_baudrate = pgm_read_dword(&baudrates[last_baud]);
_baudrate = pgm_read_dword(&baudrates[state->last_baud]);
//hal.console->printf_P(PSTR("Setting GPS baudrate %u\n"), (unsigned)_baudrate);
_port->begin(_baudrate, 256, 16);
last_baud++;
last_baud_change_ms = now;
if (last_baud == sizeof(baudrates) / sizeof(baudrates[0])) {
last_baud = 0;
state->last_baud++;
state->last_baud_change_ms = now;
if (state->last_baud == sizeof(baudrates) / sizeof(baudrates[0])) {
state->last_baud = 0;
}
// write config strings for the types of GPS we support
_send_progstr(_port, _mtk_set_binary, sizeof(_mtk_set_binary));
_send_progstr(_port, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
_send_progstr(_port, _sirf_set_binary, sizeof(_sirf_set_binary));
_send_progstr(_mtk_set_binary, sizeof(_mtk_set_binary));
_send_progstr(_ublox_set_binary, sizeof(_ublox_set_binary));
_send_progstr(_sirf_set_binary, sizeof(_sirf_set_binary));
}
_update_progstr();
@ -69,6 +74,8 @@ AP_GPS_Auto::read(void)
// configure the detected GPS
gps->init(_port, _nav_setting, _DataFlash);
hal.console->println_P(PSTR("OK"));
free(state);
state = NULL;
*_gps = gps;
return true;
}
@ -81,11 +88,10 @@ AP_GPS_Auto::read(void)
GPS *
AP_GPS_Auto::_detect(void)
{
static uint32_t detect_started_ms;
GPS *new_gps = NULL;
if (detect_started_ms == 0 && _port->available() > 0) {
detect_started_ms = hal.scheduler->millis();
if (state->detect_started_ms == 0 && _port->available() > 0) {
state->detect_started_ms = hal.scheduler->millis();
}
while (_port->available() > 0 && new_gps == NULL) {
@ -97,28 +103,28 @@ AP_GPS_Auto::_detect(void)
the uBlox into 38400 no matter what rate it is configured
for.
*/
if (_baudrate >= 38400 && AP_GPS_UBLOX::_detect(data)) {
if (_baudrate >= 38400 && AP_GPS_UBLOX::_detect(state->ublox_detect_state, data)) {
hal.console->print_P(PSTR(" ublox "));
new_gps = new AP_GPS_UBLOX();
}
else if (AP_GPS_MTK19::_detect(data)) {
else if (AP_GPS_MTK19::_detect(state->mtk19_detect_state, data)) {
hal.console->print_P(PSTR(" MTK19 "));
new_gps = new AP_GPS_MTK19();
}
else if (AP_GPS_MTK::_detect(data)) {
else if (AP_GPS_MTK::_detect(state->mtk_detect_state, data)) {
hal.console->print_P(PSTR(" MTK "));
new_gps = new AP_GPS_MTK();
}
#if !defined( __AVR_ATmega1280__ )
// save a bit of code space on a 1280
else if (AP_GPS_SIRF::_detect(data)) {
else if (AP_GPS_SIRF::_detect(state->sirf_detect_state, data)) {
hal.console->print_P(PSTR(" SIRF "));
new_gps = new AP_GPS_SIRF();
}
else if (hal.scheduler->millis() - detect_started_ms > 5000) {
else if (hal.scheduler->millis() - state->detect_started_ms > 5000) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
if (AP_GPS_NMEA::_detect(data)) {
if (AP_GPS_NMEA::_detect(state->nmea_detect_state, data)) {
hal.console->print_P(PSTR(" NMEA "));
new_gps = new AP_GPS_NMEA();
}
@ -134,5 +140,45 @@ AP_GPS_Auto::_detect(void)
}
/*
a prog_char block queue, used to send out config commands to a GPS
in 16 byte chunks. This saves us having to have a 128 byte GPS send
buffer, while allowing us to avoid a long delay in sending GPS init
strings while waiting for the GPS auto detection to happen
*/
void AP_GPS_Auto::_send_progstr(const prog_char *pstr, uint8_t size)
{
struct progstr_queue *q = &state->progstr_state.queue[state->progstr_state.next_idx];
q->pstr = pstr;
q->size = size;
q->ofs = 0;
state->progstr_state.next_idx++;
if (state->progstr_state.next_idx == PROGSTR_QUEUE_SIZE) {
state->progstr_state.next_idx = 0;
}
}
void AP_GPS_Auto::_update_progstr(void)
{
struct progstr_queue *q = &state->progstr_state.queue[state->progstr_state.idx];
// quick return if nothing to do
if (q->size == 0 || _port->tx_pending()) {
return;
}
uint8_t nbytes = q->size - q->ofs;
if (nbytes > 16) {
nbytes = 16;
}
//hal.console->printf_P(PSTR("writing %u bytes\n"), (unsigned)nbytes);
_write_progstr_block(_port, q->pstr+q->ofs, nbytes);
q->ofs += nbytes;
if (q->ofs == q->size) {
q->size = 0;
state->progstr_state.idx++;
if (state->progstr_state.idx == PROGSTR_QUEUE_SIZE) {
state->progstr_state.idx = 0;
}
}
}

34
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -24,12 +24,12 @@ public:
AP_GPS_Auto(GPS **gps);
/// Dummy init routine, does nothing
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
/// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected.
///
virtual bool read(void);
bool read(void);
private:
/// global GPS driver pointer, updated by auto-detection
@ -40,7 +40,37 @@ private:
///
GPS * _detect(void);
static const prog_char _ublox_set_binary[];
static const prog_char _mtk_set_binary[];
static const prog_char _sirf_set_binary[];
void _send_progstr(const prog_char *pstr, uint8_t size);
void _update_progstr(void);
// maximum number of pending progstrings
#define PROGSTR_QUEUE_SIZE 3
struct progstr_queue {
const prog_char *pstr;
uint8_t ofs, size;
};
struct progstr_state {
uint8_t queue_size;
uint8_t idx, next_idx;
struct progstr_queue queue[PROGSTR_QUEUE_SIZE];
};
struct detect_state {
uint32_t detect_started_ms;
uint32_t last_baud_change_ms;
uint8_t last_baud;
struct progstr_state progstr_state;
AP_GPS_UBLOX::detect_state ublox_detect_state;
AP_GPS_MTK::detect_state mtk_detect_state;
AP_GPS_MTK19::detect_state mtk19_detect_state;
AP_GPS_SIRF::detect_state sirf_detect_state;
AP_GPS_NMEA::detect_state nmea_detect_state;
} *state;
};
#endif

6
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -32,8 +32,8 @@ public:
_updated(false)
{}
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
virtual bool read(void);
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read(void);
/**
* Hardware in the loop set function
@ -45,7 +45,7 @@ public:
* @param speed_3d - ground speed in meters/second
* @param altitude - altitude in meters
*/
virtual void setHIL(Fix_Status fix_status,
void setHIL(Fix_Status fix_status,
uint64_t time_epoch_ms, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);

46
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -175,55 +175,51 @@ restart:
detect a MTK GPS
*/
bool
AP_GPS_MTK::_detect(uint8_t data)
AP_GPS_MTK::_detect(struct detect_state &state, uint8_t data)
{
static uint8_t payload_counter;
static uint8_t step;
static uint8_t ck_a, ck_b;
switch(step) {
switch (state.step) {
case 1:
if (PREAMBLE2 == data) {
step++;
state.step++;
break;
}
step = 0;
state.step = 0;
case 0:
ck_b = ck_a = payload_counter = 0;
state.ck_b = state.ck_a = state.payload_counter = 0;
if(PREAMBLE1 == data)
step++;
state.step++;
break;
case 2:
if (MESSAGE_CLASS == data) {
step++;
ck_b = ck_a = data;
state.step++;
state.ck_b = state.ck_a = data;
} else {
step = 0;
state.step = 0;
}
break;
case 3:
if (MESSAGE_ID == data) {
step++;
ck_b += (ck_a += data);
payload_counter = 0;
state.step++;
state.ck_b += (state.ck_a += data);
state.payload_counter = 0;
} else {
step = 0;
state.step = 0;
}
break;
case 4:
ck_b += (ck_a += data);
if (++payload_counter == sizeof(struct diyd_mtk_msg))
step++;
state.ck_b += (state.ck_a += data);
if (++state.payload_counter == sizeof(struct diyd_mtk_msg))
state.step++;
break;
case 5:
step++;
if (ck_a != data) {
step = 0;
state.step++;
if (state.ck_a != data) {
state.step = 0;
}
break;
case 6:
step = 0;
if (ck_b == data) {
state.step = 0;
if (state.ck_b == data) {
return true;
}
}

13
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -37,9 +37,16 @@ public:
_payload_counter(0)
{}
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
virtual bool read(void);
static bool _detect(uint8_t );
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read(void);
struct detect_state {
uint8_t payload_counter;
uint8_t step;
uint8_t ck_a, ck_b;
};
static bool _detect(struct detect_state &state, uint8_t data);
private:
struct PACKED diyd_mtk_msg {

38
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -205,52 +205,48 @@ restart:
detect a MTK16 or MTK19 GPS
*/
bool
AP_GPS_MTK19::_detect(uint8_t data)
AP_GPS_MTK19::_detect(struct detect_state &state, uint8_t data)
{
static uint8_t payload_counter;
static uint8_t step;
static uint8_t ck_a, ck_b;
restart:
switch (step) {
switch (state.step) {
case 0:
ck_b = ck_a = payload_counter = 0;
state.ck_b = state.ck_a = state.payload_counter = 0;
if (data == PREAMBLE1_V16 || data == PREAMBLE1_V19) {
step++;
state.step++;
}
break;
case 1:
if (PREAMBLE2 == data) {
step++;
state.step++;
} else {
step = 0;
state.step = 0;
goto restart;
}
break;
case 2:
if (data == sizeof(struct diyd_mtk_msg)) {
step++;
ck_b = ck_a = data;
state.step++;
state.ck_b = state.ck_a = data;
} else {
step = 0;
state.step = 0;
goto restart;
}
break;
case 3:
ck_b += (ck_a += data);
if (++payload_counter == sizeof(struct diyd_mtk_msg))
step++;
state.ck_b += (state.ck_a += data);
if (++state.payload_counter == sizeof(struct diyd_mtk_msg))
state.step++;
break;
case 4:
step++;
if (ck_a != data) {
step = 0;
state.step++;
if (state.ck_a != data) {
state.step = 0;
goto restart;
}
break;
case 5:
step = 0;
if (ck_b != data) {
state.step = 0;
if (state.ck_b != data) {
goto restart;
}
return true;

13
libraries/AP_GPS/AP_GPS_MTK19.h
View File

@ -41,9 +41,16 @@ public:
_fix_counter(0)
{}
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
virtual bool read(void);
static bool _detect(uint8_t );
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read(void);
struct detect_state {
uint8_t payload_counter;
uint8_t step;
uint8_t ck_a, ck_b;
};
static bool _detect(struct detect_state &state, uint8_t data);
private:
struct PACKED diyd_mtk_msg {

25
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -382,37 +382,34 @@ bool AP_GPS_NMEA::_term_complete()
matches a NMEA string
*/
bool
AP_GPS_NMEA::_detect(uint8_t data)
AP_GPS_NMEA::_detect(struct detect_state &state, uint8_t data)
{
static uint8_t step;
static uint8_t ck;
switch (step) {
switch (state.step) {
case 0:
ck = 0;
state.ck = 0;
if ('$' == data) {
step++;
state.step++;
}
break;
case 1:
if ('*' == data) {
step++;
state.step++;
} else {
ck ^= data;
state.ck ^= data;
}
break;
case 2:
if (hexdigit(ck>>4) == data) {
step++;
if (hexdigit(state.ck>>4) == data) {
state.step++;
} else {
step = 0;
state.step = 0;
}
break;
case 3:
if (hexdigit(ck&0xF) == data) {
if (hexdigit(state.ck&0xF) == data) {
return true;
}
step = 0;
state.step = 0;
break;
}
return false;

10
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -70,15 +70,19 @@ public:
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state
/// accordingly.
///
virtual bool read();
bool read();
static bool _detect(uint8_t data);
struct detect_state {
uint8_t step;
uint8_t ck;
};
static bool _detect(struct detect_state &state, uint8_t data);
private:
/// Coding for the GPS sentences that the parser handles

4
libraries/AP_GPS/AP_GPS_None.h
View File

@ -11,10 +11,10 @@ class AP_GPS_None : public GPS
public:
AP_GPS_None() : GPS() {}
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash) {
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash) {
_port = s;
};
virtual bool read(void) {
bool read(void) {
return false;
};
};

39
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -214,48 +214,45 @@ AP_GPS_SIRF::_accumulate(uint8_t val)
detect a SIRF GPS
*/
bool
AP_GPS_SIRF::_detect(uint8_t data)
AP_GPS_SIRF::_detect(struct detect_state &state, uint8_t data)
{
static uint16_t checksum;
static uint8_t step, payload_length, payload_counter;
switch (step) {
switch (state.step) {
case 1:
if (PREAMBLE2 == data) {
step++;
state.step++;
break;
}
step = 0;
state.step = 0;
case 0:
payload_length = payload_counter = checksum = 0;
state.payload_length = state.payload_counter = state.checksum = 0;
if (PREAMBLE1 == data)
step++;
state.step++;
break;
case 2:
step++;
state.step++;
if (data != 0) {
// only look for short messages
step = 0;
state.step = 0;
}
break;
case 3:
step++;
payload_length = data;
state.step++;
state.payload_length = data;
break;
case 4:
checksum = (checksum + data) & 0x7fff;
if (++payload_counter == payload_length)
step++;
state.checksum = (state.checksum + data) & 0x7fff;
if (++state.payload_counter == state.payload_length)
state.step++;
break;
case 5:
step++;
if ((checksum >> 8) != data) {
step = 0;
state.step++;
if ((state.checksum >> 8) != data) {
state.step = 0;
}
break;
case 6:
step = 0;
if ((checksum & 0xff) == data) {
state.step = 0;
if ((state.checksum & 0xff) == data) {
return true;
}
}

12
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -38,9 +38,15 @@ public:
_msg_id(0)
{}
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
virtual bool read();
static bool _detect(uint8_t data);
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read();
struct detect_state {
uint16_t checksum;
uint8_t step, payload_length, payload_counter;
};
static bool _detect(struct detect_state &state, uint8_t data);
private:
struct PACKED sirf_geonav {

64
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -48,9 +48,6 @@ extern const AP_HAL::HAL& hal;
#define UBLOX_HW_LOGGING 0
#endif
const prog_char AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary);
// Public Methods //////////////////////////////////////////////////////////////
void
@ -582,17 +579,6 @@ AP_GPS_UBLOX::_configure_navigation_rate(uint16_t rate_ms)
void
AP_GPS_UBLOX::_configure_gps(void)
{
const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U};
// the GPS may be setup for a different baud rate. This ensures
// it gets configured correctly
for (uint8_t i=0; i<4; i++) {
_port->begin(baudrates[i]);
_write_progstr_block(_port, _ublox_set_binary, _ublox_set_binary_size);
while (_port->tx_pending()) {
hal.scheduler->delay(1);
}
}
_port->begin(38400U);
// start the process of updating the GPS rates
@ -611,57 +597,53 @@ AP_GPS_UBLOX::_configure_gps(void)
matches a UBlox
*/
bool
AP_GPS_UBLOX::_detect(uint8_t data)
AP_GPS_UBLOX::_detect(struct detect_state &state, uint8_t data)
{
static uint8_t payload_length, payload_counter;
static uint8_t step;
static uint8_t ck_a, ck_b;
reset:
switch (step) {
switch (state.step) {
case 1:
if (PREAMBLE2 == data) {
step++;
state.step++;
break;
}
step = 0;
state.step = 0;
case 0:
if (PREAMBLE1 == data)
step++;
state.step++;
break;
case 2:
step++;
ck_b = ck_a = data;
state.step++;
state.ck_b = state.ck_a = data;
break;
case 3:
step++;
ck_b += (ck_a += data);
state.step++;
state.ck_b += (state.ck_a += data);
break;
case 4:
step++;
ck_b += (ck_a += data);
payload_length = data;
state.step++;
state.ck_b += (state.ck_a += data);
state.payload_length = data;
break;
case 5:
step++;
ck_b += (ck_a += data);
payload_counter = 0;
state.step++;
state.ck_b += (state.ck_a += data);
state.payload_counter = 0;
break;
case 6:
ck_b += (ck_a += data);
if (++payload_counter == payload_length)
step++;
state.ck_b += (state.ck_a += data);
if (++state.payload_counter == state.payload_length)
state.step++;
break;
case 7:
step++;
if (ck_a != data) {
step = 0;
state.step++;
if (state.ck_a != data) {
state.step = 0;
goto reset;
}
break;
case 8:
step = 0;
if (ck_b == data) {
state.step = 0;
if (state.ck_b == data) {
// a valid UBlox packet
return true;
} else {

17
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -58,14 +58,19 @@ public:
{}
// Methods
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
virtual bool read();
static bool _detect(uint8_t );
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read();
static const prog_char _ublox_set_binary[];
static const uint8_t _ublox_set_binary_size;
float get_lag() const { return 0.2f; } // ublox lag is lower than the default 1second
float get_lag() { return 0.5; } // ublox lag is lower than the default 1second
// structure used for gps type detection
struct detect_state {
uint8_t payload_length, payload_counter;
uint8_t step;
uint8_t ck_a, ck_b;
};
static bool _detect(struct detect_state &state, uint8_t data);
private:
// u-blox UBX protocol essentials

58
libraries/AP_GPS/GPS.cpp
View File

@ -136,64 +136,6 @@ void GPS::_write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, u
}
}
/*
a prog_char block queue, used to send out config commands to a GPS
in 16 byte chunks. This saves us having to have a 128 byte GPS send
buffer, while allowing us to avoid a long delay in sending GPS init
strings while waiting for the GPS auto detection to happen
*/
// maximum number of pending progstrings
#define PROGSTR_QUEUE_SIZE 3
struct progstr_queue {
const prog_char *pstr;
uint8_t ofs, size;
};
static struct {
AP_HAL::UARTDriver *fs;
uint8_t queue_size;
uint8_t idx, next_idx;
struct progstr_queue queue[PROGSTR_QUEUE_SIZE];
} progstr_state;
void GPS::_send_progstr(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size)
{
progstr_state.fs = _fs;
struct progstr_queue *q = &progstr_state.queue[progstr_state.next_idx];
q->pstr = pstr;
q->size = size;
q->ofs = 0;
progstr_state.next_idx++;
if (progstr_state.next_idx == PROGSTR_QUEUE_SIZE) {
progstr_state.next_idx = 0;
}
}
void GPS::_update_progstr(void)
{
struct progstr_queue *q = &progstr_state.queue[progstr_state.idx];
// quick return if nothing to do
if (q->size == 0 || progstr_state.fs->tx_pending()) {
return;
}
uint8_t nbytes = q->size - q->ofs;
if (nbytes > 16) {
nbytes = 16;
}
//hal.console->printf_P(PSTR("writing %u bytes\n"), (unsigned)nbytes);
_write_progstr_block(progstr_state.fs, q->pstr+q->ofs, nbytes);
q->ofs += nbytes;
if (q->ofs == q->size) {
q->size = 0;
progstr_state.idx++;
if (progstr_state.idx == PROGSTR_QUEUE_SIZE) {
progstr_state.idx = 0;
}
}
}
int32_t GPS::_swapl(const void *bytes) const
{
const uint8_t *b = (const uint8_t *)bytes;

4
libraries/AP_GPS/GPS.h
View File

@ -128,7 +128,7 @@ public:
}
// the expected lag (in seconds) in the position and velocity readings from the gps
virtual float get_lag() { return 1.0f; }
virtual float get_lag() const { return 0.5f; }
// the time we got our last fix in system milliseconds
uint32_t last_fix_time;
@ -185,8 +185,6 @@ protected:
enum GPS_Engine_Setting _nav_setting;
void _write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size);
void _send_progstr(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size);
void _update_progstr(void);
// velocities in cm/s if available from the GPS
int32_t _vel_north;