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

AP_GPS: new GPS API 

This is a complete rewrite of the GPS driver structure, with a static
main driver and separate backend drivers. This will allow proper
support for multiple GPSes, and will allow parameters to be set on the
GPS object
This commit is contained in:
Andrew Tridgell 2014-03-29 06:52:27 +11:00
parent 198388b5e0
commit 368daf89f1
24 changed files with 1005 additions and 1216 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

246
libraries/AP_GPS/AP_GPS.cpp Normal file
View File

@ -0,0 +1,246 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include <AP_Notify.h>
#include <AP_GPS.h>
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
// @DisplayName: GPS type
// @Description: GPS type
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1),
// @Param: TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0),
// @Param: NAVFILTER
// @DisplayName: Navigation filter setting
// @Description: Navigation filter engine setting
// @Values: 0:Portable,1:Stationary,2:Pedestrian,3:Automotive,4:Sea,5:Airborne1G,6:Airborne2G,8:Airborne4G
AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G),
AP_GROUPEND
};
/// Startup initialisation.
void AP_GPS::init(DataFlash_Class *dataflash)
{
_DataFlash = dataflash;
hal.uartB->begin(38400UL, 256, 16);
if (hal.uartE != NULL) {
hal.uartE->begin(38400UL, 256, 16);
}
}
// baudrates to try to detect GPSes with
const uint16_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 57600U, 9600U};
// initialisation blobs to send to the GPS to try to get it into the
// right mode
const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY;
/*
run detection step for one GPS instance. If this finds a GPS then it
will fill in drivers[instance] and change state[instance].status
from NO_GPS to NO_FIX.
*/
void
AP_GPS::detect_instance(uint8_t instance)
{
AP_GPS_Backend *new_gps = NULL;
AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE;
struct detect_state *dstate = &detect_state[instance];
if (port == NULL) {
// UART not available
return;
}
uint32_t now = hal.scheduler->millis();
// record the time when we started detection. This is used to try
// to avoid initialising a uBlox as a NMEA GPS
if (dstate->detect_started_ms == 0) {
dstate->detect_started_ms = now;
}
if (now - dstate->last_baud_change_ms > 1200) {
// try the next baud rate
dstate->last_baud++;
if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) {
dstate->last_baud = 0;
}
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--;
}
}
while (port->available() > 0 && new_gps == NULL) {
uint8_t data = port->read();
/*
running a uBlox at less than 38400 will lead to packet
corruption, as we can't receive the packets in the 200ms
window for 5Hz fixes. The NMEA startup message should force
the uBlox into 38400 no matter what rate it is configured
for.
*/
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) &&
pgm_read_word(&_baudrates[dstate->last_baud]) >= 38400 &&
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) {
hal.console->print_P(PSTR(" ublox "));
new_gps = new AP_GPS_UBLOX(*this, state[instance], port);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) &&
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) {
hal.console->print_P(PSTR(" MTK19 "));
new_gps = new AP_GPS_MTK19(*this, state[instance], port);
}
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) &&
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) {
hal.console->print_P(PSTR(" MTK "));
new_gps = new AP_GPS_MTK(*this, state[instance], port);
}
#if !defined( __AVR_ATmega1280__ )
// save a bit of code space on a 1280
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) &&
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) {
hal.console->print_P(PSTR(" SIRF "));
new_gps = new AP_GPS_SIRF(*this, state[instance], port);
}
else if (now - dstate->detect_started_ms > 5000) {
// prevent false detection of NMEA mode in
// a MTK or UBLOX which has booted in NMEA mode
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) &&
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) {
hal.console->print_P(PSTR(" NMEA "));
new_gps = new AP_GPS_NMEA(*this, state[instance], port);
}
}
#endif
}
if (new_gps != NULL) {
drivers[instance] = new_gps;
state[instance].status = NO_FIX;
state[instance].instance = instance;
}
}
/*
update one GPS instance
*/
void
AP_GPS::update_instance(uint8_t instance)
{
if (_type[instance] == GPS_TYPE_NONE) {
// not enabled
state[instance].status = NO_GPS;
return;
}
if (drivers[instance] == NULL || state[instance].status == NO_GPS) {
// we don't yet know the GPS type of this one, or it has timed
// out and needs to be re-initialised
detect_instance(instance);
return;
}
// we have an active driver for this instance
bool result = drivers[instance]->read();
uint32_t tnow = hal.scheduler->millis();
// if we did not get a message, and the idle timer of 1.2 seconds
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
if (!result) {
if (tnow - timing[instance].last_message_time_ms > 1200) {
state[instance].status = NO_GPS;
timing[instance].last_message_time_ms = tnow;
}
} else {
timing[instance].last_message_time_ms = tnow;
if (state[instance].status >= GPS_OK_FIX_2D) {
timing[instance].last_fix_time_ms = tnow;
}
}
}
/*
update all GPS instances
*/
void
AP_GPS::update(void)
{
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
update_instance(i);
}
}
void
AP_GPS::setHIL(GPS_Status _status, uint64_t time_epoch_ms,
Location &_location, Vector3f &_velocity, uint8_t _num_sats)
{
uint32_t tnow = hal.scheduler->millis();
GPS_State &istate = state[0];
istate.status = _status;
istate.location = _location;
istate.location.options = 0;
istate.velocity = _velocity;
istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y);
istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL;
istate.hdop = 0;
istate.num_sats = _num_sats;
istate.have_vertical_velocity = false;
istate.last_gps_time_ms = tnow;
istate.time_week = time_epoch_ms / (86400*7*(uint64_t)1000);
istate.time_week_ms = time_epoch_ms - istate.time_week*(86400*7*(uint64_t)1000);
timing[0].last_message_time_ms = tnow;
timing[0].last_fix_time_ms = tnow;
_type[0].set(GPS_TYPE_NONE);
}

292
libraries/AP_GPS/AP_GPS.h
View File

@ -1,15 +1,287 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
/// @file AP_GPS.h
/// @brief Catch-all header that defines all supported GPS classes.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
#include "AP_GPS_NMEA.h"
#include "AP_GPS_SIRF.h"
#include "AP_GPS_UBLOX.h"
#include "AP_GPS_MTK.h"
#include "AP_GPS_MTK19.h"
#include "AP_GPS_None.h"
#include "AP_GPS_Auto.h"
#include "AP_GPS_HIL.h"
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __AP_GPS_H__
#define __AP_GPS_H__
#include <AP_HAL.h>
#include <inttypes.h>
#include <AP_Progmem.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include "GPS_detect_state.h"
/**
maximum number of GPS instances available on this platform. If more
than 1 then redundent sensors may be available
*/
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
#define GPS_MAX_INSTANCES 2
#else
#define GPS_MAX_INSTANCES 1
#endif
class DataFlash_Class;
class AP_GPS_Backend;
/// @class AP_GPS
/// GPS driver main class
class AP_GPS
{
public:
// constructor
AP_GPS() {
AP_Param::setup_object_defaults(this, var_info);
}
/// Startup initialisation.
void init(DataFlash_Class *dataflash);
/// Update GPS state based on possible bytes received from the module.
/// This routine must be called periodically (typically at 10Hz or
/// more) to process incoming data.
void update(void);
// GPS driver types
enum GPS_Type {
GPS_TYPE_NONE = 0,
GPS_TYPE_AUTO = 1,
GPS_TYPE_UBLOX = 2,
GPS_TYPE_MTK = 3,
GPS_TYPE_MTK19 = 4,
GPS_TYPE_NMEA = 5,
GPS_TYPE_SIRF = 6
};
/// GPS status codes
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK_FIX_2D = 2, ///< Receiving valid messages and 2D lock
GPS_OK_FIX_3D = 3 ///< Receiving valid messages and 3D lock
};
// GPS navigation engine settings. Not all GPS receivers support
// this
enum GPS_Engine_Setting {
GPS_ENGINE_NONE = -1,
GPS_ENGINE_PORTABLE = 0,
GPS_ENGINE_STATIONARY = 2,
GPS_ENGINE_PEDESTRIAN = 3,
GPS_ENGINE_AUTOMOTIVE = 4,
GPS_ENGINE_SEA = 5,
GPS_ENGINE_AIRBORNE_1G = 6,
GPS_ENGINE_AIRBORNE_2G = 7,
GPS_ENGINE_AIRBORNE_4G = 8
};
struct GPS_State {
uint8_t instance; // the instance number of this GPS
// all the following fields must all be filled by the backend driver
GPS_Status status; ///< driver fix status
uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
uint16_t time_week; ///< GPS week number
Location location; ///< last fix location
float ground_speed; ///< ground speed in m/sec
int32_t ground_course_cd; ///< ground course in 100ths of a degree
int16_t hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
Vector3f velocity; ///< 3D velocitiy in m/s, in NED format
bool have_vertical_velocity:1; ///< does this GPS give vertical velocity?
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
};
// Accessor functions
// return number of active GPS sensors
uint8_t num_sensors(void) const {
uint8_t count = 0;
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) {
if (drivers[i] != NULL) count++;
}
return count;
}
/// Query GPS status
GPS_Status status(uint8_t instance) const {
return state[instance].status;
}
GPS_Status status(void) const {
return status(primary_instance());
}
// location of last fix
const Location &location(uint8_t instance) const {
return state[instance].location;
}
const Location &location() const {
return location(primary_instance());
}
// 3D velocity in NED format
const Vector3f &velocity(uint8_t instance) const {
return state[instance].velocity;
}
const Vector3f &velocity() const {
return velocity(primary_instance());
}
// ground speed in m/s
float ground_speed(uint8_t instance) const {
return state[instance].ground_speed;
}
float ground_speed() const {
return ground_speed(primary_instance());
}
// ground speed in cm/s
uint32_t ground_speed_cm(void) {
return ground_speed() * 100;
}
// ground course in centidegrees
int32_t ground_course_cd(uint8_t instance) const {
return state[instance].ground_course_cd;
}
int32_t ground_course_cd() const {
return ground_course_cd(primary_instance());
}
// number of locked satellites
uint8_t num_sats(uint8_t instance) const {
return state[instance].num_sats;
}
uint8_t num_sats() const {
return num_sats(primary_instance());
}
// GPS time of week in milliseconds
uint32_t time_week_ms(uint8_t instance) const {
return state[instance].time_week_ms;
}
uint32_t time_week_ms() const {
return time_week_ms(primary_instance());
}
// GPS week
uint16_t time_week(uint8_t instance) const {
return state[instance].time_week;
}
uint16_t time_week() const {
return time_week(primary_instance());
}
// horizontal dilution of precision
int16_t get_hdop(uint8_t instance) const {
return state[instance].hdop;
}
int16_t get_hdop() const {
return get_hdop(primary_instance());
}
// the time we got our last fix in system milliseconds. This is
// used when calculating how far we might have moved since that fix
uint32_t last_fix_time_ms(uint8_t instance) const {
return timing[instance].last_fix_time_ms;
}
uint32_t last_fix_time_ms(void) const {
return last_fix_time_ms(primary_instance());
}
// the time we last processed a message in milliseconds. This is
// used to indicate that we have new GPS data to process
uint32_t last_message_time_ms(uint8_t instance) const {
return timing[instance].last_message_time_ms;
}
uint32_t last_message_time_ms(void) const {
return last_message_time_ms(primary_instance());
}
// return last fix time since the 1/1/1970 in microseconds
uint64_t time_epoch_usec(uint8_t instance);
uint64_t time_epoch_usec(void) {
return time_epoch_usec(primary_instance());
}
// return true if the GPS supports vertical velocity values
bool have_vertical_velocity(uint8_t instance) const {
return state[instance].have_vertical_velocity;
}
bool have_vertical_velocity(void) const {
return have_vertical_velocity(primary_instance());
}
// the expected lag (in seconds) in the position and velocity readings from the gps
float get_lag() const { return 0.2f; }
// set position for HIL
void setHIL(GPS_Status status, uint64_t time_epoch_ms,
Location &location, Vector3f &velocity, uint8_t num_sats);
static const struct AP_Param::GroupInfo var_info[];
// dataflash for logging, if available
DataFlash_Class *_DataFlash;
// configuration parameters
AP_Int8 _type[GPS_MAX_INSTANCES];
AP_Int8 _navfilter;
private:
struct GPS_timing {
// the time we got our last fix in system milliseconds
uint32_t last_fix_time_ms;
// the time we got our last fix in system milliseconds
uint32_t last_message_time_ms;
};
GPS_timing timing[GPS_MAX_INSTANCES];
GPS_State state[GPS_MAX_INSTANCES];
AP_GPS_Backend *drivers[GPS_MAX_INSTANCES];
/// return primary GPS instance
uint8_t primary_instance(void) const { return 0; }
// state of auto-detection process, per instance
struct detect_state {
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;
struct SIRF_detect_state sirf_detect_state;
struct NMEA_detect_state nmea_detect_state;
} detect_state[GPS_MAX_INSTANCES];
static const uint16_t _baudrates[];
static const prog_char _initialisation_blob[];
void detect_instance(uint8_t instance);
void update_instance(uint8_t instance);
};
#include <GPS_Backend.h>
#include <AP_GPS_UBLOX.h>
#include <AP_GPS_MTK.h>
#include <AP_GPS_MTK19.h>
#include <AP_GPS_NMEA.h>
#include <AP_GPS_SIRF.h>
#endif // __AP_GPS_H__

184
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -1,184 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_GPS_Auto.cpp
/// @brief Simple GPS auto-detection logic.
#include <AP_HAL.h>
#include <AP_Common.h>
#include "AP_GPS.h" // includes AP_GPS_Auto.h
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),
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;
}
// Called the first time that a client tries to kick the GPS to update.
//
// We detect the real GPS, then update the pointer we have been called through
// and return.
//
bool
AP_GPS_Auto::read(void)
{
if (state == NULL) {
return false;
}
GPS *gps;
uint32_t now = hal.scheduler->millis();
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[state->last_baud]);
//hal.console->printf_P(PSTR("Setting GPS baudrate %u\n"), (unsigned)_baudrate);
_port->begin(_baudrate, 256, 16);
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(_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();
if (NULL != (gps = _detect())) {
// configure the detected GPS
gps->init(_port, _nav_setting, _DataFlash);
hal.console->println_P(PSTR("OK"));
free(state);
state = NULL;
*_gps = gps;
return true;
}
return false;
}
//
// Perform one iteration of the auto-detection process.
//
GPS *
AP_GPS_Auto::_detect(void)
{
GPS *new_gps = NULL;
if (state->detect_started_ms == 0 && _port->available() > 0) {
state->detect_started_ms = hal.scheduler->millis();
}
while (_port->available() > 0 && new_gps == NULL) {
uint8_t data = _port->read();
/*
running a uBlox at less than 38400 will lead to packet
corruption, as we can't receive the packets in the 200ms
window for 5Hz fixes. The NMEA startup message should force
the uBlox into 38400 no matter what rate it is configured
for.
*/
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(state->mtk19_detect_state, data)) {
hal.console->print_P(PSTR(" MTK19 "));
new_gps = new AP_GPS_MTK19();
}
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(state->sirf_detect_state, data)) {
hal.console->print_P(PSTR(" SIRF "));
new_gps = new AP_GPS_SIRF();
}
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(state->nmea_detect_state, data)) {
hal.console->print_P(PSTR(" NMEA "));
new_gps = new AP_GPS_NMEA();
}
}
#endif
}
if (new_gps != NULL) {
new_gps->init(_port, _nav_setting, _DataFlash);
}
return new_gps;
}
/*
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;
}
}
}

76
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -1,76 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
// Auto-detecting pseudo-GPS driver
//
#ifndef __AP_GPS_AUTO_H__
#define __AP_GPS_AUTO_H__
#include <AP_HAL.h>
#include "GPS.h"
class AP_GPS_Auto : public GPS
{
public:
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param port UARTDriver connected to the GPS module.
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected.
///
AP_GPS_Auto(GPS **gps);
/// Dummy init routine, does nothing
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.
///
bool read(void);
private:
/// global GPS driver pointer, updated by auto-detection
///
GPS ** _gps;
/// low-level auto-detect routine
///
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

28
libraries/AP_GPS/AP_GPS_Glitch.cpp
View File

@ -41,7 +41,7 @@ const AP_Param::GroupInfo GPS_Glitch::var_info[] PROGMEM = {
};
// constuctor
GPS_Glitch::GPS_Glitch(GPS*& gps) :
GPS_Glitch::GPS_Glitch(const AP_GPS &gps) :
_gps(gps)
{
AP_Param::setup_object_defaults(this, var_info);
@ -59,18 +59,20 @@ void GPS_Glitch::check_position()
bool all_ok; // true if the new gps position passes sanity checks
// exit immediately if we don't have gps lock
if (_gps == NULL || _gps->status() != GPS::GPS_OK_FIX_3D) {
if (_gps.status() < AP_GPS::GPS_OK_FIX_3D) {
_flags.glitching = true;
return;
}
// if not initialised or disabled update last good position and exit
if (!_flags.initialised || !_enabled) {
const Location &loc = _gps.location();
const Vector3f &vel = _gps.velocity();
_last_good_update = now;
_last_good_lat = _gps->latitude;
_last_good_lon = _gps->longitude;
_last_good_vel.x = _gps->velocity_north();
_last_good_vel.y = _gps->velocity_east();
_last_good_lat = loc.lat;
_last_good_lon = loc.lng;
_last_good_vel.x = vel.x;
_last_good_vel.y = vel.y;
_flags.initialised = true;
_flags.glitching = false;
return;
@ -85,8 +87,9 @@ void GPS_Glitch::check_position()
location_offset(curr_pos, _last_good_vel.x * sane_dt, _last_good_vel.y * sane_dt);
// calculate distance from recent gps position to current estimate
gps_pos.lat = _gps->latitude;
gps_pos.lng = _gps->longitude;
const Location &loc = _gps.location();
gps_pos.lat = loc.lat;
gps_pos.lng = loc.lng;
distance_cm = get_distance_cm(curr_pos, gps_pos);
// all ok if within a given hardcoded radius
@ -102,10 +105,11 @@ void GPS_Glitch::check_position()
if (all_ok) {
// position is acceptable
_last_good_update = now;
_last_good_lat = _gps->latitude;
_last_good_lon = _gps->longitude;
_last_good_vel.x = _gps->velocity_north();
_last_good_vel.y = _gps->velocity_east();
_last_good_lat = loc.lat;
_last_good_lon = loc.lng;
const Vector3f &vel = _gps.velocity();
_last_good_vel.x = vel.x;
_last_good_vel.y = vel.y;
}
// update glitching flag

4
libraries/AP_GPS/AP_GPS_Glitch.h
View File

@ -22,7 +22,7 @@ class GPS_Glitch
{
public:
// constructor
GPS_Glitch(GPS*& gps);
GPS_Glitch(const AP_GPS &gps);
// check_position - checks latest gps position against last know position, velocity and maximum acceleration and updates glitching flag
void check_position();
@ -45,7 +45,7 @@ public:
private:
/// external dependencies
GPS*& _gps; // pointer to gps
const AP_GPS &_gps; // reference to gps
/// structure for holding flags
struct GPS_Glitch_flags {

67
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -1,67 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//
// Hardware in the loop gps class.
// Code by James Goppert
//
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#include <AP_HAL.h>
#include "AP_GPS_HIL.h"
extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_HIL::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
{
_port = s;
_nav_setting = nav_setting;
_DataFlash = DataFlash;
}
bool AP_GPS_HIL::read(void)
{
if ((hal.scheduler->millis() - last_fix_time) < 200) {
// simulate a 5Hz GPS
return false;
}
bool result = _updated;
// return true once for each update pushed in
new_data = true;
_updated = false;
return result;
}
void AP_GPS_HIL::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)
{
time_week = _time_epoch_ms / (86400*7*(uint64_t)1000);
time_week_ms = _time_epoch_ms - time_week*(86400*7*(uint64_t)1000);
latitude = _latitude*1.0e7f;
longitude = _longitude*1.0e7f;
altitude_cm = _altitude*1.0e2f;
ground_speed_cm = _ground_speed*1.0e2f;
ground_course_cd = _ground_course*1.0e2f;
num_sats = _num_sats;
fix = fix_status,
hdop = 200;
_updated = true;
}

56
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -1,56 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
//
// Hardware in the loop gps class.
// Code by James Goppert
//
//
#ifndef __AP_GPS_HIL_H__
#define __AP_GPS_HIL_H__
#include <AP_HAL.h>
#include "GPS.h"
class AP_GPS_HIL : public GPS {
public:
AP_GPS_HIL() :
GPS(),
_updated(false)
{}
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read(void);
/**
* Hardware in the loop set function
* @param latitude - latitude in deggrees
* @param longitude - longitude in degrees
* @param altitude - altitude in degrees
* @param ground_speed - ground speed in meters/second
* @param ground_course - ground course in degrees
* @param speed_3d - ground speed in meters/second
* @param altitude - altitude in meters
*/
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);
private:
bool _updated;
};
#endif // __AP_GPS_HIL_H__

60
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -21,37 +21,31 @@
// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1"
//
#include <stdint.h>
#include <AP_HAL.h>
#include <AP_GPS.h>
#include "AP_GPS_MTK.h"
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_MTK::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
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)
{
_port = s;
_port->flush();
_step = 0;
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
port->print(MTK_SET_BINARY);
// set 5Hz update rate
_port->print(MTK_OUTPUT_5HZ);
port->print(MTK_OUTPUT_5HZ);
// set SBAS on
_port->print(SBAS_ON);
port->print(SBAS_ON);
// set WAAS on
_port->print(WAAS_ON);
port->print(WAAS_ON);
// Set Nav Threshold to 0 m/s
_port->print(MTK_NAVTHRES_OFF);
port->print(MTK_NAVTHRES_OFF);
}
// Process bytes available from the stream
//
// The stream is assumed to contain only our custom message. If it
@ -70,11 +64,11 @@ AP_GPS_MTK::read(void)
int16_t numc;
bool parsed = false;
numc = _port->available();
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
data = port->read();
restart:
switch(_step) {
@ -133,38 +127,38 @@ restart:
case 5:
_step++;
if (_ck_a != data) {
_error("GPS_MTK: checksum error\n");
_step = 0;
}
break;
case 6:
_step = 0;
if (_ck_b != data) {
_error("GPS_MTK: checksum error\n");
break;
}
// set fix type
if (_buffer.msg.fix_type == FIX_3D) {
fix = GPS::FIX_3D;
state.status = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.msg.fix_type == FIX_2D) {
fix = GPS::FIX_2D;
state.status = AP_GPS::GPS_OK_FIX_2D;
}else{
fix = GPS::FIX_NONE;
state.status = AP_GPS::NO_FIX;
}
latitude = _swapl(&_buffer.msg.latitude) * 10;
longitude = _swapl(&_buffer.msg.longitude) * 10;
altitude_cm = _swapl(&_buffer.msg.altitude);
ground_speed_cm = _swapl(&_buffer.msg.ground_speed);
ground_course_cd = _swapl(&_buffer.msg.ground_course) / 10000;
num_sats = _buffer.msg.satellites;
state.location.lat = swap_int32(_buffer.msg.latitude) * 10;
state.location.lng = swap_int32(_buffer.msg.longitude) * 10;
state.location.alt = swap_int32(_buffer.msg.altitude);
state.ground_speed = swap_int32(_buffer.msg.ground_speed) * 0.01f;
state.ground_course_cd = swap_int32(_buffer.msg.ground_course) / 10000;
state.num_sats = _buffer.msg.satellites;
if (fix >= GPS::FIX_2D) {
_make_gps_time(0, _swapl(&_buffer.msg.utc_time)*10);
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {
make_gps_time(0, swap_int32(_buffer.msg.utc_time)*10);
}
// we don't change _last_gps_time as we don't know the
// full date
fill_3d_velocity();
parsed = true;
}
}
@ -175,7 +169,7 @@ restart:
detect a MTK GPS
*/
bool
AP_GPS_MTK::_detect(struct detect_state &state, uint8_t data)
AP_GPS_MTK::_detect(struct MTK_detect_state &state, uint8_t data)
{
switch (state.step) {
case 1:

22
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -25,28 +25,16 @@
#ifndef __AP_GPS_MTK_H__
#define __AP_GPS_MTK_H__
#include "GPS.h"
#include <AP_Common.h>
#include <AP_GPS.h>
#include "AP_GPS_MTK_Common.h"
class AP_GPS_MTK : public GPS {
class AP_GPS_MTK : public AP_GPS_Backend {
public:
AP_GPS_MTK() :
GPS(),
_step(0),
_payload_counter(0)
{}
AP_GPS_MTK(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read(void);
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);
static bool _detect(struct MTK_detect_state &state, uint8_t data);
private:
struct PACKED diyd_mtk_msg {

71
libraries/AP_GPS/AP_GPS_MTK19.cpp
View File

@ -23,34 +23,32 @@
// Note that this driver supports both the 1.6 and 1.9 protocol varients
//
#include <AP_HAL.h>
#include "AP_GPS_MTK19.h"
#include <stdint.h>
extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_MTK19::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
AP_GPS_MTK19::AP_GPS_MTK19(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port),
_step(0),
_payload_counter(0),
_mtk_revision(0),
_fix_counter(0)
{
_port = s;
_port->flush();
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
port->print(MTK_SET_BINARY);
// set 5Hz update rate
_port->print(MTK_OUTPUT_5HZ);
port->print(MTK_OUTPUT_5HZ);
// set SBAS on
_port->print(SBAS_ON);
port->print(SBAS_ON);
// set WAAS on
_port->print(WAAS_ON);
port->print(WAAS_ON);
// Set Nav Threshold to 0 m/s
_port->print(MTK_NAVTHRES_OFF);
port->print(MTK_NAVTHRES_OFF);
}
// Process bytes available from the stream
@ -71,11 +69,11 @@ AP_GPS_MTK19::read(void)
int16_t numc;
bool parsed = false;
numc = _port->available();
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
data = port->read();
restart:
switch(_step) {
@ -144,27 +142,27 @@ restart:
// parse fix
if (_buffer.msg.fix_type == FIX_3D || _buffer.msg.fix_type == FIX_3D_SBAS) {
fix = GPS::FIX_3D;
state.status = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.msg.fix_type == FIX_2D || _buffer.msg.fix_type == FIX_2D_SBAS) {
fix = GPS::FIX_2D;
state.status = AP_GPS::GPS_OK_FIX_2D;
}else{
fix = GPS::FIX_NONE;
state.status = AP_GPS::NO_FIX;
}
if (_mtk_revision == MTK_GPS_REVISION_V16) {
latitude = _buffer.msg.latitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise
longitude = _buffer.msg.longitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise
state.location.lat = _buffer.msg.latitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise
state.location.lng = _buffer.msg.longitude * 10; // V16, V17,V18 doc says *10e7 but device says otherwise
} else {
latitude = _buffer.msg.latitude;
longitude = _buffer.msg.longitude;
state.location.lat = _buffer.msg.latitude;
state.location.lng = _buffer.msg.longitude;
}
altitude_cm = _buffer.msg.altitude;
ground_speed_cm = _buffer.msg.ground_speed;
ground_course_cd = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop;
state.location.alt = _buffer.msg.altitude;
state.ground_speed = _buffer.msg.ground_speed*0.01f;
state.ground_course_cd = _buffer.msg.ground_course;
state.num_sats = _buffer.msg.satellites;
state.hdop = _buffer.msg.hdop;
if (fix >= GPS::FIX_2D) {
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {
if (_fix_counter == 0) {
uint32_t bcd_time_ms;
if (_mtk_revision == MTK_GPS_REVISION_V16) {
@ -172,8 +170,8 @@ restart:
} else {
bcd_time_ms = _buffer.msg.utc_time;
}
_make_gps_time(_buffer.msg.utc_date, bcd_time_ms);
_last_gps_time = hal.scheduler->millis();
make_gps_time(_buffer.msg.utc_date, bcd_time_ms);
state.last_gps_time_ms = hal.scheduler->millis();
}
// the _fix_counter is to reduce the cost of the GPS
// BCD time conversion by only doing it every 10s
@ -185,16 +183,9 @@ restart:
}
}
parsed = true;
fill_3d_velocity();
#ifdef FAKE_GPS_LOCK_TIME
if (millis() > FAKE_GPS_LOCK_TIME*1000) {
fix = true;
latitude = -35000000UL;
longitude = 149000000UL;
altitude = 584;
}
#endif
parsed = true;
}
}
return parsed;
@ -205,7 +196,7 @@ restart:
detect a MTK16 or MTK19 GPS
*/
bool
AP_GPS_MTK19::_detect(struct detect_state &state, uint8_t data)
AP_GPS_MTK19::_detect(struct MTK19_detect_state &state, uint8_t data)
{
restart:
switch (state.step) {

23
libraries/AP_GPS/AP_GPS_MTK19.h
View File

@ -23,34 +23,19 @@
#ifndef AP_GPS_MTK19_h
#define AP_GPS_MTK19_h
#include "GPS.h"
#include <AP_Common.h>
#include <AP_GPS.h>
#include "AP_GPS_MTK_Common.h"
#define MTK_GPS_REVISION_V16 16
#define MTK_GPS_REVISION_V19 19
class AP_GPS_MTK19 : public GPS {
class AP_GPS_MTK19 : public AP_GPS_Backend {
public:
AP_GPS_MTK19() :
GPS(),
_step(0),
_payload_counter(0),
_mtk_revision(0),
_fix_counter(0)
{}
AP_GPS_MTK19(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
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);
static bool _detect(struct MTK19_detect_state &state, uint8_t data);
private:
struct PACKED diyd_mtk_msg {

60
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -96,19 +96,23 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
//
#define DIGIT_TO_VAL(_x) (_x - '0')
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_NMEA::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
AP_GPS_NMEA::AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port),
_parity(0),
_is_checksum_term(false),
_sentence_type(0),
_term_number(0),
_term_offset(0),
_gps_data_good(false)
{
_port = s;
// send the SiRF init strings
_port->print_P((const prog_char_t *)_SiRF_init_string);
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);
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);
port->print_P((const prog_char_t *)_ublox_init_string);
}
bool AP_GPS_NMEA::read(void)
@ -116,9 +120,9 @@ bool AP_GPS_NMEA::read(void)
int16_t numc;
bool parsed = false;
numc = _port->available();
numc = port->available();
while (numc--) {
if (_decode(_port->read())) {
if (_decode(port->read())) {
parsed = true;
}
}
@ -248,26 +252,28 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_GPRMC:
//time = _new_time;
//date = _new_date;
latitude = _new_latitude;
longitude = _new_longitude;
ground_speed_cm = _new_speed;
ground_course_cd = _new_course;
_make_gps_time(_new_date, _new_time * 10);
_last_gps_time = hal.scheduler->millis();
fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = _new_course;
make_gps_time(_new_date, _new_time * 10);
state.last_gps_time_ms = hal.scheduler->millis();
// To-Do: add support for proper reporting of 2D and 3D fix
state.status = AP_GPS::GPS_OK_FIX_3D;
fill_3d_velocity();
break;
case _GPS_SENTENCE_GPGGA:
altitude_cm = _new_altitude;
//time = _new_time;
latitude = _new_latitude;
longitude = _new_longitude;
num_sats = _new_satellite_count;
hdop = _new_hdop;
fix = GPS::FIX_3D; // To-Do: add support for proper reporting of 2D and 3D fix
state.location.alt = _new_altitude;
state.location.lat = _new_latitude;
state.location.lng = _new_longitude;
state.num_sats = _new_satellite_count;
state.hdop = _new_hdop;
// To-Do: add support for proper reporting of 2D and 3D fix
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case _GPS_SENTENCE_GPVTG:
ground_speed_cm = _new_speed;
ground_course_cd = _new_course;
state.ground_speed = _new_speed*0.01f;
state.ground_course_cd = _new_course;
// VTG has no fix indicator, can't change fix status
break;
}
@ -277,7 +283,7 @@ bool AP_GPS_NMEA::_term_complete()
case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about
// fix status.
fix = GPS::FIX_NONE;
state.status = AP_GPS::NO_FIX;
}
}
// we got a good message
@ -382,7 +388,7 @@ bool AP_GPS_NMEA::_term_complete()
matches a NMEA string
*/
bool
AP_GPS_NMEA::_detect(struct detect_state &state, uint8_t data)
AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
{
switch (state.step) {
case 0:

29
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -47,42 +47,21 @@
#ifndef __AP_GPS_NMEA_H__
#define __AP_GPS_NMEA_H__
#include <AP_HAL.h>
#include "GPS.h"
#include <AP_Progmem.h>
#include <AP_GPS.h>
/// NMEA parser
///
class AP_GPS_NMEA : public GPS
class AP_GPS_NMEA : public AP_GPS_Backend
{
public:
AP_GPS_NMEA(void) :
GPS(),
_parity(0),
_is_checksum_term(false),
_sentence_type(0),
_term_number(0),
_term_offset(0),
_gps_data_good(false)
{}
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
AP_GPS_NMEA(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state
/// accordingly.
///
bool read();
struct detect_state {
uint8_t step;
uint8_t ck;
};
static bool _detect(struct detect_state &state, uint8_t data);
static bool _detect(struct NMEA_detect_state &state, uint8_t data);
private:
/// Coding for the GPS sentences that the parser handles

21
libraries/AP_GPS/AP_GPS_None.h
View File

@ -1,21 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_GPS_NONE_H__
#define __AP_GPS_NONE_H__
#include <AP_HAL.h>
#include "GPS.h"
class AP_GPS_None : public GPS
{
public:
AP_GPS_None() : GPS() {}
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash) {
_port = s;
};
bool read(void) {
return false;
};
};
#endif // __AP_GPS_NONE_H__

51
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -33,22 +33,17 @@ static const uint8_t init_messages[] PROGMEM = {
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
};
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_SIRF::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
AP_GPS_SIRF::AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port),
_step(0),
_gather(false),
_payload_length(0),
_payload_counter(0),
_msg_id(0)
{
_port = s;
_port->flush();
_step = 0;
// For modules that default to something other than SiRF binary,
// the module-specific subclass should take care of switching to binary mode
// before calling us.
// send SiRF binary setup messages
_write_progstr_block(_port, (const prog_char *)init_messages, sizeof(init_messages));
}
// Process bytes available from the stream
//
// The stream is assumed to contain only messages we recognise. If it
@ -65,11 +60,11 @@ AP_GPS_SIRF::read(void)
int16_t numc;
bool parsed = false;
numc = _port->available();
numc = port->available();
while(numc--) {
// read the next byte
data = _port->read();
data = port->read();
switch(_step) {
@ -156,14 +151,12 @@ AP_GPS_SIRF::read(void)
case 6:
_step++;
if ((_checksum >> 8) != data) {
_error("GPS_SIRF: checksum error\n");
_step = 0;
}
break;
case 7:
_step = 0;
if ((_checksum & 0xff) != data) {
_error("GPS_SIRF: checksum error\n");
break;
}
if (_gather) {
@ -182,21 +175,19 @@ AP_GPS_SIRF::_parse_gps(void)
//time = _swapl(&_buffer.nav.time);
// parse fix type
if (_buffer.nav.fix_invalid) {
fix = GPS::FIX_NONE;
state.status = AP_GPS::NO_FIX;
}else if ((_buffer.nav.fix_type & FIX_MASK) == FIX_3D) {
fix = GPS::FIX_3D;
state.status = AP_GPS::GPS_OK_FIX_3D;
}else{
fix = GPS::FIX_2D;
state.status = AP_GPS::GPS_OK_FIX_2D;
}
latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude);
altitude_cm = _swapl(&_buffer.nav.altitude_msl);
ground_speed_cm = _swapi(&_buffer.nav.ground_speed);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
if (ground_speed_cm > 50)
ground_course_cd = _swapi(&_buffer.nav.ground_course);
num_sats = _buffer.nav.satellites;
state.location.lat = swap_int32(_buffer.nav.latitude);
state.location.lng = swap_int32(_buffer.nav.longitude);
state.location.alt = swap_int32(_buffer.nav.altitude_msl);
state.ground_speed = swap_int32(_buffer.nav.ground_speed)*0.01f;
state.ground_course_cd = swap_int16(_buffer.nav.ground_course);
state.num_sats = _buffer.nav.satellites;
fill_3d_velocity();
return true;
}
return false;
@ -214,7 +205,7 @@ AP_GPS_SIRF::_accumulate(uint8_t val)
detect a SIRF GPS
*/
bool
AP_GPS_SIRF::_detect(struct detect_state &state, uint8_t data)
AP_GPS_SIRF::_detect(struct SIRF_detect_state &state, uint8_t data)
{
switch (state.step) {
case 1:

21
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -23,30 +23,17 @@
#include <AP_HAL.h>
#include <AP_Common.h>
#include "GPS.h"
#include <AP_GPS.h>
#define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C"
class AP_GPS_SIRF : public GPS {
class AP_GPS_SIRF : public AP_GPS_Backend {
public:
AP_GPS_SIRF() :
GPS(),
_step(0),
_gather(false),
_payload_length(0),
_payload_counter(0),
_msg_id(0)
{}
AP_GPS_SIRF(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
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);
static bool _detect(struct SIRF_detect_state &state, uint8_t data);
private:
struct PACKED sirf_geonav {

173
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -15,12 +15,12 @@
*/
//
// u-blox UBX GPS driver for ArduPilot and ArduPilotMega.
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
// u-blox GPS driver for ArduPilot
// Origin code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
// Substantially rewitten for new GPS driver structure by Andrew Tridgell
//
#include <stdint.h>
#include <AP_HAL.h>
#include <AP_GPS.h>
#include "AP_GPS_UBLOX.h"
#include <DataFlash.h>
#define UBLOX_DEBUGGING 0
@ -34,10 +34,6 @@ extern const AP_HAL::HAL& hal;
# define Debug(fmt, args ...)
#endif
#include "AP_GPS_UBLOX.h"
extern const AP_HAL::HAL& hal;
/*
only do detailed hardware logging on boards likely to have more log
storage space
@ -48,29 +44,24 @@ extern const AP_HAL::HAL& hal;
#define UBLOX_HW_LOGGING 0
#endif
// Public Methods //////////////////////////////////////////////////////////////
void
AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash)
AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
AP_GPS_Backend(_gps, _state, _port),
_step(0),
_msg_id(0),
_payload_length(0),
_payload_counter(0),
_fix_count(0),
_class(0),
_new_position(0),
_new_speed(0),
need_rate_update(false),
_disable_counter(0),
next_fix(AP_GPS::NO_FIX),
rate_update_step(0),
_last_hw_status(0)
{
_port = s;
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.
Debug("uBlox nav_setting=%u\n", nav_setting);
_port->flush();
_nav_setting = nav_setting;
_DataFlash = DataFlash;
// configure the GPS for the messages we want
_configure_gps();
_step = 0;
_new_position = false;
_new_speed = false;
}
/*
@ -82,7 +73,7 @@ AP_GPS_UBLOX::init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, D
void
AP_GPS_UBLOX::send_next_rate_update(void)
{
if (_port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
if (port->txspace() < (int16_t)(sizeof(struct ubx_header)+sizeof(struct ubx_cfg_nav_rate)+2)) {
// not enough space - do it next time
return;
}
@ -143,11 +134,11 @@ AP_GPS_UBLOX::read(void)
send_next_rate_update();
}
numc = _port->available();
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
data = port->read();
reset:
switch(_step) {
@ -285,13 +276,13 @@ void AP_GPS_UBLOX::write_logging_headers(void)
{
if (!logging_started) {
logging_started = true;
_DataFlash->AddLogFormats(ubx_log_structures, 2);
gps._DataFlash->AddLogFormats(ubx_log_structures, 2);
}
}
void AP_GPS_UBLOX::log_mon_hw(void)
{
if (_DataFlash == NULL || !_DataFlash->logging_started()) {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
// log mon_hw message
@ -300,17 +291,17 @@ void AP_GPS_UBLOX::log_mon_hw(void)
struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_UBX1),
timestamp : hal.scheduler->millis(),
instance : (uint8_t)(_secondary_gps?1:0),
instance : state.instance,
noisePerMS : _buffer.mon_hw.noisePerMS,
jamInd : _buffer.mon_hw.jamInd,
aPower : _buffer.mon_hw.aPower
};
_DataFlash->WriteBlock(&pkt, sizeof(pkt));
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
void AP_GPS_UBLOX::log_mon_hw2(void)
{
if (_DataFlash == NULL || !_DataFlash->logging_started()) {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
return;
}
// log mon_hw message
@ -319,13 +310,13 @@ void AP_GPS_UBLOX::log_mon_hw2(void)
struct log_Ubx2 pkt = {
LOG_PACKET_HEADER_INIT(LOG_MSG_UBX2),
timestamp : hal.scheduler->millis(),
instance : (uint8_t)(_secondary_gps?1:0),
instance : state.instance,
ofsI : _buffer.mon_hw2.ofsI,
magI : _buffer.mon_hw2.magI,
ofsQ : _buffer.mon_hw2.ofsQ,
magQ : _buffer.mon_hw2.magQ,
};
_DataFlash->WriteBlock(&pkt, sizeof(pkt));
gps._DataFlash->WriteBlock(&pkt, sizeof(pkt));
}
#endif // UBLOX_HW_LOGGING
@ -352,13 +343,13 @@ AP_GPS_UBLOX::_parse_gps(void)
if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
Debug("Got engine settings %u\n", (unsigned)_buffer.nav_settings.dynModel);
if (_nav_setting != GPS_ENGINE_NONE &&
_buffer.nav_settings.dynModel != _nav_setting) {
if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
_buffer.nav_settings.dynModel != gps._navfilter) {
// we've received the current nav settings, change the engine
// settings and send them back
Debug("Changing engine setting from %u to %u\n",
(unsigned)_buffer.nav_settings.dynModel, (unsigned)_nav_setting);
_buffer.nav_settings.dynModel = _nav_setting;
(unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
_buffer.nav_settings.dynModel = gps._navfilter;
_buffer.nav_settings.mask = 1; // only change dynamic model
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
@ -388,15 +379,15 @@ AP_GPS_UBLOX::_parse_gps(void)
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude_cm = _buffer.posllh.altitude_msl / 10;
fix = next_fix;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
state.status = next_fix;
_new_position = true;
#if UBLOX_FAKE_3DLOCK
longitude = 1491652300L;
latitude = -353632610L;
altitude_cm = 58400;
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
#endif
break;
case MSG_STATUS:
@ -405,20 +396,20 @@ AP_GPS_UBLOX::_parse_gps(void)
_buffer.status.fix_type);
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = GPS::FIX_3D;
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = GPS::FIX_2D;
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = GPS::FIX_NONE;
fix = GPS::FIX_NONE;
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = GPS::FIX_NONE;
fix = GPS::FIX_NONE;
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
#if UBLOX_FAKE_3DLOCK
fix = GPS::FIX_3D;
next_fix = fix;
state.status = AP_GPS::GPS_OK_FIX_3D;
next_fix = state.status;
#endif
break;
case MSG_SOL:
@ -427,48 +418,48 @@ AP_GPS_UBLOX::_parse_gps(void)
_buffer.solution.fix_type);
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = GPS::FIX_3D;
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = GPS::FIX_2D;
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = GPS::FIX_NONE;
fix = GPS::FIX_NONE;
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = GPS::FIX_NONE;
fix = GPS::FIX_NONE;
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
if (next_fix >= GPS::FIX_2D) {
_last_gps_time = hal.scheduler->millis();
if (time_week == _buffer.solution.week &&
time_week_ms + 200 == _buffer.solution.time) {
state.num_sats = _buffer.solution.satellites;
state.hdop = _buffer.solution.position_DOP;
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
state.last_gps_time_ms = hal.scheduler->millis();
if (state.time_week == _buffer.solution.week &&
state.time_week_ms + 200 == _buffer.solution.time) {
// we got a 5Hz update. This relies on the way
// that uBlox gives timestamps that are always
// multiples of 200 for 5Hz
_last_5hz_time = _last_gps_time;
_last_5hz_time = state.last_gps_time_ms;
}
time_week_ms = _buffer.solution.time;
time_week = _buffer.solution.week;
state.time_week_ms = _buffer.solution.time;
state.time_week = _buffer.solution.week;
}
#if UBLOX_FAKE_3DLOCK
next_fix = fix;
num_sats = 10;
hdop = 200;
time_week = 1721;
time_week_ms = hal.scheduler->millis() + 3*60*60*1000 + 37000;
_last_gps_time = hal.scheduler->millis();
next_fix = state.status;
state.num_sats = 10;
state.hdop = 200;
state.time_week = 1721;
state.time_week_ms = hal.scheduler->millis() + 3*60*60*1000 + 37000;
state.last_gps_time_ms = hal.scheduler->millis();
#endif
break;
case MSG_VELNED:
Debug("MSG_VELNED");
ground_speed_cm = _buffer.velned.speed_2d; // cm/s
ground_course_cd = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
_have_raw_velocity = true;
_vel_north = _buffer.velned.ned_north;
_vel_east = _buffer.velned.ned_east;
_vel_down = _buffer.velned.ned_down;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course_cd = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f;
_new_speed = true;
break;
default:
@ -539,10 +530,10 @@ AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_
_update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
_update_checksum((uint8_t *)msg, size, ck_a, ck_b);
_port->write((const uint8_t *)&header, sizeof(header));
_port->write((const uint8_t *)msg, size);
_port->write((const uint8_t *)&ck_a, 1);
_port->write((const uint8_t *)&ck_b, 1);
port->write((const uint8_t *)&header, sizeof(header));
port->write((const uint8_t *)msg, size);
port->write((const uint8_t *)&ck_a, 1);
port->write((const uint8_t *)&ck_b, 1);
}
@ -579,7 +570,7 @@ AP_GPS_UBLOX::_configure_navigation_rate(uint16_t rate_ms)
void
AP_GPS_UBLOX::_configure_gps(void)
{
_port->begin(38400U);
port->begin(38400U);
// start the process of updating the GPS rates
need_rate_update = true;
@ -597,7 +588,7 @@ AP_GPS_UBLOX::_configure_gps(void)
matches a UBlox
*/
bool
AP_GPS_UBLOX::_detect(struct detect_state &state, uint8_t data)
AP_GPS_UBLOX::_detect(struct UBLOX_detect_state &state, uint8_t data)
{
reset:
switch (state.step) {

38
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -19,12 +19,11 @@
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
//
// UBlox Lea6H protocol: http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
#ifndef __AP_GPS_UBLOX_H__
#define __AP_GPS_UBLOX_H__
#include <AP_HAL.h>
#include <AP_Common.h>
#include "GPS.h"
#include <AP_GPS.h>
/*
* try to put a UBlox into binary mode. This is in two parts.
@ -40,37 +39,15 @@
*/
#define UBLOX_SET_BINARY "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,38400,0*26\r\n"
class AP_GPS_UBLOX : public GPS
class AP_GPS_UBLOX : public AP_GPS_Backend
{
public:
AP_GPS_UBLOX() :
GPS(),
_step(0),
_msg_id(0),
_payload_length(0),
_payload_counter(0),
_fix_count(0),
need_rate_update(false),
_disable_counter(0),
next_fix(GPS::FIX_NONE),
rate_update_step(0),
_last_hw_status(0)
{}
AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// Methods
void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting nav_setting, DataFlash_Class *DataFlash);
bool read();
bool read();
float get_lag() const { return 0.2f; } // 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);
static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
private:
// u-blox UBX protocol essentials
@ -263,7 +240,7 @@ private:
bool _parse_gps();
// used to update fix between status and position packets
Fix_Status next_fix;
AP_GPS::GPS_Status next_fix;
uint8_t rate_update_step;
uint32_t _last_5hz_time;
@ -280,7 +257,6 @@ private:
void write_logging_headers(void);
void log_mon_hw(void);
void log_mon_hw2(void);
};
#endif // __AP_GPS_UBLOX_H__

229
libraries/AP_GPS/GPS.cpp
View File

@ -1,229 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Common.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include <AP_Notify.h>
#include "GPS.h"
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
GPS::GPS(void) :
// ensure all the inherited fields are zeroed
time_week_ms(0),
time_week(0),
latitude(0),
longitude(0),
altitude_cm(0),
ground_speed_cm(0),
ground_course_cd(0),
hdop(0),
num_sats(0),
new_data(false),
fix(FIX_NONE),
last_fix_time(0),
_have_raw_velocity(false),
_secondary_gps(false),
_last_gps_time(0),
_idleTimer(0),
_status(GPS::NO_FIX),
_last_ground_speed_cm(0),
_velocity_north(0),
_velocity_east(0),
_velocity_down(0)
{
}
void
GPS::update(void)
{
bool result;
uint32_t tnow;
// call the GPS driver to process incoming data
result = read();
tnow = hal.scheduler->millis();
// if we did not get a message, and the idle timer of 1.2 seconds has expired, re-init
if (!result) {
if ((tnow - _idleTimer) > 1200) {
Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer);
_status = NO_GPS;
init(_port, _nav_setting, _DataFlash);
// reset the idle timer
_idleTimer = tnow;
}
} else {
// we got a message, update our status correspondingly
if (fix == FIX_3D) {
_status = GPS_OK_FIX_3D;
}else if (fix == FIX_2D) {
_status = GPS_OK_FIX_2D;
}else{
_status = NO_FIX;
}
new_data = true;
// reset the idle timer
_idleTimer = tnow;
if (_status >= GPS_OK_FIX_2D) {
last_fix_time = _idleTimer;
_last_ground_speed_cm = ground_speed_cm;
if (_have_raw_velocity) {
// the GPS is able to give us velocity numbers directly
_velocity_north = _vel_north * 0.01f;
_velocity_east = _vel_east * 0.01f;
_velocity_down = _vel_down * 0.01f;
} else {
float gps_heading = ToRad(ground_course_cd * 0.01f);
float gps_speed = ground_speed_cm * 0.01f;
float sin_heading, cos_heading;
cos_heading = cosf(gps_heading);
sin_heading = sinf(gps_heading);
_velocity_north = gps_speed * cos_heading;
_velocity_east = gps_speed * sin_heading;
// no good way to get descent rate
_velocity_down = 0;
}
}
}
if (!_secondary_gps) {
// update notify with gps status
AP_Notify::flags.gps_status = _status;
}
}
void
GPS::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)
{
}
// XXX this is probably the wrong way to do it, too
void
GPS::_error(const char *msg)
{
hal.console->println(msg);
}
///
/// write a block of configuration data to a GPS
///
void GPS::_write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size)
{
while (size--) {
_fs->write(pgm_read_byte(pstr++));
}
}
int32_t GPS::_swapl(const void *bytes) const
{
const uint8_t *b = (const uint8_t *)bytes;
union {
int32_t v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return(u.v);
}
int16_t GPS::_swapi(const void *bytes) const
{
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
return(u.v);
}
/**
current time since the unix epoch in microseconds
This costs about 60 usec on AVR2560
*/
uint64_t GPS::time_epoch_usec(void)
{
if (_last_gps_time == 0) {
return 0;
}
const uint64_t ms_per_week = 7000ULL*86400ULL;
const uint64_t unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL;
uint64_t fix_time_ms = unix_offset + time_week*ms_per_week + time_week_ms;
// add in the milliseconds since the last fix
return (fix_time_ms + (hal.scheduler->millis() - _last_gps_time)) * 1000ULL;
}
/**
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
This function takes about 340 usec on the AVR2560
*/
void GPS::_make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
{
uint8_t year, mon, day, hour, min, sec;
uint16_t msec;
year = bcd_date % 100;
mon = (bcd_date / 100) % 100;
day = bcd_date / 10000;
msec = bcd_milliseconds % 1000;
uint32_t v = bcd_milliseconds;
msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100;
min = v % 100; v /= 100;
hour = v % 100; v /= 100;
int8_t rmon = mon - 2;
if (0 >= rmon) {
rmon += 12;
year -= 1;
}
// get time in seconds since unix epoch
uint32_t ret = (year/4) - 15 + 367*rmon/12 + day;
ret += year*365 + 10501;
ret = ret*24 + hour;
ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
time_week = ret / (7*86400UL);
time_week_ms = (ret % (7*86400UL)) * 1000;
time_week_ms += msec;
}

229
libraries/AP_GPS/GPS.h
View File

@ -1,229 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file GPS.h
/// @brief Interface definition for the various GPS drivers.
#ifndef __GPS_H__
#define __GPS_H__
#include <AP_HAL.h>
#include <inttypes.h>
#include <AP_Progmem.h>
#include <AP_Math.h>
class DataFlash_Class;
/// @class GPS
/// @brief Abstract base class for GPS receiver drivers.
class GPS
{
public:
GPS();
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
/// GPS drivers should not override this function; they should implement
/// ::read instead.
///
void update(void);
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons
///
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK_FIX_2D = 2, ///< Receiving valid messages and 2D lock
GPS_OK_FIX_3D = 3 ///< Receiving valid messages and 3D lock
};
/// Fix status codes
///
enum Fix_Status {
FIX_NONE = 0, ///< No fix
FIX_2D = 2, ///< 2d fix
FIX_3D = 3, ///< 3d fix
};
// GPS navigation engine settings. Not all GPS receivers support
// this
enum GPS_Engine_Setting {
GPS_ENGINE_NONE = -1,
GPS_ENGINE_PORTABLE = 0,
GPS_ENGINE_STATIONARY = 2,
GPS_ENGINE_PEDESTRIAN = 3,
GPS_ENGINE_AUTOMOTIVE = 4,
GPS_ENGINE_SEA = 5,
GPS_ENGINE_AIRBORNE_1G = 6,
GPS_ENGINE_AIRBORNE_2G = 7,
GPS_ENGINE_AIRBORNE_4G = 8
};
/// Query GPS status
///
/// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms.
///
/// @returns Current GPS status
///
GPS_Status status(void) const {
return _status;
}
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
/// Must be implemented by the GPS driver.
///
virtual void init(AP_HAL::UARTDriver *s, enum GPS_Engine_Setting engine_setting, DataFlash_Class *dataflash) = 0;
// Properties
uint32_t time_week_ms; ///< GPS time (milliseconds from start of GPS week)
uint16_t time_week; ///< GPS week number
int32_t latitude; ///< latitude in degrees * 10,000,000
int32_t longitude; ///< longitude in degrees * 10,000,000
int32_t altitude_cm; ///< altitude in cm
uint32_t ground_speed_cm; ///< ground speed in cm/sec
int32_t ground_course_cd; ///< ground course in 100ths of a degree
int16_t hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data:1;
Fix_Status fix; ///< 0 if we have no fix, 2 for 2D fix, 3 for 3D fix
// HIL support
virtual 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);
// components of velocity in 2D, in m/s
float velocity_north(void) const {
return _status >= GPS_OK_FIX_2D ? _velocity_north : 0;
}
float velocity_east(void) const {
return _status >= GPS_OK_FIX_2D ? _velocity_east : 0;
}
float velocity_down(void) const {
return _status >= GPS_OK_FIX_3D ? _velocity_down : 0;
}
// GPS velocity vector as NED in m/s
Vector3f velocity_vector(void) const {
return Vector3f(_velocity_north, _velocity_east, _velocity_down);
}
// last ground speed in m/s. This can be used when we have no GPS
// lock to return the last ground speed we had with lock
float last_ground_speed(void) {
return static_cast<float>(_last_ground_speed_cm) * 0.01f;
}
// the expected lag (in seconds) in the position and velocity readings from the gps
virtual float get_lag() const { return 0.5f; }
// the time we got our last fix in system milliseconds
uint32_t last_fix_time;
// the time we last processed a message in milliseconds
uint32_t last_message_time_ms(void) const { return _idleTimer; }
// return last fix time since the 1/1/1970 in microseconds
uint64_t time_epoch_usec(void);
// return true if the GPS supports vertical velocity values
bool have_vertical_velocity(void) const { return _have_raw_velocity; }
void set_secondary(void) { _secondary_gps = true; }
protected:
AP_HAL::UARTDriver *_port; ///< port the GPS is attached to
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.
///
/// @returns true if a valid message was received from the GPS
///
virtual bool read(void) = 0;
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
int32_t _swapl(const void *bytes) const;
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int16_t _swapi(const void *bytes) const;
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
/// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits
///
void _error(const char *msg);
enum GPS_Engine_Setting _nav_setting;
void _write_progstr_block(AP_HAL::UARTDriver *_fs, const prog_char *pstr, uint8_t size);
// velocities in cm/s if available from the GPS
int32_t _vel_north;
int32_t _vel_east;
int32_t _vel_down;
// does this GPS support raw velocity numbers?
bool _have_raw_velocity:1;
// this is a secondary GPS, disable notify updates
bool _secondary_gps:1;
// detected baudrate
uint16_t _baudrate;
// the time we got the last GPS timestamp
uint32_t _last_gps_time;
// return time in seconds since GPS epoch given time components
void _make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds);
DataFlash_Class *_DataFlash;
private:
/// Last time that the GPS driver got a good packet from the GPS
///
uint32_t _idleTimer;
/// Our current status
GPS_Status _status;
// previous ground speed in cm/s
uint32_t _last_ground_speed_cm;
// components of the velocity, in m/s
float _velocity_north;
float _velocity_east;
float _velocity_down;
};
#endif // __GPS_H__

132
libraries/AP_GPS/GPS_Backend.cpp Normal file
View File

@ -0,0 +1,132 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_GPS.h>
extern const AP_HAL::HAL& hal;
AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) :
port(_port),
gps(_gps),
state(_state)
{
}
int32_t AP_GPS_Backend::swap_int32(int32_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int32_t v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return u.v;
}
int16_t AP_GPS_Backend::swap_int16(int16_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
return u.v;
}
/**
calculate current time since the unix epoch in microseconds
This costs about 60 usec on AVR2560
*/
uint64_t AP_GPS::time_epoch_usec(uint8_t instance)
{
const GPS_State &istate = state[instance];
if (istate.last_gps_time_ms == 0) {
return 0;
}
const uint64_t ms_per_week = 7000ULL*86400ULL;
const uint64_t unix_offset = 17000ULL*86400ULL + 52*10*7000ULL*86400ULL - 15000ULL;
uint64_t fix_time_ms = unix_offset + istate.time_week*ms_per_week + istate.time_week_ms;
// add in the milliseconds since the last fix
return (fix_time_ms + (hal.scheduler->millis() - istate.last_gps_time_ms)) * 1000ULL;
}
/**
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
This function takes about 340 usec on the AVR2560
*/
void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
{
uint8_t year, mon, day, hour, min, sec;
uint16_t msec;
year = bcd_date % 100;
mon = (bcd_date / 100) % 100;
day = bcd_date / 10000;
msec = bcd_milliseconds % 1000;
uint32_t v = bcd_milliseconds;
msec = v % 1000; v /= 1000;
sec = v % 100; v /= 100;
min = v % 100; v /= 100;
hour = v % 100; v /= 100;
int8_t rmon = mon - 2;
if (0 >= rmon) {
rmon += 12;
year -= 1;
}
// get time in seconds since unix epoch
uint32_t ret = (year/4) - 15 + 367*rmon/12 + day;
ret += year*365 + 10501;
ret = ret*24 + hour;
ret = ret*60 + min;
ret = ret*60 + sec;
// convert to time since GPS epoch
ret -= 272764785UL;
// get GPS week and time
state.time_week = ret / (7*86400UL);
state.time_week_ms = (ret % (7*86400UL)) * 1000;
state.time_week_ms += msec;
}
/*
fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
*/
void AP_GPS_Backend::fill_3d_velocity(void)
{
float gps_heading = ToRad(state.ground_course_cd * 0.01f);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading);
state.velocity.z = 0;
state.have_vertical_velocity = false;
}

54
libraries/AP_GPS/GPS_Backend.h Normal file
View File

@ -0,0 +1,54 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
GPS driver backend class
*/
#ifndef __AP_GPS_BACKEND_H__
#define __AP_GPS_BACKEND_H__
class AP_GPS_Backend
{
public:
AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// The read() method is the only one needed in each driver. It
// should return true when the backend has successfully received a
// valid packet from the GPS.
virtual bool read() = 0;
protected:
AP_HAL::UARTDriver *port; ///< UART we are attached to
AP_GPS &gps; ///< access to frontend (for parameters)
AP_GPS::GPS_State &state; ///< public state for this instance
// common utility functions
int32_t swap_int32(int32_t v) const;
int16_t swap_int16(int16_t v) const;
/*
fill in 3D velocity from 2D components
*/
void fill_3d_velocity(void);
/*
fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time
*/
void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds);
};
#endif // __AP_GPS_BACKEND_H__

55
libraries/AP_GPS/GPS_detect_state.h Normal file
View File

@ -0,0 +1,55 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
GPS detection state structures. These need to be in a separate
header to prevent a circular dependency between AP_GPS and the
backend drivers.
These structures are allocated as a single block in AP_GPS during
driver detection, then freed once the detection is finished. Each
GPS driver needs to implement a static _detect() function which uses
this state information to detect if the attached GPS is of the
specific type that it handles.
*/
struct MTK19_detect_state {
uint8_t payload_counter;
uint8_t step;
uint8_t ck_a, ck_b;
};
struct MTK_detect_state {
uint8_t payload_counter;
uint8_t step;
uint8_t ck_a, ck_b;
};
struct NMEA_detect_state {
uint8_t step;
uint8_t ck;
};
struct SIRF_detect_state {
uint16_t checksum;
uint8_t step, payload_length, payload_counter;
};
struct UBLOX_detect_state {
uint8_t payload_length, payload_counter;
uint8_t step;
uint8_t ck_a, ck_b;
};