Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

GPS: avoid output delays in the GPS auto detection

Browse Source 
this sends out the GPS init strings in 16 byte chunks to avoid causing
timing glitches when no GPS is attached
This commit is contained in:
Andrew Tridgell 2012-10-18 16:18:12 +11:00
parent 78e88994e1
commit 890bed4918
3 changed files with 67 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -54,11 +54,13 @@ AP_GPS_Auto::read(void)
last_baud = 0;
}
// write config strings for the types of GPS we support
_write_progstr_block(_fs, _mtk_set_binary, sizeof(_mtk_set_binary));
_write_progstr_block(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
_write_progstr_block(_fs, _sirf_set_binary, sizeof(_sirf_set_binary));
_send_progstr(_fs, _mtk_set_binary, sizeof(_mtk_set_binary));
_send_progstr(_fs, AP_GPS_UBLOX::_ublox_set_binary, AP_GPS_UBLOX::_ublox_set_binary_size);
_send_progstr(_fs, _sirf_set_binary, sizeof(_sirf_set_binary));
}
_update_progstr();
if (NULL != (gps = _detect())) {
// configure the detected GPS
gps->init(_nav_setting);
@ -115,3 +117,6 @@ AP_GPS_Auto::_detect(void)
return NULL;
}

57
libraries/AP_GPS/GPS.cpp
View File

@ -96,3 +96,60 @@ void GPS::_write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size)
_fs->write(pgm_read_byte(pstr++));
}
}
/*
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 {
FastSerial *fs;
uint8_t queue_size;
uint8_t idx, next_idx;
struct progstr_queue queue[PROGSTR_QUEUE_SIZE];
} progstr_state;
void GPS::_send_progstr(Stream *_fs, const prog_char *pstr, uint8_t size)
{
progstr_state.fs = (FastSerial *)_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;
}
_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;
}
}
}

2
libraries/AP_GPS/GPS.h
View File

@ -195,6 +195,8 @@ protected:
enum GPS_Engine_Setting _nav_setting;
void _write_progstr_block(Stream *_fs, const prog_char *pstr, uint8_t size);
void _send_progstr(Stream *_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;