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

AP_GPS: fixed init strings to all be in progmem 

and sent async ...
This commit is contained in:
Andrew Tridgell 2014-03-31 20:48:22 +11:00
parent bc0c9ad6d5
commit 0191355488
9 changed files with 99 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AP_GPS/AP_GPS.cpp
View File

@ -22,14 +22,6 @@
extern const AP_HAL::HAL& hal;
#define GPS_DEBUGGING 0
#if GPS_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(0); } while(0)
#else
# define Debug(fmt, args ...)
#endif
// table of user settable parameters
const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = {
// @Param: TYPE
@ -70,6 +62,38 @@ const uint16_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 57600U, 9600U};
// right mode
const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
/*
send some more initialisation string bytes if there is room in the
UART transmit buffer
*/
void AP_GPS::send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size)
{
initblob_state[instance].blob = _blob;
initblob_state[instance].remaining = size;
}
/*
send some more initialisation string bytes if there is room in the
UART transmit buffer
*/
void AP_GPS::send_blob_update(uint8_t instance)
{
// see if we can write some more of the initialisation blob
if (initblob_state[instance].remaining > 0) {
AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE;
int16_t space = port->txspace();
if (space > (int16_t)initblob_state[instance].remaining) {
space = initblob_state[instance].remaining;
}
while (space > 0) {
port->write(pgm_read_byte(initblob_state[instance].blob));
initblob_state[instance].blob++;
space--;
initblob_state[instance].remaining--;
}
}
}
/*
run detection step for one GPS instance. If this finds a GPS then it
will fill in drivers[instance] and change state[instance].status
@ -104,22 +128,10 @@ AP_GPS::detect_instance(uint8_t instance)
uint16_t baudrate = pgm_read_word(&_baudrates[dstate->last_baud]);
port->begin(baudrate, 256, 16);
dstate->last_baud_change_ms = now;
dstate->init_blob_offset = 0;
}
// see if we can write some more of the initialisation blob
if (dstate->init_blob_offset < sizeof(_initialisation_blob)) {
int16_t space = port->txspace();
if (space > (int16_t)sizeof(_initialisation_blob) - dstate->init_blob_offset) {
space = sizeof(_initialisation_blob) - dstate->init_blob_offset;
}
while (space > 0) {
port->write(pgm_read_byte(&_initialisation_blob[dstate->init_blob_offset]));
dstate->init_blob_offset++;
space--;
}
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}
send_blob_update(instance);
while (port->available() > 0 && new_gps == NULL) {
uint8_t data = port->read();
@ -174,7 +186,7 @@ AP_GPS::detect_instance(uint8_t instance)
/*
update one GPS instance
update one GPS instance. This should be called at 10Hz or greater
*/
void
AP_GPS::update_instance(uint8_t instance)
@ -191,6 +203,8 @@ AP_GPS::update_instance(uint8_t instance)
return;
}
send_blob_update(instance);
// we have an active driver for this instance
bool result = drivers[instance]->read();
uint32_t tnow = hal.scheduler->millis();
@ -222,6 +236,9 @@ AP_GPS::update(void)
}
}
/*
set HIL (hardware in the loop) status for a GPS instance
*/
void
AP_GPS::setHIL(GPS_Status _status, uint64_t time_epoch_ms,
Location &_location, Vector3f &_velocity, uint8_t _num_sats)

14
libraries/AP_GPS/AP_GPS.h
View File

@ -89,6 +89,10 @@ public:
GPS_ENGINE_AIRBORNE_4G = 8
};
/*
The GPS_State structure is filled in by the backend driver as it
parses each message from the GPS.
*/
struct GPS_State {
uint8_t instance; // the instance number of this GPS
@ -242,6 +246,10 @@ public:
AP_Int8 _type[GPS_MAX_INSTANCES];
AP_Int8 _navfilter;
// handle sending of initialisation strings to the GPS
void send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size);
void send_blob_update(uint8_t instance);
private:
struct GPS_timing {
// the time we got our last fix in system milliseconds
@ -262,7 +270,6 @@ private:
uint32_t detect_started_ms;
uint32_t last_baud_change_ms;
uint8_t last_baud;
uint8_t init_blob_offset;
struct UBLOX_detect_state ublox_detect_state;
struct MTK_detect_state mtk_detect_state;
struct MTK19_detect_state mtk19_detect_state;
@ -270,6 +277,11 @@ private:
struct NMEA_detect_state nmea_detect_state;
} detect_state[GPS_MAX_INSTANCES];
struct {
const prog_char *blob;
uint16_t remaining;
} initblob_state[GPS_MAX_INSTANCES];
static const uint16_t _baudrates[];
static const prog_char _initialisation_blob[];

25
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -24,25 +24,24 @@
#include <AP_GPS.h>
#include "AP_GPS_MTK.h"
// initialisation blobs to send to the GPS to try to get it into the
// right mode
const prog_char AP_GPS_MTK::_initialisation_blob[] PROGMEM = MTK_OUTPUT_5HZ SBAS_ON WAAS_ON MTK_NAVTHRES_OFF;
AP_GPS_MTK::AP_GPS_MTK(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port),
_step(0),
_payload_counter(0)
{
// initialize serial port for binary protocol use
port->print(MTK_SET_BINARY);
gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob));
}
// set 5Hz update rate
port->print(MTK_OUTPUT_5HZ);
// set SBAS on
port->print(SBAS_ON);
// set WAAS on
port->print(WAAS_ON);
// Set Nav Threshold to 0 m/s
port->print(MTK_NAVTHRES_OFF);
/*
send an initialisation blob to configure the GPS
*/
void AP_GPS_MTK::send_init_blob(uint8_t instance, AP_GPS &gps)
{
gps.send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob));
}

3
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -35,6 +35,7 @@ public:
bool read(void);
static bool _detect(struct MTK_detect_state &state, uint8_t data);
static void send_init_blob(uint8_t instance, AP_GPS &gps);
private:
struct PACKED diyd_mtk_msg {
@ -76,6 +77,8 @@ private:
// Buffer parse & GPS state update
void _parse_gps();
static const prog_char _initialisation_blob[];
};
#endif // __AP_GPS_MTK_H__

16
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -34,21 +34,7 @@ AP_GPS_MTK19::AP_GPS_MTK19(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_mtk_revision(0),
_fix_counter(0)
{
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
port->print(MTK_SET_BINARY);
// set 5Hz update rate
port->print(MTK_OUTPUT_5HZ);
// set SBAS on
port->print(SBAS_ON);
// set WAAS on
port->print(WAAS_ON);
// Set Nav Threshold to 0 m/s
port->print(MTK_NAVTHRES_OFF);
AP_GPS_MTK::send_init_blob(_state.instance, _gps);
}
// Process bytes available from the stream

54
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -46,30 +46,28 @@ extern const AP_HAL::HAL& hal;
// for NMEA. GPS_AUTO will try to set any SiRF unit to binary mode as part of
// the autodetection process.
//
const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
"$PSRF103,1,0,0,1*25\r\n" // GLL off
"$PSRF103,2,0,0,1*26\r\n" // GSA off
"$PSRF103,3,0,0,1*27\r\n" // GSV off
"$PSRF103,4,0,1,1*20\r\n" // RMC off
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
"$PSRF103,6,0,0,1*22\r\n" // MSS off
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
"$PSRF106,21*0F\r\n" // datum = WGS84
"";
#define SIRF_INIT_MSG \
"$PSRF103,0,0,1,1*25\r\n" /* GGA @ 1Hz */ \
"$PSRF103,1,0,0,1*25\r\n" /* GLL off */ \
"$PSRF103,2,0,0,1*26\r\n" /* GSA off */ \
"$PSRF103,3,0,0,1*27\r\n" /* GSV off */ \
"$PSRF103,4,0,1,1*20\r\n" /* RMC off */ \
"$PSRF103,5,0,1,1*20\r\n" /* VTG @ 1Hz */ \
"$PSRF103,6,0,0,1*22\r\n" /* MSS off */ \
"$PSRF103,8,0,0,1*2C\r\n" /* ZDA off */ \
"$PSRF151,1*3F\r\n" /* WAAS on (not always supported) */ \
"$PSRF106,21*0F\r\n" /* datum = WGS84 */
// MediaTek init messages //////////////////////////////////////////////////////
//
// Note that we may see a MediaTek in NMEA mode if we are connected to a non-DIYDrones
// MediaTek-based GPS.
//
const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
"$PMTK330,0*2E\r\n" // datum = WGS84
"$PMTK313,1*2E\r\n" // SBAS on
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
"";
#define MTK_INIT_MSG \
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" /* GGA & VTG once every fix */ \
"$PMTK330,0*2E\r\n" /* datum = WGS84 */ \
"$PMTK313,1*2E\r\n" /* SBAS on */ \
"$PMTK301,2*2E\r\n" /* use SBAS data for DGPS */
// ublox init messages /////////////////////////////////////////////////////////
//
@ -80,11 +78,12 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
// We don't attempt to send $PUBX,41 as the unit must already be talking NMEA
// and we don't know the baudrate.
//
const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM =
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
"";
#define UBLOX_INIT_MSG \
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" /* GGA on at one per fix */ \
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" /* VTG on at one per fix */ \
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" /* RMC off (XXX suppress other message types?) */
const prog_char AP_GPS_NMEA::_initialisation_blob[] PROGMEM = SIRF_INIT_MSG MTK_INIT_MSG UBLOX_INIT_MSG;
// NMEA message identifiers ////////////////////////////////////////////////////
//
@ -105,14 +104,7 @@ AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr
_term_offset(0),
_gps_data_good(false)
{
// send the SiRF init strings
port->print_P((const prog_char_t *)_SiRF_init_string);
// send the MediaTek init strings
port->print_P((const prog_char_t *)_MTK_init_string);
// send the ublox init strings
port->print_P((const prog_char_t *)_ublox_init_string);
gps.send_blob_start(state.instance, _initialisation_blob, sizeof(_initialisation_blob));
}
bool AP_GPS_NMEA::read(void)

2
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -152,6 +152,8 @@ private:
static const prog_char _gpgga_string[];
static const prog_char _gpvtg_string[];
//@}
static const prog_char _initialisation_blob[];
};
#endif // __AP_GPS_NMEA_H__

5
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -28,9 +28,9 @@
//
// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
//
static const uint8_t init_messages[] PROGMEM = {
const uint8_t AP_GPS_SIRF::_initialisation_blob[] PROGMEM = {
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
};
AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
@ -41,6 +41,7 @@ AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDr
_payload_counter(0),
_msg_id(0)
{
gps.send_blob_start(state.instance, (const prog_char *)_initialisation_blob, sizeof(_initialisation_blob));
}

2
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -102,6 +102,8 @@ private:
bool _parse_gps(void);
void _accumulate(uint8_t val);
static const uint8_t _initialisation_blob[];
};
#endif // AP_GPS_SIRF_h