4251ee0e4b
The ublox driver will now continuosly poll for the settings from the GPS and correct any that are found to be in correct.
This status is then reported to the arming library as an additional arming check, allowing the user to be sure that the
gps is correctly configured before using it. If a user has a GPS2 configured that is not present they will fail the arming
checks until after they have disabled the second GPS.
2 new parameters were introduced as well:
-GPS_AUTO_CONFIG: Will not request any configuration packets to attempt to change them. (If the packet is recieved then
a update will be sent to it, but in testing this scenario never occured. This is set to 1 or 0 to change the setting.
(Defaults to 1 enabling auto config)
-GPS_GNSS_MODE2: Behaves the same way as GPS_GNSS_MODE but only applies to the second GPS.
GPS drivers are now allowed 2 seconds of non responsiveness before being unloaded
412 lines
13 KiB
C++
412 lines
13 KiB
C++
// -*- 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/>.
|
|
*/
|
|
|
|
#ifndef __AP_GPS_H__
|
|
#define __AP_GPS_H__
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <inttypes.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <AP_Vehicle/AP_Vehicle.h>
|
|
#include "GPS_detect_state.h"
|
|
#include <AP_SerialManager/AP_SerialManager.h>
|
|
|
|
/**
|
|
maximum number of GPS instances available on this platform. If more
|
|
than 1 then redundent sensors may be available
|
|
*/
|
|
#define GPS_MAX_INSTANCES 2
|
|
#define GPS_RTK_INJECT_TO_ALL 127
|
|
|
|
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, const AP_SerialManager& serial_manager);
|
|
|
|
/// 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_TYPE_HIL = 7,
|
|
GPS_TYPE_SBP = 8,
|
|
GPS_TYPE_PX4 = 9,
|
|
GPS_TYPE_SBF = 10,
|
|
GPS_TYPE_GSOF = 11,
|
|
GPS_TYPE_QURT = 12,
|
|
};
|
|
|
|
/// 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_OK_FIX_3D_DGPS = 4, ///< Receiving valid messages and 3D lock with differential improvements
|
|
GPS_OK_FIX_3D_RTK = 5, ///< Receiving valid messages and 3D lock, with relative-positioning improvements
|
|
};
|
|
|
|
// 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
|
|
};
|
|
|
|
enum GPS_Config {
|
|
GPS_ALL_CONFIGURED = 255
|
|
};
|
|
|
|
/*
|
|
The GPS_State structure is filled in by the backend driver as it
|
|
parses each message from the GPS.
|
|
*/
|
|
struct GPS_State {
|
|
uint8_t instance; // the instance number of this GPS
|
|
|
|
// 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
|
|
uint16_t hdop; ///< horizontal dilution of precision in cm
|
|
uint16_t vdop; ///< vertical dilution of precision in cm
|
|
uint8_t num_sats; ///< Number of visible satelites
|
|
Vector3f velocity; ///< 3D velocitiy in m/s, in NED format
|
|
float speed_accuracy;
|
|
float horizontal_accuracy;
|
|
float vertical_accuracy;
|
|
bool have_vertical_velocity:1; ///< does this GPS give vertical velocity?
|
|
bool have_speed_accuracy:1;
|
|
bool have_horizontal_accuracy:1;
|
|
bool have_vertical_accuracy:1;
|
|
uint32_t last_gps_time_ms; ///< the system time we got the last GPS timestamp, milliseconds
|
|
};
|
|
|
|
// Accessor functions
|
|
|
|
// return number of active GPS sensors. Note that if the first GPS
|
|
// is not present but the 2nd is then we return 2
|
|
uint8_t num_sensors(void) const {
|
|
return num_instances;
|
|
}
|
|
|
|
uint8_t primary_sensor(void) const {
|
|
return primary_instance;
|
|
}
|
|
|
|
/// Query GPS status
|
|
GPS_Status status(uint8_t instance) const {
|
|
return state[instance].status;
|
|
}
|
|
GPS_Status status(void) const {
|
|
return status(primary_instance);
|
|
}
|
|
|
|
// Query the highest status this GPS supports
|
|
GPS_Status highest_supported_status(uint8_t instance) const;
|
|
GPS_Status highest_supported_status(void) const;
|
|
|
|
// location of last fix
|
|
const Location &location(uint8_t instance) const {
|
|
return state[instance].location;
|
|
}
|
|
const Location &location() const {
|
|
return location(primary_instance);
|
|
}
|
|
|
|
bool speed_accuracy(uint8_t instance, float &sacc) const {
|
|
if(state[instance].have_speed_accuracy) {
|
|
sacc = state[instance].speed_accuracy;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool speed_accuracy(float &sacc) const {
|
|
return speed_accuracy(primary_instance, sacc);
|
|
}
|
|
|
|
bool horizontal_accuracy(uint8_t instance, float &hacc) const {
|
|
if(state[instance].have_horizontal_accuracy) {
|
|
hacc = state[instance].horizontal_accuracy;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool horizontal_accuracy(float &hacc) const {
|
|
return horizontal_accuracy(primary_instance, hacc);
|
|
}
|
|
|
|
bool vertical_accuracy(uint8_t instance, float &vacc) const {
|
|
if(state[instance].have_vertical_accuracy) {
|
|
vacc = state[instance].vertical_accuracy;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool vertical_accuracy(float &vacc) const {
|
|
return vertical_accuracy(primary_instance, vacc);
|
|
}
|
|
|
|
// 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
|
|
uint16_t get_hdop(uint8_t instance) const {
|
|
return state[instance].hdop;
|
|
}
|
|
uint16_t get_hdop() const {
|
|
return get_hdop(primary_instance);
|
|
}
|
|
|
|
// vertical dilution of precision
|
|
uint16_t get_vdop(uint8_t instance) const {
|
|
return state[instance].vdop;
|
|
}
|
|
uint16_t get_vdop() const {
|
|
return get_vdop(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(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms,
|
|
const Location &location, const Vector3f &velocity, uint8_t num_sats,
|
|
uint16_t hdop, bool _have_vertical_velocity);
|
|
|
|
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;
|
|
AP_Int8 _auto_switch;
|
|
AP_Int8 _min_dgps;
|
|
AP_Int16 _sbp_logmask;
|
|
AP_Int8 _inject_to;
|
|
uint32_t _last_instance_swap_ms;
|
|
AP_Int8 _sbas_mode;
|
|
AP_Int8 _min_elevation;
|
|
AP_Int8 _raw_data;
|
|
AP_Int8 _gnss_mode[2];
|
|
AP_Int8 _save_config;
|
|
AP_Int8 _auto_config;
|
|
|
|
// handle sending of initialisation strings to the GPS
|
|
void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
|
|
void send_blob_update(uint8_t instance);
|
|
|
|
// lock out a GPS port, allowing another application to use the port
|
|
void lock_port(uint8_t instance, bool locked);
|
|
|
|
//Inject a packet of raw binary to a GPS
|
|
void inject_data(uint8_t *data, uint8_t len);
|
|
void inject_data(uint8_t instance, uint8_t *data, uint8_t len);
|
|
|
|
//MAVLink Status Sending
|
|
void send_mavlink_gps_raw(mavlink_channel_t chan);
|
|
void send_mavlink_gps2_raw(mavlink_channel_t chan);
|
|
|
|
void send_mavlink_gps_rtk(mavlink_channel_t chan);
|
|
void send_mavlink_gps2_rtk(mavlink_channel_t chan);
|
|
|
|
// Returns the index of the first unconfigured GPS (returns GPS_ALL_CONFIGURED if all instances report as being configured)
|
|
uint8_t first_unconfigured_gps(void) const;
|
|
|
|
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];
|
|
AP_HAL::UARTDriver *_port[GPS_MAX_INSTANCES];
|
|
|
|
/// primary GPS instance
|
|
uint8_t primary_instance:2;
|
|
|
|
/// number of GPS instances present
|
|
uint8_t num_instances:2;
|
|
|
|
// which ports are locked
|
|
uint8_t locked_ports:2;
|
|
|
|
// 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;
|
|
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;
|
|
struct SBP_detect_state sbp_detect_state;
|
|
} detect_state[GPS_MAX_INSTANCES];
|
|
|
|
struct {
|
|
const char *blob;
|
|
uint16_t remaining;
|
|
} initblob_state[GPS_MAX_INSTANCES];
|
|
|
|
static const uint32_t _baudrates[];
|
|
static const char _initialisation_blob[];
|
|
static const char _initialisation_raw_blob[];
|
|
|
|
void detect_instance(uint8_t instance);
|
|
void update_instance(uint8_t instance);
|
|
};
|
|
|
|
#define GPS_BAUD_TIME_MS 1200
|
|
|
|
#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"
|
|
#include "AP_GPS_SBP.h"
|
|
#include "AP_GPS_PX4.h"
|
|
#include "AP_GPS_QURT.h"
|
|
#include "AP_GPS_SBF.h"
|
|
#include "AP_GPS_GSOF.h"
|
|
|
|
#endif // __AP_GPS_H__
|