From 58caa662cda120ea122598fac84c5a20b2c160e2 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Fri, 24 May 2024 09:55:16 +0200 Subject: [PATCH] [modules] Support dual ublox GPS modules (#3267) * [modules] Support dual ublox GPS modules * Fix UCenter * Fix make test_modules * Fix: tell ins_ekf2 when no YAW is available --------- Co-authored-by: Christophe De Wagter --- conf/airframes/tudelft/rot_wing_v3g.xml | 7 + conf/airframes/tudelft/rot_wing_v3h.xml | 7 + conf/boards/cube_orangeplus.makefile | 6 +- conf/modules/gps_ublox.xml | 50 +- conf/modules/gps_ubx_ucenter.xml | 2 +- sw/airborne/modules/core/abi_sender_ids.h | 4 + sw/airborne/modules/gps/gps.c | 22 - sw/airborne/modules/gps/gps.h | 14 +- sw/airborne/modules/gps/gps_ubx.c | 624 +++++++++++----------- sw/airborne/modules/gps/gps_ubx.h | 63 +-- sw/airborne/modules/gps/gps_ubx_ucenter.c | 56 +- sw/airborne/modules/ins/ins_ekf2.cpp | 7 + 12 files changed, 418 insertions(+), 444 deletions(-) diff --git a/conf/airframes/tudelft/rot_wing_v3g.xml b/conf/airframes/tudelft/rot_wing_v3g.xml index 446319182f..3a56d48581 100644 --- a/conf/airframes/tudelft/rot_wing_v3g.xml +++ b/conf/airframes/tudelft/rot_wing_v3g.xml @@ -82,8 +82,12 @@ + + + + @@ -333,6 +337,9 @@ + + +
diff --git a/conf/airframes/tudelft/rot_wing_v3h.xml b/conf/airframes/tudelft/rot_wing_v3h.xml index 04590b43cb..1a9fd79923 100644 --- a/conf/airframes/tudelft/rot_wing_v3h.xml +++ b/conf/airframes/tudelft/rot_wing_v3h.xml @@ -82,8 +82,12 @@ + + + + @@ -333,6 +337,9 @@ + + +
diff --git a/conf/boards/cube_orangeplus.makefile b/conf/boards/cube_orangeplus.makefile index 262f13e17c..10817e6583 100644 --- a/conf/boards/cube_orangeplus.makefile +++ b/conf/boards/cube_orangeplus.makefile @@ -68,10 +68,14 @@ RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3 MODEM_PORT ?= UART2 MODEM_BAUD ?= B57600 -# The GPS1 port (UART8 is GPS2) +# The GPS1 port GPS_PORT ?= UART4 GPS_BAUD ?= B57600 +# The GPS2 port +GPS2_PORT ?= UART8 +GPS2_BAUD ?= B57600 + # InterMCU port connected to the IO processor INTERMCU_PORT ?= UART6 INTERMCU_BAUD ?= B1500000 diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml index 7d34bd7374..bd4e317e05 100644 --- a/conf/modules/gps_ublox.xml +++ b/conf/modules/gps_ublox.xml @@ -19,7 +19,7 @@ - + @@ -36,33 +36,51 @@ - - - + + + - + + + + + + + + + + ifdef SECONDARY_GPS - ifneq (,$(findstring $(SECONDARY_GPS), ublox)) - # this is the secondary GPS - $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/gps/gps_ubx.h\" - $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UBX + ifneq (,$(findstring ublox2, $(SECONDARY_GPS))) + # this is the secondary GPS + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UBX2 + else ifneq (,$(findstring ublox, $(SECONDARY_GPS))) + # this is the secondary GPS + $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UBX + endif + + ifneq (,$(findstring ublox2, $(PRIMARY_GPS))) + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UBX2 + else ifneq (,$(findstring ublox, $(PRIMARY_GPS))) + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\" + $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UBX + endif else - $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\" - $(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UBX - endif - else - # plain old single GPS usage - $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\" + # plain old single GPS usage + $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\" endif - + diff --git a/conf/modules/gps_ubx_ucenter.xml b/conf/modules/gps_ubx_ucenter.xml index 7cb9a965e4..a4eb6e5123 100644 --- a/conf/modules/gps_ubx_ucenter.xml +++ b/conf/modules/gps_ubx_ucenter.xml @@ -46,7 +46,7 @@ Warning: you still need to tell the driver, which paparazzi port you use. - + diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h index 086ebc5228..4709e1f0c1 100644 --- a/sw/airborne/modules/core/abi_sender_ids.h +++ b/sw/airborne/modules/core/abi_sender_ids.h @@ -296,6 +296,10 @@ #define GPS_DW1000_ID 15 #endif +#ifndef GPS_UBX2_ID +#define GPS_UBX2_ID 16 +#endif + /* * IDs of IMU sensors (accel, gyro) */ diff --git a/sw/airborne/modules/gps/gps.c b/sw/airborne/modules/gps/gps.c index 42ce860784..851fcc3f9e 100644 --- a/sw/airborne/modules/gps/gps.c +++ b/sw/airborne/modules/gps/gps.c @@ -69,7 +69,6 @@ PRINT_CONFIG_VAR(SECONDARY_GPS) struct GpsState gps; struct GpsTimeSync gps_time_sync; struct GpsRelposNED gps_relposned; -struct RtcmMan rtcm_man; #ifdef SECONDARY_GPS static uint8_t current_gps_id = GpsId(PRIMARY_GPS); @@ -175,17 +174,6 @@ static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev) &gps_relposned.gnssFixOK); } -static void send_gps_rxmrtcm(struct transport_tx *trans, struct link_device *dev) -{ - pprz_msg_send_GPS_RXMRTCM(trans, dev, AC_ID, - &rtcm_man.Cnt105, - &rtcm_man.Cnt177, - &rtcm_man.Cnt187, - &rtcm_man.Crc105, - &rtcm_man.Crc177, - &rtcm_man.Crc187); -} - static void send_gps_int(struct transport_tx *trans, struct link_device *dev) { #if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST @@ -364,22 +352,12 @@ void gps_init(void) register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_SOL, send_gps_sol); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_RTK, send_gps_rtk); - register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_RXMRTCM, send_gps_rxmrtcm); #endif /* Register preflight checks */ #if PREFLIGHT_CHECKS preflight_check_register(&gps_pfc, gps_preflight); #endif - - // Initializing counter variables to count the number of Rtcm msgs in the input stream(for each msg type) - rtcm_man.Cnt105 = 0; - rtcm_man.Cnt177 = 0; - rtcm_man.Cnt187 = 0; - // Initializing counter variables to count the number of messages that failed Crc Check - rtcm_man.Crc105 = 0; - rtcm_man.Crc177 = 0; - rtcm_man.Crc187 = 0; } uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) diff --git a/sw/airborne/modules/gps/gps.h b/sw/airborne/modules/gps/gps.h index 1b93ca9235..debc43d590 100644 --- a/sw/airborne/modules/gps/gps.h +++ b/sw/airborne/modules/gps/gps.h @@ -133,6 +133,8 @@ struct GpsRelposNED { int8_t relPosHPN; int8_t relPosHPE; int8_t relPosHPD; + float relPosLength; + float relPosHeading; uint32_t accN; uint32_t accE; uint32_t accD; @@ -141,17 +143,7 @@ struct GpsRelposNED { uint8_t diffSoln; uint8_t gnssFixOK; }; - -struct RtcmMan { - uint16_t RefStation; - uint16_t MsgType; // Counter variables to count the number of Rtcm msgs in the input stream(for each msg type) - uint32_t Cnt105; - uint32_t Cnt177; - uint32_t Cnt187; // Counter variables to count the number of messages that failed Crc Check - uint32_t Crc105; - uint32_t Crc177; - uint32_t Crc187; -}; +extern struct GpsRelposNED gps_relposned; /** global GPS state */ extern struct GpsState gps; diff --git a/sw/airborne/modules/gps/gps_ubx.c b/sw/airborne/modules/gps/gps_ubx.c index 6103d1e916..ded6041baa 100644 --- a/sw/airborne/modules/gps/gps_ubx.c +++ b/sw/airborne/modules/gps/gps_ubx.c @@ -73,11 +73,10 @@ #define UTM_HEM_NORTH 0 #define UTM_HEM_SOUTH 1 -struct GpsUbx gps_ubx; - -#if USE_GPS_UBX_RXM_RAW -struct GpsUbxRaw gps_ubx_raw; -#endif +struct GpsUbx gps_ubx[GPS_UBX_NB]; +uint8_t gps_ubx_reset = 0; +void gps_ubx_parse(struct GpsUbx *gubx, uint8_t c); +void gps_ubx_msg(struct GpsUbx *gubx); #if USE_GPS_UBX_RTCM extern struct GpsRelposNED gps_relposned; @@ -97,300 +96,327 @@ struct rtcm_t rtcm = { 0 }; #endif +/* + * GPS Reset + */ #ifndef GPS_UBX_BOOTRESET #define GPS_UBX_BOOTRESET 0 #endif +#define CFG_RST_Reset_Hardware 0x00 +#define CFG_RST_Reset_Controlled 0x01 +#define CFG_RST_Reset_Controlled_GPS_only 0x02 +#define CFG_RST_Reset_Controlled_GPS_stop 0x08 +#define CFG_RST_Reset_Controlled_GPS_start 0x09 + +#define CFG_RST_BBR_Hotstart 0x0000 +#define CFG_RST_BBR_Warmstart 0x0001 +#define CFG_RST_BBR_Coldstart 0xffff + void gps_ubx_init(void) { - gps_ubx.status = UNINIT; - gps_ubx.msg_available = false; - gps_ubx.error_cnt = 0; - gps_ubx.error_last = GPS_UBX_ERR_NONE; - gps_ubx.pacc_valid = false; + gps_ubx_reset = GPS_UBX_BOOTRESET; - gps_ubx.state.comp_id = GPS_UBX_ID; + // Configure the devices + gps_ubx[0].state.comp_id = GPS_UBX_ID; + gps_ubx[0].dev = &(UBX_GPS_PORT).device; +#if GPS_UBX_NB > 1 + gps_ubx[1].state.comp_id = GPS_UBX2_ID; + gps_ubx[1].dev = &(UBX2_GPS_PORT).device; +#endif - gps_ubx.reset = GPS_UBX_BOOTRESET; + // Set the defaults + for(uint8_t i = 0; i < GPS_UBX_NB; i++) { + gps_ubx[i].status = UNINIT; + gps_ubx[i].msg_available = false; + gps_ubx[i].error_cnt = 0; + gps_ubx[i].error_last = GPS_UBX_ERR_NONE; + gps_ubx[i].pacc_valid = false; + } } void gps_ubx_event(void) { - struct link_device *dev = &((UBX_GPS_LINK).device); + for(uint8_t i = 0; i < GPS_UBX_NB; i++) { + struct link_device *dev = gps_ubx[i].dev; - while (dev->char_available(dev->periph)) { - gps_ubx_parse(dev->get_byte(dev->periph)); - if (gps_ubx.msg_available) { - gps_ubx_msg(); + // Read all available bytes from the device + while (dev->char_available(dev->periph)) { + gps_ubx_parse(&gps_ubx[i], dev->get_byte(dev->periph)); + if (gps_ubx[i].msg_available) { + gps_ubx_msg(&gps_ubx[i]); + } + } + + // Reset the GPS's if needed + if (gps_ubx_reset > 0) { + switch (gps_ubx_reset) { + case 1 : ubx_send_cfg_rst(dev, CFG_RST_BBR_Hotstart, CFG_RST_Reset_Controlled); break; + case 2 : ubx_send_cfg_rst(dev, CFG_RST_BBR_Warmstart, CFG_RST_Reset_Controlled); break; + case 3 : ubx_send_cfg_rst(dev, CFG_RST_BBR_Coldstart, CFG_RST_Reset_Controlled); break; + default: DEBUG_PRINT("Unknown reset id: %i", gps_ubx[i].reset); break; + } } } - if (gps_ubx.reset > 0) { - switch (gps_ubx.reset) { - case 1 : ubx_send_cfg_rst(&(UBX_GPS_LINK).device, CFG_RST_BBR_Hotstart, CFG_RST_Reset_Controlled); break; - case 2 : ubx_send_cfg_rst(&(UBX_GPS_LINK).device, CFG_RST_BBR_Warmstart, CFG_RST_Reset_Controlled); break; - case 3 : ubx_send_cfg_rst(&(UBX_GPS_LINK).device, CFG_RST_BBR_Coldstart, CFG_RST_Reset_Controlled); break; - default: DEBUG_PRINT("Unknown reset id: %i", gps_ubx.reset); break; - } - gps_ubx.reset = 0; - } + gps_ubx_reset = 0; } void gps_ubx_parse_HITL_UBX(uint8_t *buf) { /** This code simulates gps_ubx.c:parse_ubx() */ - if (gps_ubx.msg_available) { - gps_ubx.error_cnt++; - gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; + if (gps_ubx[0].msg_available) { + gps_ubx[0].error_cnt++; + gps_ubx[0].error_last = GPS_UBX_ERR_OVERRUN; } else { - gps_ubx.msg_class = DL_HITL_UBX_class(buf); - gps_ubx.msg_id = DL_HITL_UBX_id(buf); + gps_ubx[0].msg_class = DL_HITL_UBX_class(buf); + gps_ubx[0].msg_id = DL_HITL_UBX_id(buf); uint8_t l = DL_HITL_UBX_ubx_payload_length(buf); uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(buf); - memcpy(gps_ubx.msg_buf, ubx_payload, l); - gps_ubx.msg_available = true; + memcpy(gps_ubx[0].msg_buf, ubx_payload, l); + gps_ubx[0].msg_available = true; } } -static void gps_ubx_parse_nav_pvt(void) +static void gps_ubx_parse_nav_pvt(struct GpsUbx *gubx) { - uint8_t flags = UBX_NAV_PVT_flags(gps_ubx.msg_buf); + uint8_t flags = UBX_NAV_PVT_flags(gubx->msg_buf); // Copy fix information uint8_t gnssFixOK = bit_is_set(flags, 0); uint8_t diffSoln = bit_is_set(flags, 1); uint8_t carrSoln = (flags & 0xC0) >> 6; if (diffSoln && carrSoln == 2) { - gps_ubx.state.fix = 5; // rtk + gubx->state.fix = 5; // rtk } else if(diffSoln && carrSoln == 1) { - gps_ubx.state.fix = 4; // dgnss + gubx->state.fix = 4; // dgnss } else if(gnssFixOK) { - gps_ubx.state.fix = 3; // 3D + gubx->state.fix = 3; // 3D } else { - gps_ubx.state.fix = 0; + gubx->state.fix = 0; } // Copy time information - gps_ubx.state.tow = UBX_NAV_PVT_iTOW(gps_ubx.msg_buf); - uint16_t year = UBX_NAV_PVT_year(gps_ubx.msg_buf); - uint8_t month = UBX_NAV_PVT_month(gps_ubx.msg_buf); - uint8_t day = UBX_NAV_PVT_day(gps_ubx.msg_buf); - gps_ubx.state.week = gps_week_number(year, month, day); - gps_ubx.state.num_sv = UBX_NAV_PVT_numSV(gps_ubx.msg_buf); + gubx->state.tow = UBX_NAV_PVT_iTOW(gubx->msg_buf); + uint16_t year = UBX_NAV_PVT_year(gubx->msg_buf); + uint8_t month = UBX_NAV_PVT_month(gubx->msg_buf); + uint8_t day = UBX_NAV_PVT_day(gubx->msg_buf); + gubx->state.week = gps_week_number(year, month, day); + gubx->state.num_sv = UBX_NAV_PVT_numSV(gubx->msg_buf); // Copy LLA position - gps_ubx.state.lla_pos.lat = UBX_NAV_PVT_lat(gps_ubx.msg_buf); - gps_ubx.state.lla_pos.lon = UBX_NAV_PVT_lon(gps_ubx.msg_buf); - gps_ubx.state.lla_pos.alt = UBX_NAV_PVT_height(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT); + gubx->state.lla_pos.lat = UBX_NAV_PVT_lat(gubx->msg_buf); + gubx->state.lla_pos.lon = UBX_NAV_PVT_lon(gubx->msg_buf); + gubx->state.lla_pos.alt = UBX_NAV_PVT_height(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_POS_LLA_BIT); // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180) // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg // solution: First to radians, and then scale to 1e-7 radians // First x 10 for loosing less resolution, then to radians, then multiply x 10 again - gps_ubx.state.course = (RadOfDeg(UBX_NAV_PVT_headMot(gps_ubx.msg_buf) * 10)) * 10; - SetBit(gps_ubx.state.valid_fields, GPS_VALID_COURSE_BIT); - gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_PVT_headAcc(gps_ubx.msg_buf) * 10)) * 10; + gubx->state.course = (RadOfDeg(UBX_NAV_PVT_headMot(gubx->msg_buf) * 10)) * 10; + SetBit(gubx->state.valid_fields, GPS_VALID_COURSE_BIT); + gubx->state.cacc = (RadOfDeg(UBX_NAV_PVT_headAcc(gubx->msg_buf) * 10)) * 10; // Copy HMSL and ground speed - gps_ubx.state.hmsl = UBX_NAV_PVT_hMSL(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT); - gps_ubx.state.gspeed = UBX_NAV_PVT_gSpeed(gps_ubx.msg_buf) / 10; + gubx->state.hmsl = UBX_NAV_PVT_hMSL(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_HMSL_BIT); + gubx->state.gspeed = UBX_NAV_PVT_gSpeed(gubx->msg_buf) / 10; // Copy NED velocities - gps_ubx.state.ned_vel.x = UBX_NAV_PVT_velN(gps_ubx.msg_buf) / 10; - gps_ubx.state.ned_vel.y = UBX_NAV_PVT_velE(gps_ubx.msg_buf) / 10; - gps_ubx.state.ned_vel.z = UBX_NAV_PVT_velD(gps_ubx.msg_buf) / 10; - SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT); + gubx->state.ned_vel.x = UBX_NAV_PVT_velN(gubx->msg_buf) / 10; + gubx->state.ned_vel.y = UBX_NAV_PVT_velE(gubx->msg_buf) / 10; + gubx->state.ned_vel.z = UBX_NAV_PVT_velD(gubx->msg_buf) / 10; + SetBit(gubx->state.valid_fields, GPS_VALID_VEL_NED_BIT); // Copy accuracy information - gps_ubx.state.pdop = UBX_NAV_PVT_pDOP(gps_ubx.msg_buf); - gps_ubx.state.hacc = UBX_NAV_PVT_hAcc(gps_ubx.msg_buf) / 10; - gps_ubx.state.vacc = UBX_NAV_PVT_vAcc(gps_ubx.msg_buf) / 10; - gps_ubx.state.sacc = UBX_NAV_PVT_sAcc(gps_ubx.msg_buf) / 10; + gubx->state.pdop = UBX_NAV_PVT_pDOP(gubx->msg_buf); + gubx->state.hacc = UBX_NAV_PVT_hAcc(gubx->msg_buf) / 10; + gubx->state.vacc = UBX_NAV_PVT_vAcc(gubx->msg_buf) / 10; + gubx->state.sacc = UBX_NAV_PVT_sAcc(gubx->msg_buf) / 10; - if (!gps_ubx.pacc_valid) { + if (!gubx->pacc_valid) { // workaround for PVT only - gps_ubx.state.pacc = gps_ubx.state.hacc; // report horizontal accuracy + gubx->state.pacc = gubx->state.hacc; // report horizontal accuracy } } -static void gps_ubx_parse_nav_sol(void) +static void gps_ubx_parse_nav_sol(struct GpsUbx *gubx) { // Copy time and fix information - uint8_t fix = UBX_NAV_SOL_gpsFix(gps_ubx.msg_buf); - if ((fix == GPS_FIX_3D && fix > gps_ubx.state.fix) || fix < GPS_FIX_3D) { + uint8_t fix = UBX_NAV_SOL_gpsFix(gubx->msg_buf); + if ((fix == GPS_FIX_3D && fix > gubx->state.fix) || fix < GPS_FIX_3D) { // update only if fix is better than current or fix not 3D // leaving fix if in GNSS or RTK mode - gps_ubx.state.fix = fix; + gubx->state.fix = fix; } - gps_ubx.state.tow = UBX_NAV_SOL_iTOW(gps_ubx.msg_buf); - gps_ubx.state.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); - gps_ubx.state.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); + gubx->state.tow = UBX_NAV_SOL_iTOW(gubx->msg_buf); + gubx->state.week = UBX_NAV_SOL_week(gubx->msg_buf); + gubx->state.num_sv = UBX_NAV_SOL_numSV(gubx->msg_buf); // Copy ecef position - gps_ubx.state.ecef_pos.x = UBX_NAV_SOL_ecefX(gps_ubx.msg_buf); - gps_ubx.state.ecef_pos.y = UBX_NAV_SOL_ecefY(gps_ubx.msg_buf); - gps_ubx.state.ecef_pos.z = UBX_NAV_SOL_ecefZ(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_ECEF_BIT); + gubx->state.ecef_pos.x = UBX_NAV_SOL_ecefX(gubx->msg_buf); + gubx->state.ecef_pos.y = UBX_NAV_SOL_ecefY(gubx->msg_buf); + gubx->state.ecef_pos.z = UBX_NAV_SOL_ecefZ(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_POS_ECEF_BIT); // Copy ecef velocity - gps_ubx.state.ecef_vel.x = UBX_NAV_SOL_ecefVX(gps_ubx.msg_buf); - gps_ubx.state.ecef_vel.y = UBX_NAV_SOL_ecefVY(gps_ubx.msg_buf); - gps_ubx.state.ecef_vel.z = UBX_NAV_SOL_ecefVZ(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gubx->state.ecef_vel.x = UBX_NAV_SOL_ecefVX(gubx->msg_buf); + gubx->state.ecef_vel.y = UBX_NAV_SOL_ecefVY(gubx->msg_buf); + gubx->state.ecef_vel.z = UBX_NAV_SOL_ecefVZ(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_VEL_ECEF_BIT); // Copy accuracy measurements - gps_ubx.state.pacc = UBX_NAV_SOL_pAcc(gps_ubx.msg_buf); - gps_ubx.state.sacc = UBX_NAV_SOL_sAcc(gps_ubx.msg_buf); - gps_ubx.state.pdop = UBX_NAV_SOL_pDOP(gps_ubx.msg_buf); - gps_ubx.pacc_valid = true; + gubx->state.pacc = UBX_NAV_SOL_pAcc(gubx->msg_buf); + gubx->state.sacc = UBX_NAV_SOL_sAcc(gubx->msg_buf); + gubx->state.pdop = UBX_NAV_SOL_pDOP(gubx->msg_buf); + gubx->pacc_valid = true; } -static void gps_ubx_parse_nav_posecef(void) +static void gps_ubx_parse_nav_posecef(struct GpsUbx *gubx) { - gps_ubx.state.tow = UBX_NAV_POSECEF_iTOW(gps_ubx.msg_buf); + gubx->state.tow = UBX_NAV_POSECEF_iTOW(gubx->msg_buf); // Copy ecef position - gps_ubx.state.ecef_pos.x = UBX_NAV_POSECEF_ecefX(gps_ubx.msg_buf); - gps_ubx.state.ecef_pos.y = UBX_NAV_POSECEF_ecefY(gps_ubx.msg_buf); - gps_ubx.state.ecef_pos.z = UBX_NAV_POSECEF_ecefZ(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_ECEF_BIT); + gubx->state.ecef_pos.x = UBX_NAV_POSECEF_ecefX(gubx->msg_buf); + gubx->state.ecef_pos.y = UBX_NAV_POSECEF_ecefY(gubx->msg_buf); + gubx->state.ecef_pos.z = UBX_NAV_POSECEF_ecefZ(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_POS_ECEF_BIT); // Copy accuracy information - gps_ubx.state.pacc = UBX_NAV_POSECEF_pAcc(gps_ubx.msg_buf); - gps_ubx.pacc_valid = true; + gubx->state.pacc = UBX_NAV_POSECEF_pAcc(gubx->msg_buf); + gubx->pacc_valid = true; } -static void gps_ubx_parse_nav_posllh(void) +static void gps_ubx_parse_nav_posllh(struct GpsUbx *gubx) { // Copy LLA position - gps_ubx.state.lla_pos.lat = UBX_NAV_POSLLH_lat(gps_ubx.msg_buf); - gps_ubx.state.lla_pos.lon = UBX_NAV_POSLLH_lon(gps_ubx.msg_buf); - gps_ubx.state.lla_pos.alt = UBX_NAV_POSLLH_height(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT); + gubx->state.lla_pos.lat = UBX_NAV_POSLLH_lat(gubx->msg_buf); + gubx->state.lla_pos.lon = UBX_NAV_POSLLH_lon(gubx->msg_buf); + gubx->state.lla_pos.alt = UBX_NAV_POSLLH_height(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_POS_LLA_BIT); // Copy HMSL - gps_ubx.state.hmsl = UBX_NAV_POSLLH_hMSL(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT); + gubx->state.hmsl = UBX_NAV_POSLLH_hMSL(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_HMSL_BIT); // Copy accuracy information - gps_ubx.state.hacc = UBX_NAV_POSLLH_hAcc(gps_ubx.msg_buf) / 10; - gps_ubx.state.vacc = UBX_NAV_POSLLH_vAcc(gps_ubx.msg_buf) / 10; + gubx->state.hacc = UBX_NAV_POSLLH_hAcc(gubx->msg_buf) / 10; + gubx->state.vacc = UBX_NAV_POSLLH_vAcc(gubx->msg_buf) / 10; } -static void gps_ubx_parse_nav_posutm(void) +static void gps_ubx_parse_nav_posutm(struct GpsUbx *gubx) { - uint8_t hem = UBX_NAV_POSUTM_hem(gps_ubx.msg_buf); + uint8_t hem = UBX_NAV_POSUTM_hem(gubx->msg_buf); // Copy UTM state - gps_ubx.state.utm_pos.east = UBX_NAV_POSUTM_east(gps_ubx.msg_buf); - gps_ubx.state.utm_pos.north = UBX_NAV_POSUTM_north(gps_ubx.msg_buf); + gubx->state.utm_pos.east = UBX_NAV_POSUTM_east(gubx->msg_buf); + gubx->state.utm_pos.north = UBX_NAV_POSUTM_north(gubx->msg_buf); if (hem == UTM_HEM_SOUTH) { - gps_ubx.state.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ + gubx->state.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ } - gps_ubx.state.utm_pos.alt = UBX_NAV_POSUTM_alt(gps_ubx.msg_buf) * 10; - gps_ubx.state.utm_pos.zone = UBX_NAV_POSUTM_zone(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_UTM_BIT); + gubx->state.utm_pos.alt = UBX_NAV_POSUTM_alt(gubx->msg_buf) * 10; + gubx->state.utm_pos.zone = UBX_NAV_POSUTM_zone(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_POS_UTM_BIT); // Copy HMSL - gps_ubx.state.hmsl = gps_ubx.state.utm_pos.alt; - SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT); + gubx->state.hmsl = gubx->state.utm_pos.alt; + SetBit(gubx->state.valid_fields, GPS_VALID_HMSL_BIT); } -static void gps_ubx_parse_velecef(void) +static void gps_ubx_parse_velecef(struct GpsUbx *gubx) { - gps_ubx.state.tow = UBX_NAV_VELECEF_iTOW(gps_ubx.msg_buf); + gubx->state.tow = UBX_NAV_VELECEF_iTOW(gubx->msg_buf); // Copy ecef velocity - gps_ubx.state.ecef_vel.x = UBX_NAV_VELECEF_ecefVX(gps_ubx.msg_buf); - gps_ubx.state.ecef_vel.y = UBX_NAV_VELECEF_ecefVY(gps_ubx.msg_buf); - gps_ubx.state.ecef_vel.z = UBX_NAV_VELECEF_ecefVZ(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gubx->state.ecef_vel.x = UBX_NAV_VELECEF_ecefVX(gubx->msg_buf); + gubx->state.ecef_vel.y = UBX_NAV_VELECEF_ecefVY(gubx->msg_buf); + gubx->state.ecef_vel.z = UBX_NAV_VELECEF_ecefVZ(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_VEL_ECEF_BIT); // Copy accuracy measurements - gps_ubx.state.sacc = UBX_NAV_VELECEF_sAcc(gps_ubx.msg_buf); + gubx->state.sacc = UBX_NAV_VELECEF_sAcc(gubx->msg_buf); } -static void gps_ubx_parse_nav_velned(void) +static void gps_ubx_parse_nav_velned(struct GpsUbx *gubx) { // Copy groundspeed and total 3d speed - gps_ubx.state.speed_3d = UBX_NAV_VELNED_speed(gps_ubx.msg_buf); - gps_ubx.state.gspeed = UBX_NAV_VELNED_gSpeed(gps_ubx.msg_buf); + gubx->state.speed_3d = UBX_NAV_VELNED_speed(gubx->msg_buf); + gubx->state.gspeed = UBX_NAV_VELNED_gSpeed(gubx->msg_buf); // Copy NED velocities - gps_ubx.state.ned_vel.x = UBX_NAV_VELNED_velN(gps_ubx.msg_buf); - gps_ubx.state.ned_vel.y = UBX_NAV_VELNED_velE(gps_ubx.msg_buf); - gps_ubx.state.ned_vel.z = UBX_NAV_VELNED_velD(gps_ubx.msg_buf); - SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT); + gubx->state.ned_vel.x = UBX_NAV_VELNED_velN(gubx->msg_buf); + gubx->state.ned_vel.y = UBX_NAV_VELNED_velE(gubx->msg_buf); + gubx->state.ned_vel.z = UBX_NAV_VELNED_velD(gubx->msg_buf); + SetBit(gubx->state.valid_fields, GPS_VALID_VEL_NED_BIT); // Copy course - gps_ubx.state.course = (RadOfDeg(UBX_NAV_VELNED_heading(gps_ubx.msg_buf) * 10)) * 10; - gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_VELNED_cAcc(gps_ubx.msg_buf) * 10)) * 10; - SetBit(gps_ubx.state.valid_fields, GPS_VALID_COURSE_BIT); + gubx->state.course = (RadOfDeg(UBX_NAV_VELNED_heading(gubx->msg_buf) * 10)) * 10; + gubx->state.cacc = (RadOfDeg(UBX_NAV_VELNED_cAcc(gubx->msg_buf) * 10)) * 10; + SetBit(gubx->state.valid_fields, GPS_VALID_COURSE_BIT); // Copy time of week - gps_ubx.state.tow = UBX_NAV_VELNED_iTOW(gps_ubx.msg_buf); + gubx->state.tow = UBX_NAV_VELNED_iTOW(gubx->msg_buf); } -static void gps_ubx_parse_nav_svinfo(void) +static void gps_ubx_parse_nav_svinfo(struct GpsUbx *gubx) { // Get the number of channels - gps_ubx.state.nb_channels = Min(UBX_NAV_SVINFO_numCh(gps_ubx.msg_buf), GPS_NB_CHANNELS); + gubx->state.nb_channels = Min(UBX_NAV_SVINFO_numCh(gubx->msg_buf), GPS_NB_CHANNELS); // Go through all the different channels - for (uint8_t i = 0; i < gps_ubx.state.nb_channels; i++) { - gps_ubx.state.svinfos[i].svid = UBX_NAV_SVINFO_svid(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].flags = UBX_NAV_SVINFO_flags(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].qi = UBX_NAV_SVINFO_quality(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].cno = UBX_NAV_SVINFO_cno(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].elev = UBX_NAV_SVINFO_elev(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].azim = UBX_NAV_SVINFO_azim(gps_ubx.msg_buf, i); + for (uint8_t i = 0; i < gubx->state.nb_channels; i++) { + gubx->state.svinfos[i].svid = UBX_NAV_SVINFO_svid(gubx->msg_buf, i); + gubx->state.svinfos[i].flags = UBX_NAV_SVINFO_flags(gubx->msg_buf, i); + gubx->state.svinfos[i].qi = UBX_NAV_SVINFO_quality(gubx->msg_buf, i); + gubx->state.svinfos[i].cno = UBX_NAV_SVINFO_cno(gubx->msg_buf, i); + gubx->state.svinfos[i].elev = UBX_NAV_SVINFO_elev(gubx->msg_buf, i); + gubx->state.svinfos[i].azim = UBX_NAV_SVINFO_azim(gubx->msg_buf, i); } } -static void gps_ubx_parse_nav_sat(void) +static void gps_ubx_parse_nav_sat(struct GpsUbx *gubx) { // Get the number of channels(sattelites) and time of week - gps_ubx.state.tow = UBX_NAV_SAT_iTOW(gps_ubx.msg_buf); - gps_ubx.state.nb_channels = Min(UBX_NAV_SAT_numSvs(gps_ubx.msg_buf), GPS_NB_CHANNELS); + gubx->state.tow = UBX_NAV_SAT_iTOW(gubx->msg_buf); + gubx->state.nb_channels = Min(UBX_NAV_SAT_numSvs(gubx->msg_buf), GPS_NB_CHANNELS); // Check the version - uint8_t version = UBX_NAV_SAT_version(gps_ubx.msg_buf); + uint8_t version = UBX_NAV_SAT_version(gubx->msg_buf); if (version != 1) { return; } // Go through all the different channels - for (uint8_t i = 0; i < gps_ubx.state.nb_channels; i++) { - uint32_t flags = UBX_NAV_SVINFO_flags(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].svid = UBX_NAV_SAT_svId(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].cno = UBX_NAV_SAT_cno(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].elev = UBX_NAV_SAT_elev(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].azim = UBX_NAV_SAT_azim(gps_ubx.msg_buf, i); - gps_ubx.state.svinfos[i].qi = flags & 0x7; - gps_ubx.state.svinfos[i].flags = (flags >> 3) & 0x1; + for (uint8_t i = 0; i < gubx->state.nb_channels; i++) { + uint32_t flags = UBX_NAV_SVINFO_flags(gubx->msg_buf, i); + gubx->state.svinfos[i].svid = UBX_NAV_SAT_svId(gubx->msg_buf, i); + gubx->state.svinfos[i].cno = UBX_NAV_SAT_cno(gubx->msg_buf, i); + gubx->state.svinfos[i].elev = UBX_NAV_SAT_elev(gubx->msg_buf, i); + gubx->state.svinfos[i].azim = UBX_NAV_SAT_azim(gubx->msg_buf, i); + gubx->state.svinfos[i].qi = flags & 0x7; + gubx->state.svinfos[i].flags = (flags >> 3) & 0x1; } } -static void gps_ubx_parse_nav_status(void) +static void gps_ubx_parse_nav_status(struct GpsUbx *gubx) { - uint8_t fix = UBX_NAV_STATUS_gpsFix(gps_ubx.msg_buf); - if ((fix == GPS_FIX_3D && fix > gps_ubx.state.fix) || fix < GPS_FIX_3D) { + uint8_t fix = UBX_NAV_STATUS_gpsFix(gubx->msg_buf); + if ((fix == GPS_FIX_3D && fix > gubx->state.fix) || fix < GPS_FIX_3D) { // update only if fix is better than current or fix not 3D // leaving fix if in GNSS or RTK mode - gps_ubx.state.fix = fix; + gubx->state.fix = fix; } - gps_ubx.state.tow = UBX_NAV_STATUS_iTOW(gps_ubx.msg_buf); - gps_ubx.status_flags = UBX_NAV_STATUS_flags(gps_ubx.msg_buf); + gubx->state.tow = UBX_NAV_STATUS_iTOW(gubx->msg_buf); + gubx->status_flags = UBX_NAV_STATUS_flags(gubx->msg_buf); } -static void gps_ubx_parse_nav_relposned(void) +static void gps_ubx_parse_nav_relposned(struct GpsUbx *gubx) { #if USE_GPS_UBX_RTCM - uint8_t version = UBX_NAV_RELPOSNED_version(gps_ubx.msg_buf); + uint8_t version = UBX_NAV_RELPOSNED_version(gubx->msg_buf); if (version <= NAV_RELPOSNED_VERSION) { - uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf); + uint8_t flags = UBX_NAV_RELPOSNED_flags(gubx->msg_buf); uint8_t carrSoln = RTCMgetbitu(&flags, 3, 2); uint8_t relPosValid = RTCMgetbitu(&flags, 5, 1); uint8_t diffSoln = RTCMgetbitu(&flags, 6, 1); @@ -399,142 +425,90 @@ static void gps_ubx_parse_nav_relposned(void) /* Only save the latest valid relative position */ if (relPosValid) { if (diffSoln && carrSoln == 2) { - gps_ubx.state.fix = 5; // rtk + gubx->state.fix = 5; // rtk } else if(diffSoln && carrSoln == 1) { - gps_ubx.state.fix = 4; // dgnss + gubx->state.fix = 4; // dgnss } else if(gnssFixOK) { - gps_ubx.state.fix = 3; // 3D + gubx->state.fix = 3; // 3D } else { - gps_ubx.state.fix = 0; + gubx->state.fix = 0; } - gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gps_ubx.msg_buf); - gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf); - gps_relposned.relPosN = UBX_NAV_RELPOSNED_relPosN(gps_ubx.msg_buf); - gps_relposned.relPosE = UBX_NAV_RELPOSNED_relPosE(gps_ubx.msg_buf); - gps_relposned.relPosD = UBX_NAV_RELPOSNED_relPosD(gps_ubx.msg_buf) ; - gps_relposned.relPosHPN = UBX_NAV_RELPOSNED_relPosHPN(gps_ubx.msg_buf); - gps_relposned.relPosHPE = UBX_NAV_RELPOSNED_relPosHPE(gps_ubx.msg_buf); - gps_relposned.relPosHPD = UBX_NAV_RELPOSNED_relPosHPD(gps_ubx.msg_buf); - gps_relposned.accN = UBX_NAV_RELPOSNED_accN(gps_ubx.msg_buf); - gps_relposned.accE = UBX_NAV_RELPOSNED_accE(gps_ubx.msg_buf); - gps_relposned.accD = UBX_NAV_RELPOSNED_accD(gps_ubx.msg_buf); + gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gubx->msg_buf); + gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gubx->msg_buf); + gps_relposned.relPosN = UBX_NAV_RELPOSNED_relPosN(gubx->msg_buf); + gps_relposned.relPosE = UBX_NAV_RELPOSNED_relPosE(gubx->msg_buf); + gps_relposned.relPosD = UBX_NAV_RELPOSNED_relPosD(gubx->msg_buf) ; + gps_relposned.relPosHPN = UBX_NAV_RELPOSNED_relPosHPN(gubx->msg_buf); + gps_relposned.relPosHPE = UBX_NAV_RELPOSNED_relPosHPE(gubx->msg_buf); + gps_relposned.relPosHPD = UBX_NAV_RELPOSNED_relPosHPD(gubx->msg_buf); + gps_relposned.relPosLength = UBX_NAV_RELPOSNED_relPosLength(gubx->msg_buf) / 100.f; + gps_relposned.relPosHeading = UBX_NAV_RELPOSNED_relPosHeading(gubx->msg_buf) * 1e-5; + gps_relposned.accN = UBX_NAV_RELPOSNED_relPosHeading(gubx->msg_buf) * 1e-4; + gps_relposned.accE = UBX_NAV_RELPOSNED_relPosLength(gubx->msg_buf); + gps_relposned.accD = UBX_NAV_RELPOSNED_accD(gubx->msg_buf); gps_relposned.carrSoln = carrSoln; gps_relposned.relPosValid = relPosValid; gps_relposned.diffSoln = diffSoln; gps_relposned.gnssFixOK = gnssFixOK; + } else { + gps_relposned.relPosHeading = NAN; + gps_relposned.accN = 999; } } +#else + (void)gubx; #endif } -static void gps_ubx_parse_rxm_raw(void) -{ -#if USE_GPS_UBX_RXM_RAW - gps_ubx_raw.iTOW = UBX_RXM_RAW_iTOW(gps_ubx.msg_buf); - gps_ubx_raw.week = UBX_RXM_RAW_week(gps_ubx.msg_buf); - gps_ubx_raw.numSV = UBX_RXM_RAW_numSV(gps_ubx.msg_buf); - uint8_t i; - uint8_t max_SV = Min(gps_ubx_raw.numSV, GPS_UBX_NB_CHANNELS); - for (i = 0; i < max_SV; i++) { - gps_ubx_raw.measures[i].cpMes = UBX_RXM_RAW_cpMes(gps_ubx.msg_buf, i); - gps_ubx_raw.measures[i].prMes = UBX_RXM_RAW_prMes(gps_ubx.msg_buf, i); - gps_ubx_raw.measures[i].doMes = UBX_RXM_RAW_doMes(gps_ubx.msg_buf, i); - gps_ubx_raw.measures[i].sv = UBX_RXM_RAW_sv(gps_ubx.msg_buf, i); - gps_ubx_raw.measures[i].mesQI = UBX_RXM_RAW_mesQI(gps_ubx.msg_buf, i); - gps_ubx_raw.measures[i].cno = UBX_RXM_RAW_cno(gps_ubx.msg_buf, i); - gps_ubx_raw.measures[i].lli = UBX_RXM_RAW_lli(gps_ubx.msg_buf, i); - } -#endif // USE_GPS_UBX_RXM_RAW -} - -static void gps_ubx_parse_rxm_rtcm(void) -{ -#if USE_GPS_UBX_RTCM - uint8_t version = UBX_RXM_RTCM_version(gps_ubx.msg_buf); - if (version == RXM_RTCM_VERSION) { - // uint8_t flags = UBX_RXM_RTCM_flags(gps_ubx.msg_buf); - // bool crcFailed = RTCMgetbitu(&flags, 7, 1); - // uint16_t refStation = UBX_RXM_RTCM_refStation(gps_ubx.msg_buf); - // uint16_t msgType = UBX_RXM_RTCM_msgType(gps_ubx.msg_buf); - // DEBUG_PRINT("Message %i from refStation %i processed (CRCfailed: %i)\n", msgType, refStation, crcFailed); - - rtcm_man.RefStation = UBX_RXM_RTCM_refStation(gps_ubx.msg_buf); - rtcm_man.MsgType = UBX_RXM_RTCM_msgType(gps_ubx.msg_buf); - uint8_t flags = UBX_RXM_RTCM_flags(gps_ubx.msg_buf); - bool crcFailed = RTCMgetbitu(&flags, 7, 1); - switch (rtcm_man.MsgType) { - case 1005: - rtcm_man.Cnt105 += 1; - rtcm_man.Crc105 += crcFailed; - break; - case 1077: - rtcm_man.Cnt177 += 1; - rtcm_man.Crc177 += crcFailed;; - break; - case 1087: - rtcm_man.Cnt187 += 1; - rtcm_man.Crc187 += crcFailed;; - break; - default: - break; - } - } else { - DEBUG_PRINT("Unknown RXM_RTCM version: %i\n", version); - } -#endif // USE_GPS_UBX_RTCM -} - -void gps_ubx_read_message(void) +void gps_ubx_read_message(struct GpsUbx *gubx); +void gps_ubx_read_message(struct GpsUbx *gubx) { - if (gps_ubx.msg_class == UBX_NAV_ID) { - switch (gps_ubx.msg_id) { + if (gubx->msg_class == UBX_NAV_ID) { + switch (gubx->msg_id) { case UBX_NAV_POSECEF_ID: - gps_ubx_parse_nav_posecef(); + gps_ubx_parse_nav_posecef(gubx); break; case UBX_NAV_POSLLH_ID: - gps_ubx_parse_nav_posllh(); + gps_ubx_parse_nav_posllh(gubx); break; case UBX_NAV_STATUS_ID: - gps_ubx_parse_nav_status(); + gps_ubx_parse_nav_status(gubx); break; case UBX_NAV_SOL_ID: - gps_ubx_parse_nav_sol(); + gps_ubx_parse_nav_sol(gubx); break; case UBX_NAV_PVT_ID: - gps_ubx_parse_nav_pvt(); + gps_ubx_parse_nav_pvt(gubx); break; case UBX_NAV_POSUTM_ID: - gps_ubx_parse_nav_posutm(); + gps_ubx_parse_nav_posutm(gubx); break; case UBX_NAV_VELECEF_ID: - gps_ubx_parse_velecef(); + gps_ubx_parse_velecef(gubx); break; case UBX_NAV_VELNED_ID: - gps_ubx_parse_nav_velned(); + gps_ubx_parse_nav_velned(gubx); break; case UBX_NAV_SVINFO_ID: - gps_ubx_parse_nav_svinfo(); + gps_ubx_parse_nav_svinfo(gubx); break; case UBX_NAV_SAT_ID: - gps_ubx_parse_nav_sat(); + gps_ubx_parse_nav_sat(gubx); break; case UBX_NAV_RELPOSNED_ID: - gps_ubx_parse_nav_relposned(); + gps_ubx_parse_nav_relposned(gubx); break; default: break; } } - else if (gps_ubx.msg_class == UBX_RXM_ID) { - switch (gps_ubx.msg_id) { - case UBX_RXM_RAW_ID: - gps_ubx_parse_rxm_raw(); - break; - case UBX_RXM_RTCM_ID: - gps_ubx_parse_rxm_rtcm(); - break; + else if (gubx->msg_class == UBX_RXM_ID) { + switch (gubx->msg_id) { + // case UBX_RXM_RTCM_ID: + // gps_ubx_parse_rxm_rtcm(gubx); + // break; default: break; } @@ -547,103 +521,113 @@ void gps_ubx_read_message(void) #endif /* UBX parsing */ -void gps_ubx_parse(uint8_t c) +void gps_ubx_parse(struct GpsUbx *gubx, uint8_t c) { #if LOG_RAW_GPS sdLogWriteByte(pprzLogFile, c); #endif - if (gps_ubx.status < GOT_PAYLOAD) { - gps_ubx.ck_a += c; - gps_ubx.ck_b += gps_ubx.ck_a; + if (gubx->status < GOT_PAYLOAD) { + gubx->ck_a += c; + gubx->ck_b += gubx->ck_a; } - switch (gps_ubx.status) { + switch (gubx->status) { case UNINIT: if (c == UBX_SYNC1) { - gps_ubx.status++; + gubx->status++; } break; case GOT_SYNC1: if (c != UBX_SYNC2) { - gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC; + gubx->error_last = GPS_UBX_ERR_OUT_OF_SYNC; goto error; } - gps_ubx.ck_a = 0; - gps_ubx.ck_b = 0; - gps_ubx.status++; + gubx->ck_a = 0; + gubx->ck_b = 0; + gubx->status++; break; case GOT_SYNC2: - if (gps_ubx.msg_available) { + if (gubx->msg_available) { /* Previous message has not yet been parsed: discard this one */ - gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; + gubx->error_last = GPS_UBX_ERR_OVERRUN; goto error; } - gps_ubx.msg_class = c; - gps_ubx.status++; + gubx->msg_class = c; + gubx->status++; break; case GOT_CLASS: - gps_ubx.msg_id = c; - gps_ubx.status++; + gubx->msg_id = c; + gubx->status++; break; case GOT_ID: - gps_ubx.len = c; - gps_ubx.status++; + gubx->len = c; + gubx->status++; break; case GOT_LEN1: - gps_ubx.len |= (c << 8); - if (gps_ubx.len > GPS_UBX_MAX_PAYLOAD) { - gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG; + gubx->len |= (c << 8); + if (gubx->len > GPS_UBX_MAX_PAYLOAD) { + gubx->error_last = GPS_UBX_ERR_MSG_TOO_LONG; goto error; } - gps_ubx.msg_idx = 0; - gps_ubx.status++; + gubx->msg_idx = 0; + gubx->status++; break; case GOT_LEN2: - gps_ubx.msg_buf[gps_ubx.msg_idx] = c; - gps_ubx.msg_idx++; - if (gps_ubx.msg_idx >= gps_ubx.len) { - gps_ubx.status++; + gubx->msg_buf[gubx->msg_idx] = c; + gubx->msg_idx++; + if (gubx->msg_idx >= gubx->len) { + gubx->status++; } break; case GOT_PAYLOAD: - if (c != gps_ubx.ck_a) { - gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + if (c != gubx->ck_a) { + gubx->error_last = GPS_UBX_ERR_CHECKSUM; goto error; } - gps_ubx.status++; + gubx->status++; break; case GOT_CHECKSUM1: - if (c != gps_ubx.ck_b) { - gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + if (c != gubx->ck_b) { + gubx->error_last = GPS_UBX_ERR_CHECKSUM; goto error; } - gps_ubx.msg_available = true; + gubx->msg_available = true; goto restart; break; default: - gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED; + gubx->error_last = GPS_UBX_ERR_UNEXPECTED; goto error; } return; error: - gps_ubx.error_cnt++; + gubx->error_cnt++; restart: - gps_ubx.status = UNINIT; + gubx->status = UNINIT; return; } static void ubx_send_1byte(struct link_device *dev, uint8_t byte) { dev->put_byte(dev->periph, 0, byte); - gps_ubx.send_ck_a += byte; - gps_ubx.send_ck_b += gps_ubx.send_ck_a; + for(uint8_t i = 0; i < GPS_UBX_NB; i++) { + if(gps_ubx[i].dev == dev) { + gps_ubx[i].send_ck_a += byte; + gps_ubx[i].send_ck_b += gps_ubx[i].send_ck_a; + break; + } + } } void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_t len) { dev->put_byte(dev->periph, 0, UBX_SYNC1); dev->put_byte(dev->periph, 0, UBX_SYNC2); - gps_ubx.send_ck_a = 0; - gps_ubx.send_ck_b = 0; + for(uint8_t i = 0; i < GPS_UBX_NB; i++) { + if(gps_ubx[i].dev == dev) { + gps_ubx[i].send_ck_a = 0; + gps_ubx[i].send_ck_b = 0; + break; + } + } ubx_send_1byte(dev, nav_id); ubx_send_1byte(dev, msg_id); ubx_send_1byte(dev, (uint8_t)(len & 0xFF)); @@ -652,8 +636,13 @@ void ubx_header(struct link_device *dev, uint8_t nav_id, uint8_t msg_id, uint16_ void ubx_trailer(struct link_device *dev) { - dev->put_byte(dev->periph, 0, gps_ubx.send_ck_a); - dev->put_byte(dev->periph, 0, gps_ubx.send_ck_b); + for(uint8_t i = 0; i < GPS_UBX_NB; i++) { + if(gps_ubx[i].dev == dev) { + dev->put_byte(dev->periph, 0, gps_ubx[i].send_ck_a); + dev->put_byte(dev->periph, 0, gps_ubx[i].send_ck_b); + break; + } + } dev->send_message(dev->periph, 0); } @@ -665,9 +654,9 @@ void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes) } } -void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , uint8_t reset_mode) +void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , UNUSED uint8_t reset_mode) { -#ifdef UBX_GPS_LINK +#ifdef UBX_GPS_PORT UbxSend_CFG_RST(dev, bbr, reset_mode, 0x00); #endif /* else less harmful for HITL */ } @@ -678,23 +667,23 @@ void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr , uint8_t reset_mode #include "modules/gps/gps_ubx_ucenter.h" #endif -void gps_ubx_msg(void) +void gps_ubx_msg(struct GpsUbx *gubx) { // current timestamp uint32_t now_ts = get_sys_time_usec(); - gps_ubx.state.last_msg_ticks = sys_time.nb_sec_rem; - gps_ubx.state.last_msg_time = sys_time.nb_sec; - gps_ubx_read_message(); + gubx->state.last_msg_ticks = sys_time.nb_sec_rem; + gubx->state.last_msg_time = sys_time.nb_sec; + gps_ubx_read_message(gubx); gps_ubx_ucenter_event(); - if (gps_ubx.msg_class == UBX_NAV_ID && - (gps_ubx.msg_id == UBX_NAV_VELNED_ID || - gps_ubx.msg_id == UBX_NAV_PVT_ID || - (gps_ubx.msg_id == UBX_NAV_SOL_ID && - !bit_is_set(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT)))) { - if (gps_ubx.state.fix >= GPS_FIX_3D) { - gps_ubx.state.last_3dfix_ticks = sys_time.nb_sec_rem; - gps_ubx.state.last_3dfix_time = sys_time.nb_sec; + if (gubx->msg_class == UBX_NAV_ID && + (gubx->msg_id == UBX_NAV_VELNED_ID || + gubx->msg_id == UBX_NAV_PVT_ID || + (gubx->msg_id == UBX_NAV_SOL_ID && + !bit_is_set(gubx->state.valid_fields, GPS_VALID_VEL_NED_BIT)))) { + if (gubx->state.fix >= GPS_FIX_3D) { + gubx->state.last_3dfix_ticks = sys_time.nb_sec_rem; + gubx->state.last_3dfix_time = sys_time.nb_sec; #ifdef GPS_LED LED_ON(GPS_LED); } else { @@ -703,9 +692,16 @@ void gps_ubx_msg(void) #else } #endif - AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps_ubx.state); + AbiSendMsgGPS(gubx->state.comp_id, now_ts, &gubx->state); + } + gubx->msg_available = false; +} + +void gps_ubx_periodic_check(void) +{ + for(uint8_t i = 0; i < GPS_UBX_NB; i++) { + gps_periodic_check(&gps_ubx[i].state); } - gps_ubx.msg_available = false; } /* @@ -766,7 +762,9 @@ void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data) if (crc1 == crc2) { // write to GPS - gps_ublox_write(&(UBX_GPS_LINK).device, rtcm.buff, rtcm.len + 3); +#ifdef UBX_GPS_PORT + gps_ublox_write(&(UBX_GPS_PORT).device, rtcm.buff, rtcm.len + 3); +#endif switch (packet_id) { case RTCM3_MSG_1005 : break; case RTCM3_MSG_1077 : break; @@ -796,4 +794,4 @@ void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data) } } -#endif +#endif /* USE_GPS_UBX_RTCM */ diff --git a/sw/airborne/modules/gps/gps_ubx.h b/sw/airborne/modules/gps/gps_ubx.h index 3ed089fd28..ca823f2861 100644 --- a/sw/airborne/modules/gps/gps_ubx.h +++ b/sw/airborne/modules/gps/gps_ubx.h @@ -29,16 +29,16 @@ #include "modules/gps/gps.h" -#ifdef GPS_CONFIGURE -#warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE" -#endif - #ifdef GPS_I2C #include "modules/gps/gps_ubx_i2c.h" #else #include "mcu_periph/uart.h" #endif +#ifndef GPS_UBX_NB +#define GPS_UBX_NB 1 +#endif + #ifndef PRIMARY_GPS #define PRIMARY_GPS GPS_UBX #endif @@ -47,12 +47,11 @@ extern void gps_ubx_init(void); extern void gps_ubx_event(void); extern void gps_ubx_parse_HITL_UBX(uint8_t *buf); -#define gps_ubx_periodic_check() gps_periodic_check(&gps_ubx.state) - #define GPS_UBX_NB_CHANNELS 40 - #define GPS_UBX_MAX_PAYLOAD 512 + struct GpsUbx { + struct link_device *dev; bool msg_available; uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned)); uint8_t msg_id; @@ -65,7 +64,6 @@ struct GpsUbx { uint8_t send_ck_a, send_ck_b; uint8_t error_cnt; uint8_t error_last; - uint8_t reset; uint8_t status_flags; uint8_t sol_flags; @@ -74,28 +72,8 @@ struct GpsUbx { struct GpsState state; }; -extern struct GpsUbx gps_ubx; - -#if USE_GPS_UBX_RXM_RAW -struct GpsUbxRawMes { - double cpMes; - double prMes; - float doMes; - uint8_t sv; - int8_t mesQI; - int8_t cno; - uint8_t lli; -}; - -struct GpsUbxRaw { - int32_t iTOW; - int16_t week; - uint8_t numSV; - struct GpsUbxRawMes measures[GPS_UBX_NB_CHANNELS]; -}; - -extern struct GpsUbxRaw gps_ubx_raw; -#endif +extern struct GpsUbx gps_ubx[GPS_UBX_NB]; +extern uint8_t gps_ubx_reset; /* * This part is used by the autopilot to read data from a uart @@ -107,29 +85,6 @@ extern void ubx_trailer(struct link_device *dev); extern void ubx_send_bytes(struct link_device *dev, uint8_t len, uint8_t *bytes); extern void ubx_send_cfg_rst(struct link_device *dev, uint16_t bbr, uint8_t reset_mode); -extern void gps_ubx_read_message(void); -extern void gps_ubx_parse(uint8_t c); -extern void gps_ubx_msg(void); - -/* - * GPS Reset - */ - -#define CFG_RST_Reset_Hardware 0x00 -#define CFG_RST_Reset_Controlled 0x01 -#define CFG_RST_Reset_Controlled_GPS_only 0x02 -#define CFG_RST_Reset_Controlled_GPS_stop 0x08 -#define CFG_RST_Reset_Controlled_GPS_start 0x09 - -#define CFG_RST_BBR_Hotstart 0x0000 -#define CFG_RST_BBR_Warmstart 0x0001 -#define CFG_RST_BBR_Coldstart 0xffff - -#define gps_ubx_Reset(_val) { \ - gps_ubx.reset = _val; \ - if (gps_ubx.reset > CFG_RST_BBR_Warmstart) \ - gps_ubx.reset = CFG_RST_BBR_Coldstart; \ - ubx_send_cfg_rst(&(UBX_GPS_LINK).device, gps_ubx.reset, CFG_RST_Reset_Controlled); \ - } +extern void gps_ubx_periodic_check(void); #endif /* GPS_UBX_H */ diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index b1998a2869..9f3b5d8609 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -39,6 +39,10 @@ #define DEBUG_PRINT(...) {} #endif +#if GPS_UBX_NB > 1 +#warning "Only one GPS_UBX is supported for ucenter configuration." +#endif + ////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////// // @@ -60,7 +64,7 @@ static bool gps_ubx_ucenter_configure(uint8_t nr); #define GPS_UBX_UCENTER_REPLY_CFG_PRT 4 // Target baudrate for the module -#define UBX_GPS_BAUD (UBX_GPS_LINK).baudrate +#define UBX_GPS_BAUD (UBX_GPS_PORT).baudrate // All U-Center data struct gps_ubx_ucenter_struct gps_ubx_ucenter; @@ -89,7 +93,7 @@ void gps_ubx_ucenter_init(void) gps_ubx_ucenter.replies[i] = 0; } - gps_ubx_ucenter.dev = &(UBX_GPS_LINK).device; + gps_ubx_ucenter.dev = &(UBX_GPS_PORT).device; } @@ -164,38 +168,38 @@ void gps_ubx_ucenter_event(void) } // Read Configuration Reply's - switch (gps_ubx.msg_class) { + switch (gps_ubx[0].msg_class) { case UBX_ACK_ID: - if (gps_ubx.msg_id == UBX_ACK_ACK_ID) { + if (gps_ubx[0].msg_id == UBX_ACK_ACK_ID) { gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK; DEBUG_PRINT("ACK\n"); - } else if (gps_ubx.msg_id == UBX_ACK_NAK_ID) { + } else if (gps_ubx[0].msg_id == UBX_ACK_NAK_ID) { gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK; DEBUG_PRINT("NACK\n"); } break; case UBX_MON_ID: - if (gps_ubx.msg_id == UBX_MON_VER_ID) { + if (gps_ubx[0].msg_id == UBX_MON_VER_ID) { gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_VERSION; - gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf, 0) - '0'; - gps_ubx_ucenter.sw_ver_l = 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 2) - '0'); - gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf, 3) - '0'; - gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf, 33) - '0'; - gps_ubx_ucenter.hw_ver_h += 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 32) - '0'); - gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf, 37) - '0'; - gps_ubx_ucenter.hw_ver_l += 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 36) - '0'); + gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx[0].msg_buf, 0) - '0'; + gps_ubx_ucenter.sw_ver_l = 10 * (UBX_MON_VER_c(gps_ubx[0].msg_buf, 2) - '0'); + gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx[0].msg_buf, 3) - '0'; + gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx[0].msg_buf, 33) - '0'; + gps_ubx_ucenter.hw_ver_h += 10 * (UBX_MON_VER_c(gps_ubx[0].msg_buf, 32) - '0'); + gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx[0].msg_buf, 37) - '0'; + gps_ubx_ucenter.hw_ver_l += 10 * (UBX_MON_VER_c(gps_ubx[0].msg_buf, 36) - '0'); DEBUG_PRINT("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l); DEBUG_PRINT("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l); - } else if (gps_ubx.msg_id == UBX_MON_GNSS_ID) { - gps_ubx_ucenter.gnss_in_use = UBX_MON_GNSS_enabled(gps_ubx.msg_buf); + } else if (gps_ubx[0].msg_id == UBX_MON_GNSS_ID) { + gps_ubx_ucenter.gnss_in_use = UBX_MON_GNSS_enabled(gps_ubx[0].msg_buf); } break; case UBX_CFG_ID: - if (gps_ubx.msg_id == UBX_CFG_PRT_ID) { + if (gps_ubx[0].msg_id == UBX_CFG_PRT_ID) { gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_CFG_PRT; - gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf, 0); - gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf, 0); + gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx[0].msg_buf, 0); + gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx[0].msg_buf, 0); DEBUG_PRINT("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run); DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id); @@ -258,7 +262,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) break; case 2: gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(UBX_GPS_LINK), B38400); // Try the most common first? + uart_periph_set_baudrate(&(UBX_GPS_PORT), B38400); // Try the most common first? gps_ubx_ucenter_config_port_poll(); break; case 3: @@ -267,7 +271,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) return false; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(UBX_GPS_LINK), B9600); // Maybe the factory default? + uart_periph_set_baudrate(&(UBX_GPS_PORT), B9600); // Maybe the factory default? gps_ubx_ucenter_config_port_poll(); break; case 4: @@ -276,7 +280,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) return false; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(UBX_GPS_LINK), B57600); // The high-rate default? + uart_periph_set_baudrate(&(UBX_GPS_PORT), B57600); // The high-rate default? gps_ubx_ucenter_config_port_poll(); break; case 5: @@ -285,7 +289,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) return false; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(UBX_GPS_LINK), B4800); // Default NMEA baudrate? + uart_periph_set_baudrate(&(UBX_GPS_PORT), B4800); // Default NMEA baudrate? gps_ubx_ucenter_config_port_poll(); break; case 6: @@ -294,7 +298,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) return false; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(UBX_GPS_LINK), B115200); // Last possible option for ublox + uart_periph_set_baudrate(&(UBX_GPS_PORT), B115200); // Last possible option for ublox gps_ubx_ucenter_config_port_poll(); break; case 7: @@ -303,7 +307,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) return false; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - uart_periph_set_baudrate(&(UBX_GPS_LINK), B230400); // Last possible option for ublox + uart_periph_set_baudrate(&(UBX_GPS_PORT), B230400); // Last possible option for ublox gps_ubx_ucenter_config_port_poll(); break; case 8: @@ -315,7 +319,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr) // Autoconfig Failed... let's setup the failsafe baudrate // Should we try even a different baudrate? gps_ubx_ucenter.baud_init = 0; // Set as zero to indicate that we couldn't verify the baudrate - uart_periph_set_baudrate(&(UBX_GPS_LINK), B9600); + uart_periph_set_baudrate(&(UBX_GPS_PORT), B9600); return false; default: break; @@ -495,7 +499,7 @@ static bool gps_ubx_ucenter_configure(uint8_t nr) } #endif // Now the GPS baudrate should have changed - uart_periph_set_baudrate(&(UBX_GPS_LINK), gps_ubx_ucenter.baud_target); + uart_periph_set_baudrate(&(UBX_GPS_PORT), gps_ubx_ucenter.baud_target); gps_ubx_ucenter.baud_run = UART_SPEED(gps_ubx_ucenter.baud_target); #endif /*GPS_I2C*/ UbxSend_MON_GET_VER(gps_ubx_ucenter.dev); diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp index 5f57af5440..0dc31ee517 100644 --- a/sw/airborne/modules/ins/ins_ekf2.cpp +++ b/sw/airborne/modules/ins/ins_ekf2.cpp @@ -927,6 +927,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), #if INS_EKF2_GPS_COURSE_YAW gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7); gps_msg.yaw_offset = 0; +#elif defined(INS_EKF2_GPS_YAW_OFFSET) + if(ISFINITE(gps_relposned.relPosHeading)) { + gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading)); + } else { + gps_msg.yaw = NAN; + } + gps_msg.yaw_offset = INS_EKF2_GPS_YAW_OFFSET; #else gps_msg.yaw = NAN; gps_msg.yaw_offset = NAN;