[ubx] improve ubx parser and ubx_ucenter module

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

conf/ubx.xml

@@ -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"/>
@@ -93,6 +95,7 @@
         <field name="PRRes" format="I4" unit="cm"/>
       </block>
     </message>
+
   </class>
 
   <class name="CFG" ID="0x06">
@@ -190,6 +193,7 @@
       <field name="res3" format="U4"/>
       <field name="res4" format="U4"/>
     </message>
+
   </class>
 
   <class name="ACK" ID="0x05">
@@ -198,6 +202,7 @@
       <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"/>
@@ -205,6 +210,26 @@
 
   </class>
 
+  <class name="RXM" ID="0x02">
+
+    <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">

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);
+    GpsUartSetBaudrate(UBX_GPS_BAUD);
-    gps_ubx_ucenter.baud_run = 38400;
+    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

@@ -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;
+  uint32_t baud_init;
-  uint16_t baud_run;
+  uint32_t baud_run;
 
   uint8_t sw_ver_h;
   uint8_t sw_ver_l;

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
 }
 
 

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

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)