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
41#define RTCM3_MSG_1097 0xC5
46#if PRINT_DEBUG_GPS_UBX
48#define DEBUG_PRINT(...) printf(__VA_ARGS__)
50#define DEBUG_PRINT(...) {}
54#include "ubx_protocol.h"
65#define GOT_CHECKSUM1 8
67#define RXM_RTCM_VERSION 0x02
68#define NAV_RELPOSNED_VERSION 0x01
69#define NAV_DAHEADING_VERSION 0x02
70#define NAV_DAHEADING_REF_ID 0xFFFF
73#ifndef GPS_UBX_HEADING_BASELINE
74#define GPS_UBX_HEADING_BASELINE 0.
78#ifndef GPS_UBX_HEADING_BASELINE_TOL
79#define GPS_UBX_HEADING_BASELINE_TOL 0.4
83#ifndef GPS_UBX_HEADING_BASELINE_MAX
84#define GPS_UBX_HEADING_BASELINE_MAX 10.
87#define GPS_UBX_ERR_NONE 0
88#define GPS_UBX_ERR_OVERRUN 1
89#define GPS_UBX_ERR_MSG_TOO_LONG 2
90#define GPS_UBX_ERR_CHECKSUM 3
91#define GPS_UBX_ERR_UNEXPECTED 4
92#define GPS_UBX_ERR_OUT_OF_SYNC 5
94#define UTM_HEM_NORTH 0
95#define UTM_HEM_SOUTH 1
103extern struct RtcmMan rtcm_man;
105#ifndef INJECT_BUFF_SIZE
106#define INJECT_BUFF_SIZE 1024 + 6
113 uint8_t buff[INJECT_BUFF_SIZE];
115struct rtcm_t rtcm = { 0 };
122#ifndef GPS_UBX_BOOTRESET
123#define GPS_UBX_BOOTRESET 0
126#define CFG_RST_Reset_Hardware 0x00
127#define CFG_RST_Reset_Controlled 0x01
128#define CFG_RST_Reset_Controlled_GPS_only 0x02
129#define CFG_RST_Reset_Controlled_GPS_stop 0x08
130#define CFG_RST_Reset_Controlled_GPS_start 0x09
132#define CFG_RST_BBR_Hotstart 0x0000
133#define CFG_RST_BBR_Warmstart 0x0001
134#define CFG_RST_BBR_Coldstart 0xffff
142 gps_ubx[0].dev = &(UBX_GPS_PORT).device;
145 gps_ubx[1].dev = &(UBX2_GPS_PORT).device;
151 gps_ubx[i].msg_available =
false;
164 while (
dev->char_available(
dev->periph)) {
166 if (
gps_ubx[i].msg_available) {
188 if (
gps_ubx[0].msg_available) {
192 gps_ubx[0].msg_class = DL_HITL_UBX_class(buf);
193 gps_ubx[0].msg_id = DL_HITL_UBX_id(buf);
194 uint8_t l = DL_HITL_UBX_ubx_payload_length(buf);
195 uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf);
196 memcpy(
gps_ubx[0].msg_buf, ubx_payload, l);
197 gps_ubx[0].msg_available =
true;
206 uint8_t gnssFixOK = bit_is_set(flags, 0);
207 uint8_t diffSoln = bit_is_set(flags, 1);
208 uint8_t carrSoln = (flags & 0xC0) >> 6;
209 if (diffSoln && carrSoln == 2) {
211 }
else if(diffSoln && carrSoln == 1) {
213 }
else if(gnssFixOK) {
239 gubx->
state.
cacc = (RadOfDeg(UBX_NAV_PVT_headAcc(gubx->
msg_buf) * 10)) * 10;
375 gubx->
state.
cacc = (RadOfDeg(UBX_NAV_VELNED_cAcc(gubx->
msg_buf) * 10)) * 10;
413 for (
uint16_t i = 0; i < nb_sats; i++) {
414 if ((UBX_NAV_SAT_flags(gubx->
msg_buf, i) >> 3) & 0x1) {
415 switch (UBX_NAV_SAT_gnssId(gubx->
msg_buf, i)) {
416 case 0: gnss |= 0x01;
break;
417 case 6: gnss |= 0x02;
break;
418 case 3: gnss |= 0x04;
break;
419 case 2: gnss |= 0x08;
break;
468 uint8_t gnssFixOK = (flags >> 0) & 0x1;
469 uint8_t diffSoln = (flags >> 1) & 0x1;
470 uint8_t relPosValid = (flags >> 2) & 0x1;
471 uint8_t carrSoln = (flags >> 3) & 0x3;
472 uint8_t relPosHeadingValid = (flags >> 8) & 0x1;
476 if (diffSoln && carrSoln == 2) {
478 }
else if(diffSoln && carrSoln == 1) {
480 }
else if(gnssFixOK) {
489 relpos.
tow = UBX_NAV_RELPOSNED_iTOW(gubx->
msg_buf);
490 relpos.
pos.
x = UBX_NAV_RELPOSNED_relPosN(gubx->
msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPN(gubx->
msg_buf) * 1e-4;
491 relpos.
pos.
y = UBX_NAV_RELPOSNED_relPosE(gubx->
msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPE(gubx->
msg_buf) * 1e-4;
492 relpos.
pos.
z = UBX_NAV_RELPOSNED_relPosD(gubx->
msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPD(gubx->
msg_buf) * 1e-4;
493 relpos.
distance = UBX_NAV_RELPOSNED_relPosLength(gubx->
msg_buf) * 1e-2 + UBX_NAV_RELPOSNED_relPosHPLength(gubx->
msg_buf) * 1e-4;
501 relpos.
heading = RadOfDeg(UBX_NAV_RELPOSNED_relPosHeading(gubx->
msg_buf) * 1e-5);
525 bool relPosValid = (flags >> 2) & 0x1;
526 uint8_t carrSoln = (flags >> 3) & 0x3;
528 bool relPosHeadingValid = ((flags >> 6) & 0x1) && carrSoln == 2;
536 relpos.
tow = UBX_NAV_DAHEADING_iTOW(gubx->
msg_buf);
537 relpos.
pos.
x = UBX_NAV_DAHEADING_relPosN(gubx->
msg_buf) * 1e-3;
538 relpos.
pos.
y = UBX_NAV_DAHEADING_relPosE(gubx->
msg_buf) * 1e-3;
539 relpos.
pos.
z = UBX_NAV_DAHEADING_relPosD(gubx->
msg_buf) * 1e-3;
540 relpos.
distance = UBX_NAV_DAHEADING_relPosLength(gubx->
msg_buf) * 1e-3;
548 relpos.
heading = RadOfDeg(UBX_NAV_DAHEADING_relPosHeading(gubx->
msg_buf) * 1e-5);
564 case UBX_NAV_POSECEF_ID:
567 case UBX_NAV_POSLLH_ID:
570 case UBX_NAV_STATUS_ID:
579 case UBX_NAV_POSUTM_ID:
582 case UBX_NAV_VELECEF_ID:
585 case UBX_NAV_VELNED_ID:
588 case UBX_NAV_SVINFO_ID:
594 case UBX_NAV_RELPOSNED_ID:
597 case UBX_NAV_DAHEADING_ID:
604 else if (gubx->
msg_class == UBX_RXM_ID) {
632 if (c == UBX_SYNC1) {
637 if (c != UBX_SYNC2) {
663 gubx->
len |= (c << 8);
679 if (c != gubx->
ck_a) {
686 if (c != gubx->
ck_b) {
707 dev->put_byte(
dev->periph, 0,
byte);
719 dev->put_byte(
dev->periph, 0, UBX_SYNC1);
720 dev->put_byte(
dev->periph, 0, UBX_SYNC2);
743 dev->send_message(
dev->periph, 0);
749 for (i = 0; i < len; i++) {
757 UbxSend_CFG_RST(
dev, bbr, reset_mode, 0x00);
761#ifndef GPS_UBX_UCENTER
762#define gps_ubx_ucenter_event() {}
777 (gubx->
msg_id == UBX_NAV_VELNED_ID ||
778 gubx->
msg_id == UBX_NAV_PVT_ID ||
779 (gubx->
msg_id == UBX_NAV_SOL_ID &&
794 gubx->msg_available =
false;
812 for (i = 0; i < n; i++) {
813 dev->put_byte(
dev->periph, 0, buff[i]);
815 dev->send_message(
dev->periph, 0);
832#ifdef GPS_UBX_UCENTER
840 for (i = 0; i < length; i++) {
841 if (rtcm.nbyte == 0) {
844 rtcm.buff[rtcm.nbyte++] = data[i];
848 if (rtcm.nbyte < INJECT_BUFF_SIZE) {
849 rtcm.buff[rtcm.nbyte++] = data[i];
850 if (rtcm.nbyte == 3) {
855 if (rtcm.nbyte == rtcm.len + 3) {
857 unsigned int crc1 =
crc24q(rtcm.buff, rtcm.len);
858 unsigned int crc2 =
RTCMgetbitu(rtcm.buff, rtcm.len * 8, 24);
864 DEBUG_PRINT(
"Inject message %i - %d\n", packet_id, rtcm.buff[0]);
873 default:
DEBUG_PRINT(
"Unknown type: %i", packet_id);
break;
876 DEBUG_PRINT(
"Skipping message %i (CRC failed) - %d", packet_id, rtcm.buff[0]);
878 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.
static struct uart_periph * dev
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.
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
int16_t azim
azimuth in deg
uint32_t tow
GPS time of week in ms.
float distance_acc
Distance accuracy in meters.
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)
float heading
Relative heading to the reference station in radians.
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
#define GPS_FIX_DGPS
DGPS fix.
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.
float heading_acc
Heading accuracy in radians.
struct EcefCoor_i ecef_pos
position in ECEF in cm
uint32_t hacc
horizontal accuracy in cm
#define GPS_VALID_POS_LLA_BIT
double distance
Relative distance to the reference station in meters.
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
uint32_t tow
Time of week (GPS) in ms.
#define GPS_FIX_NONE
No GPS fix.
#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.
struct NedCoor_f pos_acc
Position accuracy in meters.
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
struct NedCoor_d pos
Relative postion to the reference station in meters.
#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)
#define GPS_FIX_RTK
RTK GPS fix.
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
uint16_t reference_id
Reference station identification.
#define GPS_VALID_POS_UTM_BIT
uint8_t num_sv
number of sat in fix
#define CFG_RST_BBR_Coldstart
static void gps_ubx_parse_nav_sat(struct GpsUbx *gubx)
void gps_ubx_periodic_check(void)
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
void gps_ubx_read_message(struct GpsUbx *gubx)
#define GPS_UBX_ERR_OVERRUN
#define GPS_UBX_BOOTRESET
static void gps_ubx_parse_nav_posecef(struct GpsUbx *gubx)
#define NAV_RELPOSNED_VERSION
static void ubx_send_1byte(struct link_device *dev, uint8_t byte)
static void gps_ubx_parse_nav_pvt(struct GpsUbx *gubx)
static void gps_ubx_parse_nav_daheading(struct GpsUbx *gubx)
static void gps_ubx_parse_nav_posutm(struct GpsUbx *gubx)
void gps_ublox_write(struct link_device *dev, uint8_t *buff, uint32_t n)
#define GPS_UBX_ERR_MSG_TOO_LONG
#define CFG_RST_Reset_Controlled
#define CFG_RST_BBR_Hotstart
#define GPS_UBX_ERR_CHECKSUM
void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr, UNUSED uint8_t reset_mode)
#define GPS_UBX_HEADING_BASELINE_TOL
Largest difference between the measured and the configured antenna distance [m].
static void gps_ubx_parse_nav_sol(struct GpsUbx *gubx)
#define NAV_DAHEADING_VERSION
void gps_ubx_msg(struct GpsUbx *gubx)
#define NAV_DAHEADING_REF_ID
RELPOS reference id used for the dual-antenna heading.
struct GpsUbx gps_ubx[GPS_UBX_NB]
#define gps_ubx_ucenter_event()
static void gps_ubx_parse_nav_posllh(struct GpsUbx *gubx)
#define GPS_UBX_HEADING_BASELINE
Distance between the two antennas of a heading receiver [m] (dual-antenna or moving base),...
#define CFG_RST_BBR_Warmstart
static void gps_ubx_parse_nav_velned(struct GpsUbx *gubx)
static void gps_ubx_parse_velecef(struct GpsUbx *gubx)
static void gps_ubx_parse_nav_svinfo(struct GpsUbx *gubx)
void gps_ubx_parse(struct GpsUbx *gubx, uint8_t c)
void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes)
static void gps_ubx_parse_nav_status(struct GpsUbx *gubx)
static void gps_ubx_parse_nav_relposned(struct GpsUbx *gubx)
static bool gps_ubx_heading_baseline_ok(double distance)
A receiver can report a fixed and valid heading on a wrong carrier phase solution (seen indoors with ...
#define GPS_UBX_ERR_OUT_OF_SYNC
#define GPS_UBX_HEADING_BASELINE_MAX
Longest antenna distance for which a heading is used when GPS_UBX_HEADING_BASELINE is not set [m] (as...
UBX protocol specific code.
uint8_t gnss_in_use
Satellite systems used in the solution (UBX-NAV-SAT): 1 GPS, 2 GLONASS, 4 BeiDou, 8 Galileo.
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD]
#define GPS_UBX_MAX_PAYLOAD
e.g. UBX-NAV-SAT is 8 + 12 * numSvs bytes
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
arch independent LED (Light Emitting Diodes) API
static FILE * pprzLogFile
#define UNINIT
Receiving pprz messages.
SdioError sdLogWriteByte(const FileDes fd, const uint8_t value)
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.