diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp new file mode 100644 index 0000000000..617e4407e0 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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 . + */ + +#include +#include +#include +#include +#include + +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; imillis(); + 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); +} diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 0a440e0606..30095b0585 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -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 . + */ +#ifndef __AP_GPS_H__ +#define __AP_GPS_H__ +#include +#include +#include +#include +#include +#include +#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 +#include +#include +#include +#include +#include + +#endif // __AP_GPS_H__ diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp deleted file mode 100644 index 9bfd6d4c0b..0000000000 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ /dev/null @@ -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 -#include - -#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; - } - } -} - diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h deleted file mode 100644 index b90b30b77c..0000000000 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ /dev/null @@ -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 -#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 diff --git a/libraries/AP_GPS/AP_GPS_Glitch.cpp b/libraries/AP_GPS/AP_GPS_Glitch.cpp index b06e9a57f8..ce64c72de6 100644 --- a/libraries/AP_GPS/AP_GPS_Glitch.cpp +++ b/libraries/AP_GPS/AP_GPS_Glitch.cpp @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_Glitch.h b/libraries/AP_GPS/AP_GPS_Glitch.h index 63e72b23cb..5e7a2ea2a0 100644 --- a/libraries/AP_GPS/AP_GPS_Glitch.h +++ b/libraries/AP_GPS/AP_GPS_Glitch.h @@ -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 { diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp deleted file mode 100644 index 9f628d5b6c..0000000000 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ /dev/null @@ -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 . - */ - -// -// Hardware in the loop gps class. -// Code by James Goppert -// -// GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" -// - -#include -#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; -} - diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h deleted file mode 100644 index 40d99b4332..0000000000 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ /dev/null @@ -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 . - */ - -// -// Hardware in the loop gps class. -// Code by James Goppert -// -// -#ifndef __AP_GPS_HIL_H__ -#define __AP_GPS_HIL_H__ - -#include -#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__ diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 7ab0bf7d68..8b0edb4c74 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -21,37 +21,31 @@ // GPS configuration : Custom protocol per "DIYDrones Custom Binary Sentence Specification V1.1" // -#include - -#include - +#include #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; - - if (fix >= GPS::FIX_2D) { - _make_gps_time(0, _swapl(&_buffer.msg.utc_time)*10); + 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 (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: diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index a66f399c32..1dc78631cd 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -25,28 +25,16 @@ #ifndef __AP_GPS_MTK_H__ #define __AP_GPS_MTK_H__ -#include "GPS.h" -#include +#include #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 { diff --git a/libraries/AP_GPS/AP_GPS_MTK19.cpp b/libraries/AP_GPS/AP_GPS_MTK19.cpp index 23a5c0a77e..336f76bbad 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK19.cpp @@ -23,34 +23,32 @@ // Note that this driver supports both the 1.6 and 1.9 protocol varients // -#include #include "AP_GPS_MTK19.h" -#include 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) { diff --git a/libraries/AP_GPS/AP_GPS_MTK19.h b/libraries/AP_GPS/AP_GPS_MTK19.h index cf5d93ff74..c1323fb44d 100644 --- a/libraries/AP_GPS/AP_GPS_MTK19.h +++ b/libraries/AP_GPS/AP_GPS_MTK19.h @@ -23,34 +23,19 @@ #ifndef AP_GPS_MTK19_h #define AP_GPS_MTK19_h -#include "GPS.h" -#include +#include #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 { diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 39dacb7477..f3690559a8 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -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: diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 5c19b7e263..0fd4a29736 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -47,42 +47,21 @@ #ifndef __AP_GPS_NMEA_H__ #define __AP_GPS_NMEA_H__ -#include -#include "GPS.h" -#include - +#include /// 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 diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h deleted file mode 100644 index 143f85dbd0..0000000000 --- a/libraries/AP_GPS/AP_GPS_None.h +++ /dev/null @@ -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 -#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__ diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index abb5042d77..cfa4426086 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -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: diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index 11e4a87697..b24c371dd1 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -23,30 +23,17 @@ #include #include -#include "GPS.h" +#include #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 { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index c2b6f21dc2..a4b17128a4 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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 - -#include +#include +#include "AP_GPS_UBLOX.h" #include #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) { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 6f67a84256..3e4ec4cb4f 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -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 -#include -#include "GPS.h" +#include /* * 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(); - - float get_lag() const { return 0.2f; } // ublox lag is lower than the default 1second + bool read(); - // 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__ diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp deleted file mode 100644 index 39aa1cd651..0000000000 --- a/libraries/AP_GPS/GPS.cpp +++ /dev/null @@ -1,229 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - - - -#include -#include -#include -#include -#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; -} diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h deleted file mode 100644 index d48b90887a..0000000000 --- a/libraries/AP_GPS/GPS.h +++ /dev/null @@ -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 -#include -#include -#include - -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(_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__ diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp new file mode 100644 index 0000000000..d1e5c86aa1 --- /dev/null +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -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 . + */ + +#include + +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; +} diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h new file mode 100644 index 0000000000..78f0e6a675 --- /dev/null +++ b/libraries/AP_GPS/GPS_Backend.h @@ -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 . + */ + +/* + 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__ diff --git a/libraries/AP_GPS/GPS_detect_state.h b/libraries/AP_GPS/GPS_detect_state.h new file mode 100644 index 0000000000..8875211e63 --- /dev/null +++ b/libraries/AP_GPS/GPS_detect_state.h @@ -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 . + */ + +/* + 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; +};