25 #include "pprzlink/dl_protocol.h"
28 #ifndef USE_GPS_UBX_RTCM
29 #define USE_GPS_UBX_RTCM 0
35 #define RTCM3_PREAMBLE 0xD3
36 #define RTCM3_MSG_1005 0x69
37 #define RTCM3_MSG_1077 0xB1
38 #define RTCM3_MSG_4072 0x72
39 #define RTCM3_MSG_1230 0xE6
40 #define RTCM3_MSG_1087 0xBB
43 #if PRINT_DEBUG_GPS_UBX
44 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
46 #define DEBUG_PRINT(...) {}
50 #include "ubx_protocol.h"
61 #define GOT_CHECKSUM1 8
63 #define RXM_RTCM_VERSION 0x02
64 #define NAV_RELPOSNED_VERSION 0x01
66 #define GPS_UBX_ERR_NONE 0
67 #define GPS_UBX_ERR_OVERRUN 1
68 #define GPS_UBX_ERR_MSG_TOO_LONG 2
69 #define GPS_UBX_ERR_CHECKSUM 3
70 #define GPS_UBX_ERR_UNEXPECTED 4
71 #define GPS_UBX_ERR_OUT_OF_SYNC 5
73 #define UTM_HEM_NORTH 0
74 #define UTM_HEM_SOUTH 1
78 #if USE_GPS_UBX_RXM_RAW
79 struct GpsUbxRaw gps_ubx_raw;
86 #ifndef INJECT_BUFF_SIZE
87 #define INJECT_BUFF_SIZE 1024 + 6
96 struct rtcm_t rtcm = { 0 };
100 #ifndef GPS_UBX_BOOTRESET
101 #define GPS_UBX_BOOTRESET 0
119 struct link_device *
dev = &((UBX_GPS_LINK).device);
121 while (
dev->char_available(
dev->periph)) {
148 uint8_t l = DL_HITL_UBX_ubx_payload_length(buf);
149 uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf);
160 uint8_t gnssFixOK = bit_is_set(flags, 0);
161 uint8_t diffSoln = bit_is_set(flags, 1);
162 uint8_t carrSoln = (flags & 0xC0) >> 6;
163 if (diffSoln && carrSoln == 2) {
165 }
else if(diffSoln && carrSoln == 1) {
167 }
else if(gnssFixOK) {
401 if (diffSoln && carrSoln == 2) {
403 }
else if(diffSoln && carrSoln == 1) {
405 }
else if(gnssFixOK) {
433 #if USE_GPS_UBX_RXM_RAW
439 for (i = 0; i < max_SV; i++) {
440 gps_ubx_raw.measures[i].cpMes = UBX_RXM_RAW_cpMes(
gps_ubx.
msg_buf, i);
441 gps_ubx_raw.measures[i].prMes = UBX_RXM_RAW_prMes(
gps_ubx.
msg_buf, i);
442 gps_ubx_raw.measures[i].doMes = UBX_RXM_RAW_doMes(
gps_ubx.
msg_buf, i);
444 gps_ubx_raw.measures[i].mesQI = UBX_RXM_RAW_mesQI(
gps_ubx.
msg_buf, i);
445 gps_ubx_raw.measures[i].cno = UBX_RXM_RAW_cno(
gps_ubx.
msg_buf, i);
446 gps_ubx_raw.measures[i].lli = UBX_RXM_RAW_lli(
gps_ubx.
msg_buf, i);
483 DEBUG_PRINT(
"Unknown RXM_RTCM version: %i\n", version);
493 case UBX_NAV_POSECEF_ID:
496 case UBX_NAV_POSLLH_ID:
499 case UBX_NAV_STATUS_ID:
508 case UBX_NAV_POSUTM_ID:
511 case UBX_NAV_VELECEF_ID:
514 case UBX_NAV_VELNED_ID:
517 case UBX_NAV_SVINFO_ID:
523 case UBX_NAV_RELPOSNED_ID:
535 case UBX_RXM_RTCM_ID:
561 if (c == UBX_SYNC1) {
566 if (c != UBX_SYNC2) {
636 dev->put_byte(
dev->periph, 0,
byte);
643 dev->put_byte(
dev->periph, 0, UBX_SYNC1);
644 dev->put_byte(
dev->periph, 0, UBX_SYNC2);
657 dev->send_message(
dev->periph, 0);
663 for (i = 0; i < len; i++) {
671 UbxSend_CFG_RST(
dev, bbr, reset_mode, 0x00);
675 #ifndef GPS_UBX_UCENTER
676 #define gps_ubx_ucenter_event() {}
719 for (i = 0; i < n; i++) {
722 dev->send_message(
dev->periph, 0);
739 #ifdef GPS_UBX_UCENTER
747 for (i = 0; i < length; i++) {
748 if (rtcm.nbyte == 0) {
751 rtcm.buff[rtcm.nbyte++] = data[i];
755 if (rtcm.nbyte < INJECT_BUFF_SIZE) {
756 rtcm.buff[rtcm.nbyte++] = data[i];
757 if (rtcm.nbyte == 3) {
762 if (rtcm.nbyte == rtcm.len + 3) {
764 unsigned int crc1 =
crc24q(rtcm.buff, rtcm.len);
765 unsigned int crc2 =
RTCMgetbitu(rtcm.buff, rtcm.len * 8, 24);
776 default:
DEBUG_PRINT(
"Unknown type: %i", packet_id);
break;
779 DEBUG_PRINT(
"Skipping message %i (CRC failed) - %d", packet_id, rtcm.buff[0]);
781 for (j = 1; j < rtcm.len; j++) {
unsigned int RTCMgetbitu(unsigned char *buff, int pos, int lenb)
unsigned int crc24q(const unsigned char *buff, int len)
Main include for ABI (AirBorneInterface).
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
uint16_t gps_week_number(uint16_t year, uint8_t month, uint8_t day)
Number of weeks since navigation epoch (6 January 1980)
void WEAK gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
Default parser for GPS injected data.
struct GpsRelposNED gps_relposned
int16_t azim
azimuth in deg
uint32_t tow
GPS time of week in ms.
uint8_t qi
quality bitfield (GPS receiver specific)
int32_t hmsl
height above mean sea level (MSL) in mm
int8_t elev
elevation in deg
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
uint32_t sacc
speed accuracy in cm/s
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over MSL)
uint32_t cacc
course accuracy in rad*1e7
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
#define GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_NED_BIT
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
struct EcefCoor_i ecef_pos
position in ECEF in cm
uint32_t hacc
horizontal accuracy in cm
#define GPS_VALID_POS_LLA_BIT
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
uint16_t pdop
position dilution of precision scaled by 100
#define GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_HMSL_BIT
struct NedCoor_i ned_vel
speed NED in cm/s
uint32_t last_msg_time
cpu time in sec at last received GPS message
uint8_t svid
Satellite ID.
uint8_t nb_channels
Number of scanned satellites.
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
uint32_t pacc
position accuracy in cm
uint16_t gspeed
norm of 2d ground speed in cm/s
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
uint8_t comp_id
id of current gps
uint32_t vacc
vertical accuracy in cm
#define GPS_FIX_3D
3D GPS fix
uint16_t speed_3d
norm of 3d speed in cm/s
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
uint8_t flags
bitfield with GPS receiver specific flags
#define GPS_VALID_COURSE_BIT
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
#define GPS_VALID_POS_UTM_BIT
uint8_t num_sv
number of sat in fix
data structures for GPS with RTK capabilities
void ubx_trailer(struct link_device *dev)
void gps_ubx_parse_HITL_UBX(uint8_t *buf)
void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len)
#define GPS_UBX_ERR_UNEXPECTED
#define GPS_UBX_ERR_OVERRUN
#define GPS_UBX_BOOTRESET
void gps_ubx_parse(uint8_t c)
static void gps_ubx_parse_nav_svinfo(void)
static void gps_ubx_parse_nav_sat(void)
void gps_ubx_read_message(void)
#define NAV_RELPOSNED_VERSION
static void ubx_send_1byte(struct link_device *dev, uint8_t byte)
void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr, uint8_t reset_mode)
void gps_ublox_write(struct link_device *dev, uint8_t *buff, uint32_t n)
static void gps_ubx_parse_nav_posecef(void)
#define GPS_UBX_ERR_MSG_TOO_LONG
#define UNINIT
Includes macros generated from ubx.xml.
static void gps_ubx_parse_nav_pvt(void)
#define GPS_UBX_ERR_CHECKSUM
static void gps_ubx_parse_nav_status(void)
static void gps_ubx_parse_nav_sol(void)
static void gps_ubx_parse_velecef(void)
#define gps_ubx_ucenter_event()
static void gps_ubx_parse_nav_velned(void)
static void gps_ubx_parse_nav_relposned(void)
static void gps_ubx_parse_rxm_raw(void)
static void gps_ubx_parse_nav_posutm(void)
void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes)
static void gps_ubx_parse_nav_posllh(void)
static void gps_ubx_parse_rxm_rtcm(void)
#define GPS_UBX_ERR_OUT_OF_SYNC
UBX protocol specific code.
#define CFG_RST_BBR_Coldstart
#define GPS_UBX_NB_CHANNELS
#define CFG_RST_Reset_Controlled
#define CFG_RST_BBR_Hotstart
#define CFG_RST_BBR_Warmstart
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD]
#define GPS_UBX_MAX_PAYLOAD
int gps_ubx_ucenter_get_status(void)
int32_t lat
in degrees*1e7
int32_t alt
in millimeters (above WGS84 reference ellipsoid or above MSL)
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint8_t zone
UTM zone number.
int32_t east
in centimeters
int32_t lon
in degrees*1e7
int32_t north
in centimeters
uint8_t buff[25]
Buffer used for general comunication over SPI (in buffer)
arch independent LED (Light Emitting Diodes) API
static const struct usb_device_descriptor dev
volatile uint32_t nb_sec
full seconds since startup
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.