Browse Source
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 objectmaster
Andrew Tridgell
11 years ago
24 changed files with 1006 additions and 1217 deletions
@ -0,0 +1,246 @@
@@ -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); |
||||
} |
||||
@ -1,15 +1,287 @@
@@ -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__
|
||||
|
||||
@ -1,184 +0,0 @@
@@ -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; |
||||
} |
||||
} |
||||
} |
||||
|
||||
@ -1,76 +0,0 @@
@@ -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 |
||||
@ -1,67 +0,0 @@
@@ -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; |
||||
} |
||||
|
||||
@ -1,56 +0,0 @@
@@ -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__
|
||||
@ -1,21 +0,0 @@
@@ -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__
|
||||
@ -1,229 +0,0 @@
@@ -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; |
||||
} |
||||
@ -1,229 +0,0 @@
@@ -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__
|
||||
@ -0,0 +1,132 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,54 @@
@@ -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__
|
||||
@ -0,0 +1,55 @@
@@ -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; |
||||
}; |
||||
Loading…
Reference in new issue