aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
4251ee0e4b
Ardupilot2/libraries/AP_GPS/AP_GPS_UBLOX.h
Michael du Breuil 4251ee0e4b AP_GPS: u-blox verify ublox settings 
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
2016-02-12 00:37:04 -08:00

540 lines
15 KiB
C++
Raw Blame History

// -*- 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/>.
*/
//
// u-blox UBX GPS driver for ArduPilot and ArduPilotMega.
// Code by Michael Smith, Jordi Munoz and Jose Julio, DIYDrones.com
//
// UBlox Lea6H protocol: http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
#ifndef __AP_GPS_UBLOX_H__
#define __AP_GPS_UBLOX_H__
#include <AP_HAL/AP_HAL.h>
#include "AP_GPS.h"
/*
* try to put a UBlox into binary mode. This is in two parts.
*
* First we send a ubx binary message that enables the NAV_SOL message
* at rate 1. Then we send a NMEA message to set the baud rate to our
* desired rate. The reason for doing the NMEA message second is if we
* send it first the second message will be ignored for a baud rate
* change.
* The reason we need the NAV_SOL rate message at all is some uBlox
* modules are configured with all ubx binary messages off, which
* would mean we would never detect it.
*/
#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"
#define UBLOX_SET_BINARY_RAW_BAUD "\265\142\006\001\003\000\001\006\001\022\117$PUBX,41,1,0003,0001,115200,0*1E\r\n"
#define UBLOX_RXM_RAW_LOGGING 1
#define UBLOX_MAX_RXM_RAW_SATS 22
#define UBLOX_MAX_RXM_RAWX_SATS 32
#define UBLOX_GNSS_SETTINGS 1
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
#define UBX_MSG_TYPES 2
#define UBLOX_MAX_PORTS 6
#define MEASURE_RATE 200
#define RATE_POSLLH 1
#define RATE_STATUS 1
#define RATE_SOL 1
#define RATE_VELNED 1
#define RATE_DOP 1
#define RATE_HW 5
#define RATE_HW2 5
#define CONFIG_RATE_NAV (1<<0)
#define CONFIG_RATE_POSLLH (1<<1)
#define CONFIG_RATE_STATUS (1<<2)
#define CONFIG_RATE_SOL (1<<3)
#define CONFIG_RATE_VELNED (1<<4)
#define CONFIG_RATE_DOP (1<<5)
#define CONFIG_RATE_MON_HW (1<<6)
#define CONFIG_RATE_MON_HW2 (1<<7)
#define CONFIG_RATE_RAW (1<<8)
#define CONFIG_VERSION (1<<9)
#define CONFIG_NAV_SETTINGS (1<<10)
#define CONFIG_GNSS (1<<11)
#define CONFIG_SBAS (1<<12)
#define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
| CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
//Configuration Sub-Sections
#define SAVE_CFG_IO (1<<0)
#define SAVE_CFG_MSG (1<<1)
#define SAVE_CFG_INF (1<<2)
#define SAVE_CFG_NAV (1<<3)
#define SAVE_CFG_RXM (1<<4)
#define SAVE_CFG_RINV (1<<9)
#define SAVE_CFG_ANT (1<<10)
#define SAVE_CFG_ALL (SAVE_CFG_IO|SAVE_CFG_MSG|SAVE_CFG_INF|SAVE_CFG_NAV|SAVE_CFG_RXM|SAVE_CFG_RINV|SAVE_CFG_ANT)
class AP_GPS_UBLOX : public AP_GPS_Backend
{
public:
AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
// Methods
bool read();
AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_DGPS; }
static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
bool is_configured(void) {
if (!gps._auto_config) {
return true;
} else {
return !_unconfigured_messages;
}
}
private:
// u-blox UBX protocol essentials
struct PACKED ubx_header {
uint8_t preamble1;
uint8_t preamble2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
};
#if UBLOX_GNSS_SETTINGS
struct PACKED ubx_cfg_gnss {
uint8_t msgVer;
uint8_t numTrkChHw;
uint8_t numTrkChUse;
uint8_t numConfigBlocks;
PACKED struct configBlock {
uint8_t gnssId;
uint8_t resTrkCh;
uint8_t maxTrkCh;
uint8_t reserved1;
uint32_t flags;
} configBlock[UBLOX_MAX_GNSS_CONFIG_BLOCKS];
};
#endif
struct PACKED ubx_cfg_nav_rate {
uint16_t measure_rate_ms;
uint16_t nav_rate;
uint16_t timeref;
};
struct PACKED ubx_cfg_msg {
uint8_t msg_class;
uint8_t msg_id;
};
struct PACKED ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
struct PACKED ubx_cfg_msg_rate_6 {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rates[6];
};
struct PACKED ubx_cfg_nav_settings {
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t res1;
uint32_t res2;
uint32_t res3;
uint32_t res4;
};
struct PACKED ubx_cfg_prt {
uint8_t portID;
};
struct PACKED ubx_cfg_sbas {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
};
struct PACKED ubx_nav_posllh {
uint32_t time; // GPS msToW
int32_t longitude;
int32_t latitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
uint32_t horizontal_accuracy;
uint32_t vertical_accuracy;
};
struct PACKED ubx_nav_status {
uint32_t time; // GPS msToW
uint8_t fix_type;
uint8_t fix_status;
uint8_t differential_status;
uint8_t res;
uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds
};
struct PACKED ubx_nav_dop {
uint32_t time; // GPS msToW
uint16_t gDOP;
uint16_t pDOP;
uint16_t tDOP;
uint16_t vDOP;
uint16_t hDOP;
uint16_t nDOP;
uint16_t eDOP;
};
struct PACKED ubx_nav_solution {
uint32_t time;
int32_t time_nsec;
uint16_t week;
uint8_t fix_type;
uint8_t fix_status;
int32_t ecef_x;
int32_t ecef_y;
int32_t ecef_z;
uint32_t position_accuracy_3d;
int32_t ecef_x_velocity;
int32_t ecef_y_velocity;
int32_t ecef_z_velocity;
uint32_t speed_accuracy;
uint16_t position_DOP;
uint8_t res;
uint8_t satellites;
uint32_t res2;
};
struct PACKED ubx_nav_velned {
uint32_t time; // GPS msToW
int32_t ned_north;
int32_t ned_east;
int32_t ned_down;
uint32_t speed_3d;
uint32_t speed_2d;
int32_t heading_2d;
uint32_t speed_accuracy;
uint32_t heading_accuracy;
};
// Lea6 uses a 60 byte message
struct PACKED ubx_mon_hw_60 {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[17];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
};
// Neo7 uses a 68 byte message
struct PACKED ubx_mon_hw_68 {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
};
struct PACKED ubx_mon_hw2 {
int8_t ofsI;
uint8_t magI;
int8_t ofsQ;
uint8_t magQ;
uint8_t cfgSource;
uint8_t reserved0[3];
uint32_t lowLevCfg;
uint32_t reserved1[2];
uint32_t postStatus;
uint32_t reserved2;
};
struct PACKED ubx_mon_ver {
char swVersion[30];
char hwVersion[10];
char extension; // extensions are not enabled
};
struct PACKED ubx_nav_svinfo_header {
uint32_t itow;
uint8_t numCh;
uint8_t globalFlags;
uint16_t reserved;
};
#if UBLOX_RXM_RAW_LOGGING
struct PACKED ubx_rxm_raw {
int32_t iTOW;
int16_t week;
uint8_t numSV;
uint8_t reserved1;
struct ubx_rxm_raw_sv {
double cpMes;
double prMes;
float doMes;
uint8_t sv;
int8_t mesQI;
int8_t cno;
uint8_t lli;
} svinfo[UBLOX_MAX_RXM_RAW_SATS];
};
struct PACKED ubx_rxm_rawx {
double rcvTow;
uint16_t week;
int8_t leapS;
uint8_t numMeas;
uint8_t recStat;
uint8_t reserved1[3];
PACKED struct ubx_rxm_rawx_sv {
double prMes;
double cpMes;
float doMes;
uint8_t gnssId;
uint8_t svId;
uint8_t reserved2;
uint8_t freqId;
uint16_t locktime;
uint8_t cno;
uint8_t prStdev;
uint8_t cpStdev;
uint8_t doStdev;
uint8_t trkStat;
uint8_t reserved3;
} svinfo[UBLOX_MAX_RXM_RAWX_SATS];
};
#endif
struct PACKED ubx_ack_ack {
uint8_t clsID;
uint8_t msgID;
};
struct PACKED ubx_cfg_cfg {
uint32_t clearMask;
uint32_t saveMask;
uint32_t loadMask;
};
// Receive buffer
union PACKED {
ubx_nav_posllh posllh;
ubx_nav_status status;
ubx_nav_dop dop;
ubx_nav_solution solution;
ubx_nav_velned velned;
ubx_cfg_msg_rate msg_rate;
ubx_cfg_msg_rate_6 msg_rate_6;
ubx_cfg_nav_settings nav_settings;
ubx_cfg_nav_rate nav_rate;
ubx_cfg_prt prt;
ubx_mon_hw_60 mon_hw_60;
ubx_mon_hw_68 mon_hw_68;
ubx_mon_hw2 mon_hw2;
ubx_mon_ver mon_ver;
#if UBLOX_GNSS_SETTINGS
ubx_cfg_gnss gnss;
#endif
ubx_cfg_sbas sbas;
ubx_nav_svinfo_header svinfo_header;
#if UBLOX_RXM_RAW_LOGGING
ubx_rxm_raw rxm_raw;
ubx_rxm_rawx rxm_rawx;
#endif
ubx_ack_ack ack;
uint8_t bytes[];
} _buffer;
enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
CLASS_NAV = 0x01,
CLASS_ACK = 0x05,
CLASS_CFG = 0x06,
CLASS_MON = 0x0A,
CLASS_RXM = 0x02,
MSG_ACK_NACK = 0x00,
MSG_ACK_ACK = 0x01,
MSG_POSLLH = 0x2,
MSG_STATUS = 0x3,
MSG_DOP = 0x4,
MSG_SOL = 0x6,
MSG_VELNED = 0x12,
MSG_CFG_CFG = 0x09,
MSG_CFG_RATE = 0x08,
MSG_CFG_MSG = 0x01,
MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_PRT = 0x00,
MSG_CFG_SBAS = 0x16,
MSG_CFG_GNSS = 0x3E,
MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B,
MSG_MON_VER = 0x04,
MSG_NAV_SVINFO = 0x30,
MSG_RXM_RAW = 0x10,
MSG_RXM_RAWX = 0x15
};
enum ubx_gnss_identifier {
GNSS_GPS = 0x00,
GNSS_SBAS = 0x01,
GNSS_GALILEO = 0x02,
GNSS_BEIDOU = 0x03,
GNSS_IMES = 0x04,
GNSS_QZSS = 0x05,
GNSS_GLONASS = 0x06
};
enum ubs_nav_fix_type {
FIX_NONE = 0,
FIX_DEAD_RECKONING = 1,
FIX_2D = 2,
FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5
};
enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1,
NAV_STATUS_DGPS_USED = 2
};
enum ubx_hardware_version {
ANTARIS = 0,
UBLOX_5,
UBLOX_6,
UBLOX_7,
UBLOX_M8
};
enum config_step {
STEP_RATE_NAV = 0,
STEP_RATE_POSLLH,
STEP_RATE_VELNED,
STEP_PORT,
STEP_POLL_SVINFO,
STEP_POLL_SBAS,
STEP_POLL_NAV,
STEP_POLL_GNSS,
STEP_NAV_RATE,
STEP_POSLLH,
STEP_STATUS,
STEP_SOL,
STEP_VELNED,
STEP_DOP,
STEP_MON_HW,
STEP_MON_HW2,
STEP_RAW,
STEP_RAWX,
STEP_VERSION,
STEP_LAST
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _msg_id;
uint16_t _payload_length;
uint16_t _payload_counter;
uint8_t _class;
bool _cfg_saved;
uint32_t _last_vel_time;
uint32_t _last_pos_time;
uint32_t _last_cfg_sent_time;
uint8_t _num_cfg_save_tries;
uint32_t _last_config_time;
uint16_t _delay_time;
uint8_t _next_message;
uint8_t _ublox_port;
bool _have_version;
uint32_t _unconfigured_messages;
uint8_t _hardware_generation;
// do we have new position information?
bool _new_position:1;
// do we have new speed information?
bool _new_speed:1;
uint8_t _disable_counter;
// Buffer parse & GPS state update
bool _parse_gps();
// used to update fix between status and position packets
AP_GPS::GPS_Status next_fix;
uint32_t _last_5hz_time;
bool noReceivedHdop;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_rate(void);
void _configure_sbas(bool enable);
void _update_checksum(uint8_t *data, uint16_t len, uint8_t &ck_a, uint8_t &ck_b);
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint16_t size);
void send_next_rate_update(void);
bool _request_message_rate(uint8_t msg_class, uint8_t msg_id);
void _request_navigation_rate(void);
void _request_next_config(void);
void _request_port(void);
void _request_version(void);
void _save_cfg(void);
void _verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void unexpected_message(void);
void write_logging_headers(void);
void log_mon_hw(void);
void log_mon_hw2(void);
void log_mon_ver(void);
void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
// Calculates the correct log message ID based on what GPS instance is being logged
uint8_t _ubx_msg_log_index(uint8_t ubx_msg) {
return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES));
}
};
#endif // __AP_GPS_UBLOX_H__
Reference in New Issue View Git Blame Copy Permalink