Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-02-05 18:51:00 +08:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

[modules] Support dual ublox GPS modules (#3267)

Browse Source 
* [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 <dewagter@gmail.com>
This commit is contained in:
Freek van Tienen
2024-05-24 09:55:16 +02:00
committed by GitHub
parent d748af1afb
commit 58caa662cd
12 changed files with 418 additions and 444 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
conf/airframes/tudelft/rot_wing_v3g.xml
View File

@@ -82,8 +82,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
@@ -333,6 +337,9 @@
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="MULTI_GPS_MODE" value="PRIMARY_GPS"/>
</section>
<section name="GROUND_DETECT">

7
conf/airframes/tudelft/rot_wing_v3h.xml
View File

@@ -82,8 +82,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
@@ -333,6 +337,9 @@
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
<define name="INS_EKF2_GPS_YAW_OFFSET" value="0"/>
<define name="MULTI_GPS_MODE" value="PRIMARY_GPS"/>
</section>
<section name="GROUND_DETECT">

6
conf/boards/cube_orangeplus.makefile
View File

@@ -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

34
conf/modules/gps_ublox.xml
View File

@@ -19,7 +19,7 @@
<settings>
<dl_settings>
<dl_settings name="gps_ublox">
<dl_setting MIN="1" MAX="3" STEP="1" values="Hotstart|Warmstart|Coldstart" module="gps/gps_ubx" VAR="gps_ubx.reset" shortname="reset"/>
<dl_setting MIN="1" MAX="3" STEP="1" values="Hotstart|Warmstart|Coldstart" module="gps/gps_ubx" VAR="gps_ubx_reset" shortname="reset"/>
</dl_settings>
</dl_settings>
</settings>
@@ -36,21 +36,39 @@
<event fun="gps_ubx_event()"/>
<datalink message="HITL_UBX" fun="gps_ubx_parse_HITL_UBX(buf)"/>
<makefile target="ap|fbw">
<configure name="UBX_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
<configure name="UBX_GPS_BAUD" default="$(GPS_BAUD)"/>
<file name="gps_ubx.c"/>
<!-- GPS Ublox 1 -->
<configure name="UBX_GPS_PORT" default="$(GPS_PORT)" case="upper|lower"/>
<configure name="UBX_GPS_BAUD" default="$(GPS_BAUD)"/>
<define name="USE_$(UBX_GPS_PORT_UPPER)"/>
<define name="UBX_GPS_LINK" value="$(UBX_GPS_PORT_LOWER)"/>
<define name="UBX_GPS_PORT" value="$(UBX_GPS_PORT_LOWER)"/>
<define name="$(UBX_GPS_PORT_UPPER)_BAUD" value="$(UBX_GPS_BAUD)"/>
<!-- GPS Ublox 2 -->
<configure name="UBX2_GPS_PORT" default="$(GPS2_PORT)" case="upper|lower"/>
<configure name="UBX2_GPS_BAUD" default="$(GPS2_BAUD)"/>
<define name="USE_$(UBX2_GPS_PORT_UPPER)" cond="ifneq ($(UBX2_GPS_PORT)$(SECONDARY_GPS),)"/>
<define name="UBX2_GPS_PORT" value="$(UBX2_GPS_PORT_LOWER)" cond="ifneq ($(UBX2_GPS_PORT)$(SECONDARY_GPS),)"/>
<define name="$(UBX2_GPS_PORT_UPPER)_BAUD" value="$(UBX2_GPS_BAUD)" cond="ifneq ($(UBX2_GPS_BAUD)$(SECONDARY_GPS),)"/>
<define name="GPS_UBX_NB" value="2" cond="ifneq ($(UBX2_GPS_PORT)$(SECONDARY_GPS),)"/>
<raw>
ifdef SECONDARY_GPS
ifneq (,$(findstring $(SECONDARY_GPS), ublox))
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
else
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
@@ -62,7 +80,7 @@
<test>
<define name="PRIMARY_GPS" value="GPS_UBX"/>
<define name="USE_UART2"/>
<define name="UBX_GPS_LINK" value="uart2"/>
<define name="UBX_GPS_PORT" value="uart2"/>
</test>
</makefile>
</module>

2
conf/modules/gps_ubx_ucenter.xml
View File

@@ -46,7 +46,7 @@ Warning: you still need to tell the driver, which paparazzi port you use.
<file name="gps_ubx_ucenter.c"/>
<define name="GPS_UBX_UCENTER"/>
<test>
<define name="UBX_GPS_LINK" value="uart2"/>
<define name="UBX_GPS_PORT" value="uart2"/>
<define name="USE_UART2"/>
</test>
</makefile>

4
sw/airborne/modules/core/abi_sender_ids.h
View File

@@ -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)
*/

22
sw/airborne/modules/gps/gps.c
View File

@@ -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)

14
sw/airborne/modules/gps/gps.h
View File

@@ -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;

618
sw/airborne/modules/gps/gps_ubx.c
View File

File diff suppressed because it is too large Load Diff

63
sw/airborne/modules/gps/gps_ubx.h
View File

@@ -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 */

56
sw/airborne/modules/gps/gps_ubx_ucenter.c
View File

@@ -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);

7
sw/airborne/modules/ins/ins_ekf2.cpp
View File

@@ -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;