Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 27 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
conf/airframes/tudelft/rot_wing_v3g.xml
+7
View File
@@ -82,8 +82,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it --> <define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module> </module>
<module name="air_data"/> <module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox"> <module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/> <configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/> <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module> </module>
@@ -333,6 +337,9 @@
<define name="USE_AIRSPEED" value="TRUE"/> <define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/> <define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/> <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>
<section name="GROUND_DETECT"> <section name="GROUND_DETECT">
conf/airframes/tudelft/rot_wing_v3h.xml
+7
View File
@@ -82,8 +82,12 @@
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it --> <define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module> </module>
<module name="air_data"/> <module name="air_data"/>
<configure name="PRIMARY_GPS" value="ublox"/>
<configure name="SECONDARY_GPS" value="ublox2"/>
<module name="gps" type="ublox"> <module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/> <configure name="UBX_GPS_BAUD" value="B460800"/>
<configure name="UBX2_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/> <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module> </module>
@@ -333,6 +337,9 @@
<define name="USE_AIRSPEED" value="TRUE"/> <define name="USE_AIRSPEED" value="TRUE"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/> <define name="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/> <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>
<section name="GROUND_DETECT"> <section name="GROUND_DETECT">
conf/boards/cube_orangeplus.makefile
+5 -1
View File
@@ -68,10 +68,14 @@ RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT ?= UART3
MODEM_PORT ?= UART2 MODEM_PORT ?= UART2
MODEM_BAUD ?= B57600 MODEM_BAUD ?= B57600
# The GPS1 port (UART8 is GPS2) # The GPS1 port
GPS_PORT ?= UART4 GPS_PORT ?= UART4
GPS_BAUD ?= B57600 GPS_BAUD ?= B57600
# The GPS2 port
GPS2_PORT ?= UART8
GPS2_BAUD ?= B57600
# InterMCU port connected to the IO processor # InterMCU port connected to the IO processor
INTERMCU_PORT ?= UART6 INTERMCU_PORT ?= UART6
INTERMCU_BAUD ?= B1500000 INTERMCU_BAUD ?= B1500000
conf/modules/gps_ublox.xml
+34 -16
View File
@@ -19,7 +19,7 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings name="gps_ublox"> <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>
</dl_settings> </dl_settings>
</settings> </settings>
@@ -36,33 +36,51 @@
<event fun="gps_ubx_event()"/> <event fun="gps_ubx_event()"/>
<datalink message="HITL_UBX" fun="gps_ubx_parse_HITL_UBX(buf)"/> <datalink message="HITL_UBX" fun="gps_ubx_parse_HITL_UBX(buf)"/>
<makefile target="ap|fbw"> <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"/> <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="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)"/> <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> <raw>
ifdef SECONDARY_GPS ifdef SECONDARY_GPS
ifneq (,$(findstring $(SECONDARY_GPS), ublox)) ifneq (,$(findstring ublox2, $(SECONDARY_GPS)))
# this is the secondary GPS # this is the secondary GPS
$(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/gps/gps_ubx.h\" $(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/gps/gps_ubx.h\"
$(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_UBX $(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 else
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\" # plain old single GPS usage
$(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_UBX $(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\"
endif
else
# plain old single GPS usage
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/gps/gps_ubx.h\"
endif endif
</raw> </raw>
<test> <test>
<define name="PRIMARY_GPS" value="GPS_UBX"/> <define name="PRIMARY_GPS" value="GPS_UBX"/>
<define name="USE_UART2"/> <define name="USE_UART2"/>
<define name="UBX_GPS_LINK" value="uart2"/> <define name="UBX_GPS_PORT" value="uart2"/>
</test> </test>
</makefile> </makefile>
</module> </module>
conf/modules/gps_ubx_ucenter.xml
+1 -1
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"/> <file name="gps_ubx_ucenter.c"/>
<define name="GPS_UBX_UCENTER"/> <define name="GPS_UBX_UCENTER"/>
<test> <test>
<define name="UBX_GPS_LINK" value="uart2"/> <define name="UBX_GPS_PORT" value="uart2"/>
<define name="USE_UART2"/> <define name="USE_UART2"/>
</test> </test>
</makefile> </makefile>
sw/airborne/modules/core/abi_sender_ids.h
+4
View File
@@ -296,6 +296,10 @@
#define GPS_DW1000_ID 15 #define GPS_DW1000_ID 15
#endif #endif
#ifndef GPS_UBX2_ID
#define GPS_UBX2_ID 16
#endif
/* /*
* IDs of IMU sensors (accel, gyro) * IDs of IMU sensors (accel, gyro)
*/ */
sw/airborne/modules/gps/gps.c
-22
View File
@@ -69,7 +69,6 @@ PRINT_CONFIG_VAR(SECONDARY_GPS)
struct GpsState gps; struct GpsState gps;
struct GpsTimeSync gps_time_sync; struct GpsTimeSync gps_time_sync;
struct GpsRelposNED gps_relposned; struct GpsRelposNED gps_relposned;
struct RtcmMan rtcm_man;
#ifdef SECONDARY_GPS #ifdef SECONDARY_GPS
static uint8_t current_gps_id = GpsId(PRIMARY_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); &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) static void send_gps_int(struct transport_tx *trans, struct link_device *dev)
{ {
#if PPRZLINK_DEFAULT_VER == 2 && GPS_POS_BROADCAST #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_GPS_SOL, send_gps_sol);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_SVINFO, send_svinfo); 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_RTK, send_gps_rtk);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_RXMRTCM, send_gps_rxmrtcm);
#endif #endif
/* Register preflight checks */ /* Register preflight checks */
#if PREFLIGHT_CHECKS #if PREFLIGHT_CHECKS
preflight_check_register(&gps_pfc, gps_preflight); preflight_check_register(&gps_pfc, gps_preflight);
#endif #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) uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
sw/airborne/modules/gps/gps.h
+3 -11
View File
@@ -133,6 +133,8 @@ struct GpsRelposNED {
int8_t relPosHPN; int8_t relPosHPN;
int8_t relPosHPE; int8_t relPosHPE;
int8_t relPosHPD; int8_t relPosHPD;
float relPosLength;
float relPosHeading;
uint32_t accN; uint32_t accN;
uint32_t accE; uint32_t accE;
uint32_t accD; uint32_t accD;
@@ -141,17 +143,7 @@ struct GpsRelposNED {
uint8_t diffSoln; uint8_t diffSoln;
uint8_t gnssFixOK; uint8_t gnssFixOK;
}; };
extern struct GpsRelposNED gps_relposned;
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;
};
/** global GPS state */ /** global GPS state */
extern struct GpsState gps; extern struct GpsState gps;
sw/airborne/modules/gps/gps_ubx.c
+311 -313
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/gps/gps_ubx.h
+9 -54
View File
@@ -29,16 +29,16 @@
#include "modules/gps/gps.h" #include "modules/gps/gps.h"
#ifdef GPS_CONFIGURE
#warning "Please use gps_ubx_ucenter.xml module instead of GPS_CONFIGURE"
#endif
#ifdef GPS_I2C #ifdef GPS_I2C
#include "modules/gps/gps_ubx_i2c.h" #include "modules/gps/gps_ubx_i2c.h"
#else #else
#include "mcu_periph/uart.h" #include "mcu_periph/uart.h"
#endif #endif
#ifndef GPS_UBX_NB
#define GPS_UBX_NB 1
#endif
#ifndef PRIMARY_GPS #ifndef PRIMARY_GPS
#define PRIMARY_GPS GPS_UBX #define PRIMARY_GPS GPS_UBX
#endif #endif
@@ -47,12 +47,11 @@ extern void gps_ubx_init(void);
extern void gps_ubx_event(void); extern void gps_ubx_event(void);
extern void gps_ubx_parse_HITL_UBX(uint8_t *buf); 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_NB_CHANNELS 40
#define GPS_UBX_MAX_PAYLOAD 512 #define GPS_UBX_MAX_PAYLOAD 512
struct GpsUbx { struct GpsUbx {
struct link_device *dev;
bool msg_available; bool msg_available;
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned)); uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned));
uint8_t msg_id; uint8_t msg_id;
@@ -65,7 +64,6 @@ struct GpsUbx {
uint8_t send_ck_a, send_ck_b; uint8_t send_ck_a, send_ck_b;
uint8_t error_cnt; uint8_t error_cnt;
uint8_t error_last; uint8_t error_last;
uint8_t reset;
uint8_t status_flags; uint8_t status_flags;
uint8_t sol_flags; uint8_t sol_flags;
@@ -74,28 +72,8 @@ struct GpsUbx {
struct GpsState state; struct GpsState state;
}; };
extern struct GpsUbx gps_ubx; extern struct GpsUbx gps_ubx[GPS_UBX_NB];
extern uint8_t gps_ubx_reset;
#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
/* /*
* This part is used by the autopilot to read data from a uart * 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_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 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_periodic_check(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); \
}
#endif /* GPS_UBX_H */ #endif /* GPS_UBX_H */
sw/airborne/modules/gps/gps_ubx_ucenter.c
+30 -26
View File
@@ -39,6 +39,10 @@
#define DEBUG_PRINT(...) {} #define DEBUG_PRINT(...) {}
#endif #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 #define GPS_UBX_UCENTER_REPLY_CFG_PRT 4
// Target baudrate for the module // 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 // All U-Center data
struct gps_ubx_ucenter_struct gps_ubx_ucenter; 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.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 // Read Configuration Reply's
switch (gps_ubx.msg_class) { switch (gps_ubx[0].msg_class) {
case UBX_ACK_ID: 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; gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK;
DEBUG_PRINT("ACK\n"); 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; gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK;
DEBUG_PRINT("NACK\n"); DEBUG_PRINT("NACK\n");
} }
break; break;
case UBX_MON_ID: 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.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_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.msg_buf, 2) - '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.msg_buf, 3) - '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.msg_buf, 33) - '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.msg_buf, 32) - '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.msg_buf, 37) - '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.msg_buf, 36) - '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 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); 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) { } else if (gps_ubx[0].msg_id == UBX_MON_GNSS_ID) {
gps_ubx_ucenter.gnss_in_use = UBX_MON_GNSS_enabled(gps_ubx.msg_buf); gps_ubx_ucenter.gnss_in_use = UBX_MON_GNSS_enabled(gps_ubx[0].msg_buf);
} }
break; break;
case UBX_CFG_ID: 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.reply = GPS_UBX_UCENTER_REPLY_CFG_PRT;
gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(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.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.baud_run: %u\n", gps_ubx_ucenter.baud_run);
DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id); 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; break;
case 2: case 2:
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; 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(); gps_ubx_ucenter_config_port_poll();
break; break;
case 3: case 3:
@@ -267,7 +271,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr)
return false; return false;
} }
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; 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(); gps_ubx_ucenter_config_port_poll();
break; break;
case 4: case 4:
@@ -276,7 +280,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr)
return false; return false;
} }
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; 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(); gps_ubx_ucenter_config_port_poll();
break; break;
case 5: case 5:
@@ -285,7 +289,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr)
return false; return false;
} }
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; 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(); gps_ubx_ucenter_config_port_poll();
break; break;
case 6: case 6:
@@ -294,7 +298,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr)
return false; return false;
} }
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; 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(); gps_ubx_ucenter_config_port_poll();
break; break;
case 7: case 7:
@@ -303,7 +307,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr)
return false; return false;
} }
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; 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(); gps_ubx_ucenter_config_port_poll();
break; break;
case 8: case 8:
@@ -315,7 +319,7 @@ static bool gps_ubx_ucenter_autobaud(uint8_t nr)
// Autoconfig Failed... let's setup the failsafe baudrate // Autoconfig Failed... let's setup the failsafe baudrate
// Should we try even a different 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 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; return false;
default: default:
break; break;
@@ -495,7 +499,7 @@ static bool gps_ubx_ucenter_configure(uint8_t nr)
} }
#endif #endif
// Now the GPS baudrate should have changed // 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); gps_ubx_ucenter.baud_run = UART_SPEED(gps_ubx_ucenter.baud_target);
#endif /*GPS_I2C*/ #endif /*GPS_I2C*/
UbxSend_MON_GET_VER(gps_ubx_ucenter.dev); UbxSend_MON_GET_VER(gps_ubx_ucenter.dev);
sw/airborne/modules/ins/ins_ekf2.cpp
+7
View File
@@ -927,6 +927,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
#if INS_EKF2_GPS_COURSE_YAW #if INS_EKF2_GPS_COURSE_YAW
gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7); gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
gps_msg.yaw_offset = 0; 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 #else
gps_msg.yaw = NAN; gps_msg.yaw = NAN;
gps_msg.yaw_offset = NAN; gps_msg.yaw_offset = NAN;