diff --git a/conf/ubx.xml b/conf/ubx.xml index ddc9f1850e..a45f5d1ee9 100644 --- a/conf/ubx.xml +++ b/conf/ubx.xml @@ -4,7 +4,9 @@ + + @@ -92,10 +94,11 @@ - - + - + + + @@ -189,33 +192,55 @@ - - + - + - - - - - - - - + - + + + + - + + + + - - + - - - - - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index afcea7d488..683c855cbd 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -285,6 +285,10 @@ static inline void gps_ubx_ucenter_config_nav(void) #define _UBX_GPS_BAUD(_u) __UBX_GPS_BAUD(_u) #define UBX_GPS_BAUD _UBX_GPS_BAUD(GPS_LINK) +#ifndef GPS_UBX_UCENTER_RATE +#define GPS_UBX_UCENTER_RATE 0x00FA +#endif + static inline void gps_ubx_ucenter_config_port(void) { // I2C Interface @@ -293,7 +297,7 @@ static inline void gps_ubx_ucenter_config_port(void) #endif // UART Interface #if GPS_PORT_ID == GPS_PORT_UART1 || GPS_PORT_ID == GPS_PORT_UART2 - UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, 38400, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x000008D0, UBX_GPS_BAUD, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); #endif #if GPS_PORT_ID == GPS_PORT_USB UbxSend_CFG_PRT(GPS_PORT_ID, 0x0, 0x0, 0x0, 0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); @@ -374,8 +378,8 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) break; case 6: // Now the GPS baudrate should have changed - GpsUartSetBaudrate(B38400); - gps_ubx_ucenter.baud_run = 38400; + GpsUartSetBaudrate(UBX_GPS_BAUD); + gps_ubx_ucenter.baud_run = UBX_GPS_BAUD; gps_ubx_ucenter_config_nav(); break; case 7: @@ -405,13 +409,18 @@ static bool_t gps_ubx_ucenter_configure(uint8_t nr) gps_ubx_ucenter_config_sbas(); break; case 14: - UbxSend_CFG_RATE(0x00FA, 0x0001, 0x0000); + UbxSend_CFG_RATE(GPS_UBX_UCENTER_RATE, 0x0001, 0x0000); break; case 15: +#if USE_GPS_UBX_RXM_RAW + gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_RAW_ID, 1); +#endif + break; + case 16: // Try to save on non-ROM devices... UbxSend_CFG_CFG(0x00000000,0xffffffff,0x00000000); break; - case 16: + case 17: #if DEBUG_GPS_UBX_UCENTER // Debug Downlink the result of all configuration steps: see messages DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies); diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.h b/sw/airborne/modules/gps/gps_ubx_ucenter.h index b0e93bf428..2bd4317085 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.h +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.h @@ -31,7 +31,7 @@ #include "std.h" /** U-Center Variables */ -#define GPS_UBX_UCENTER_CONFIG_STEPS 17 +#define GPS_UBX_UCENTER_CONFIG_STEPS 18 struct gps_ubx_ucenter_struct { @@ -39,8 +39,8 @@ struct gps_ubx_ucenter_struct uint8_t reply; uint8_t cnt; - uint16_t baud_init; - uint16_t baud_run; + uint32_t baud_init; + uint32_t baud_run; uint8_t sw_ver_h; uint8_t sw_ver_l; diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 160042852e..f1df97f524 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -57,6 +57,10 @@ struct GpsUbx gps_ubx; +#if USE_GPS_UBX_RXM_RAW +struct GpsUbxRaw gps_ubx_raw; +#endif + void gps_impl_init(void) { gps_ubx.status = UNINIT; gps_ubx.msg_available = FALSE; @@ -161,6 +165,25 @@ void gps_ubx_read_message(void) { gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf); } } +#if USE_GPS_UBX_RXM_RAW + else if (gps_ubx.msg_class == UBX_RXM_ID) { + if (gps_ubx.msg_id == UBX_RXM_RAW_ID) { + 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; + for (i = 0; i < gps_ubx_raw.numSV; 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 } diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index e2d1c2ce58..1c1c08839e 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -56,10 +56,31 @@ struct GpsUbx { uint8_t status_flags; uint8_t sol_flags; uint8_t have_velned; + }; 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_NB_CHANNELS]; +}; + +extern struct GpsUbxRaw gps_ubx_raw; +#endif /* * This part is used by the autopilot to read data from a uart diff --git a/sw/tools/gen_ubx.ml b/sw/tools/gen_ubx.ml index f9509158fa..3501648b33 100644 --- a/sw/tools/gen_ubx.ml +++ b/sw/tools/gen_ubx.ml @@ -27,7 +27,8 @@ open Printf let out = stdout let sizeof = function -"U4" | "I4" -> 4 + | "R8" -> 8 + | "U4" | "I4" | "R4" -> 4 | "U2" | "I2" -> 2 | "U1" | "I1" -> 1 | x -> failwith (sprintf "sizeof: unknown format '%s'" x) @@ -36,12 +37,14 @@ let (+=) = fun r x -> r := !r + x let c_type = fun format -> match format with - "I2" -> "int16_t" + | "R8" -> "double" + | "R4" -> "float" | "I4" -> "int32_t" - | "U2" -> "uint16_t" | "U4" -> "uint32_t" - | "U1" -> "uint8_t" + | "I2" -> "int16_t" + | "U2" -> "uint16_t" | "I1" -> "int8_t" + | "U1" -> "uint8_t" | _ -> failwith (sprintf "Gen_ubx.c_type: unknown format '%s'" format) let get_at = fun offset format block_size -> @@ -49,7 +52,14 @@ let get_at = fun offset format block_size -> let block_offset = if block_size = 0 then "" else sprintf "+%d*_ubx_block" block_size in match format with - "U4" | "I4" -> sprintf "(%s)(*((uint8_t*)_ubx_payload+%d%s)|*((uint8_t*)_ubx_payload+1+%d%s)<<8|((%s)*((uint8_t*)_ubx_payload+2+%d%s))<<16|((%s)*((uint8_t*)_ubx_payload+3+%d%s))<<24)" t offset block_offset offset block_offset t offset block_offset t offset block_offset + | "R8" -> + let s = ref (sprintf "*((uint8_t*)_ubx_payload+%d%s)" offset block_offset) in + for i = 1 to 7 do + s := !s ^ sprintf "|((uint64_t)*((uint8_t*)_ubx_payload+%d+%d%s))<<%d" i offset block_offset (8*i) + done; + sprintf "({ union { uint64_t u; double f; } _f; _f.u = (uint64_t)(%s); /*Swap32IfBigEndian(_f.u)*/; _f.f; })" !s + | "R4" -> sprintf "({ union { uint32_t u; float f; } _f; _f.u = (uint32_t)(*((uint8_t*)_ubx_payload+%d%s)|*((uint8_t*)_ubx_payload+1+%d%s)<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+%d%s))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+%d%s))<<24); _f.f; })" offset block_offset offset block_offset offset block_offset offset block_offset + | "U4" | "I4" -> sprintf "(%s)(*((uint8_t*)_ubx_payload+%d%s)|*((uint8_t*)_ubx_payload+1+%d%s)<<8|((%s)*((uint8_t*)_ubx_payload+2+%d%s))<<16|((%s)*((uint8_t*)_ubx_payload+3+%d%s))<<24)" t offset block_offset offset block_offset t offset block_offset t offset block_offset | "U2" | "I2" -> sprintf "(%s)(*((uint8_t*)_ubx_payload+%d%s)|*((uint8_t*)_ubx_payload+1+%d%s)<<8)" t offset block_offset offset block_offset | "U1" | "I1" -> sprintf "(%s)(*((uint8_t*)_ubx_payload+%d%s))" t offset block_offset | _ -> failwith (sprintf "Gen_ubx.c_type: unknown format '%s'" format)