Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ubx] improve ubx parser and ubx_ucenter module

Browse Source 
- configurable data output rate
- configurable baud rate (taken from GPS_BAUD variable)
- add support of double and float to ubx parser
- add option to configure RXM_RAW messages (on LEA-T versions)
This commit is contained in:
Gautier Hattenberger
2013-11-21 11:16:19 +01:00
parent 3f75ab1a6a
commit 485887a822
6 changed files with 125 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/ubx.xml
+49 -24
View File
@@ -4,7 +4,9 @@
<!DOCTYPE ubx SYSTEM "ubx.dtd">
<ubx>
<class name="NAV" ID="0x01">
<message name="POSLLH" ID="0x02" length="28">
<field name="ITOW" format="U4" unit="ms"/>
<field name="LON" format="I4" scaling="1e-7" unit="deg"/>
@@ -92,10 +94,11 @@
<field name="Azim" format="I2" unit="deg"/>
<field name="PRRes" format="I4" unit="cm"/>
</block>
</message>
</class>
</message>
<class name="CFG" ID="0x06">
</class>
<class name="CFG" ID="0x06">
<message name="PRT" ID="0x00">
<block length="20">
@@ -189,33 +192,55 @@
<field name="res2" format="U4"/>
<field name="res3" format="U4"/>
<field name="res4" format="U4"/>
</message>
</class>
</message>
<class name="ACK" ID="0x05">
</class>
<message name="ACK" ID="0x01" length="2">
<field name="ClsID" format="U1"/>
<field name="MsgID" format="U1"/>
</message>
<message name="NAK" ID="0x00" length="2">
<field name="ClsID" format="U1"/>
<field name="MsgID" format="U1"/>
</message>
<class name="ACK" ID="0x05">
</class>
<message name="ACK" ID="0x01" length="2">
<field name="ClsID" format="U1"/>
<field name="MsgID" format="U1"/>
</message>
<class name="MON" ID="0x0A">
<message name="NAK" ID="0x00" length="2">
<field name="ClsID" format="U1"/>
<field name="MsgID" format="U1"/>
</message>
<message name="GET_VER" ID="0x04" length="0">
</message>
</class>
<message name="VER" ID="0x04">
<block length="1">
<field name="c" format="U1"/>
</block>
</message>
<class name="RXM" ID="0x02">
</class>
<message name="RAW" ID="0x10">
<field name="iTOW" format="I4" unit="ms"/>
<field name="week" format="I2" unit="weeks"/>
<field name="numSV" format="U1"/>
<field name="reserverd1" format="U1"/>
<block length="24">
<field name="cpMes" format="R8" unit="cycles"/>
<field name="prMes" format="R8" unit="m"/>
<field name="doMes" format="R4" unit="Hz"/>
<field name="sv" format="U1"/>
<field name="mesQI" format="I1"/>
<field name="cno" format="I1" unit="dbHz"/>
<field name="lli" format="U1"/>
</block>
</message>
</class>
<class name="MON" ID="0x0A">
<message name="GET_VER" ID="0x04" length="0">
</message>
<message name="VER" ID="0x04">
<block length="1">
<field name="c" format="U1"/>
</block>
</message>
</class>
</ubx>
sw/airborne/modules/gps/gps_ubx_ucenter.c
+14 -5
View File
@@ -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);
sw/airborne/modules/gps/gps_ubx_ucenter.h
+3 -3
View File
@@ -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;
sw/airborne/subsystems/gps/gps_ubx.c
+23
View File
@@ -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
}
sw/airborne/subsystems/gps/gps_ubx.h
+21
View File
@@ -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
sw/tools/gen_ubx.ml
+15 -5
View File
@@ -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)