Skip to content

Commit

Permalink
[rtcm3-rtk] RTK using ublox M8P-2
Browse files Browse the repository at this point in the history
  • Loading branch information
@mavlab-field
mavlab-field committed Oct 24, 2016
1 parent 68f1bed commit a00eace
Show file tree
Hide file tree
Showing 18 changed files with 1,352 additions and 13 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Expand Up @@ -159,6 +159,7 @@ paparazzi.sublime-workspace
/sw/ground_segment/misc/kestrel2ivy
/sw/ground_segment/misc/natnet2ivy
/sw/ground_segment/misc/sbp2ivy
/sw/ground_segment/misc/rtcm2ivy
/sw/ground_segment/misc/video_synchronizer
/sw/ground_segment/misc/ivy2serial
/sw/ground_segment/misc/sbs2ivy
Expand Down
6 changes: 6 additions & 0 deletions .packages
View file
Expand Up @@ -5,3 +5,9 @@
rcommit = f53f1f326a730caf6fe5ec9bf8e84f04c7c8e986
rpath = tools

[package "sw/airborne/subsystems/gps"]
url = https://github.com/paparazzi/ubx_RTK_groundstation.git
lcommit = 81e7e4661bcb1261e89876d0fb1d7f41a881d7ac
lpath = sw/airborne/subsystems/gps/librtcm3
rcommit = 27ceb21c7d60548faa8c362d1dbde00057c5e678
rpath = lib
1 change: 1 addition & 0 deletions conf/airframes/examples/bebop2_indi.xml
View file
Expand Up @@ -14,6 +14,7 @@
<define name="STABILIZATION_INDI_G2_R" value="0.0"/>
</target>

<!-- define name="USE_GPS_UBX_RTCM" value="TRUE"/-->
<!-- Subsystem section -->
<module name="telemetry" type="transparent_udp"/>
<module name="radio_control" type="datalink"/>
Expand Down
1 change: 1 addition & 0 deletions conf/control_panel_example.xml
View file
Expand Up @@ -67,6 +67,7 @@
<program name="SVInfo" command="sw/ground_segment/python/svinfo/svinfo.py"/>
<program name="IridiumDialer" command="sw/tools/iridium/iridium_link.py"/>
<program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
<program name="rtcm2ivy" command="sw/ground_segment/misc/rtcm2ivy"/>
</section>

<section name="sessions">
Expand Down
6 changes: 6 additions & 0 deletions conf/telemetry/default_rotorcraft.xml
View file
Expand Up @@ -158,6 +158,12 @@
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>

<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>

</process>

Expand Down
125 changes: 125 additions & 0 deletions conf/ubx.xml
View file
Expand Up @@ -95,6 +95,77 @@
<field name="PRRes" format="I4" unit="cm"/>
</block>
</message>

<message name="HPPOSECEF" ID="0x13" length="28">
<field name="VERSION" format="U1"/>
<field name="RES1" format="U2"/>
<field name="RES2" format="U1"/>
<field name="ITOW" format="U4" unit="ms"/>
<field name="ECEF_X" format="I4" unit="cm"/>
<field name="ECEF_Y" format="I4" unit="cm"/>
<field name="ECEF_Z" format="I4" unit="cm"/>
<field name="ECEF_XHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="ECEF_YHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="ECEF_ZHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="RES3" format="U1"/>
<field name="Pacc" format="U4" scaling="1e-1" unit="mm"/>
</message>

<message name="HPPOSLLH" ID="0x14" length="36">
<field name="VERSION" format="U1"/>
<field name="RES1" format="U2"/>
<field name="RES2" format="U1"/>
<field name="ITOW" format="U4" unit="ms"/>
<field name="LON" format="I4" scaling="1e-7" unit="deg"/>
<field name="LAT" format="I4" scaling="1e-7" unit="deg"/>
<field name="HEIGHT" format="I4" unit="mm"/>
<field name="HMSL" format="I4" unit="mm"/>
<field name="LONHP" format="I1" scaling="1e-9" unit="deg"/>
<field name="LATHP" format="I1" scaling="1e-9" unit="deg"/>
<field name="HEIGHTHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="HMSLHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="Hacc" format="U4" scaling="1e-1" unit="mm"/>
<field name="Vacc" format="U4" scaling="1e-1" unit="mm"/>
</message>

<message name="RELPOSNED" ID="0x3C" length="40">
<field name="VERSION" format="U1"/>
<field name="RES1" format="U1"/>
<field name="refStationId" format="U2"/>
<field name="ITOW" format="U4" unit="ms"/>
<field name="RELPOSN" format="I4" unit="cm"/>
<field name="RELPOSE" format="I4" unit="cm"/>
<field name="RELPOSD" format="I4" unit="cm"/>
<field name="RELPOSNHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="RELPOSEHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="RELPOSDHP" format="I1" scaling="1e-1" unit="mm"/>
<field name="RES2" format="U1"/>
<field name="Nacc" format="U4" scaling="1e-1" unit="mm"/>
<field name="Eacc" format="U4" scaling="1e-1" unit="mm"/>
<field name="Dacc" format="U4" scaling="1e-1" unit="mm"/>
<field name="Flags" format="U4"/>
</message>

<message name="SVIN" ID="0x3B" length="40">
<field name="VERSION" format="U1" />
<field name="RES1" format="U1" />
<field name="RES2" format="U2" />
<field name="ITOW" format="U4" unit="ms" />
<field name="dur" format="U4" unit="s" />
<field name="meanX" format="I4" unit="cm" />
<field name="meanY" format="I4" unit="cm" />
<field name="meanZ" format="I4" unit="cm" />
<field name="meanXHP" format="I1" scaling="1e-1" unit="cm" />
<field name="meanYHP" format="I1" scaling="1e-1" unit="cm" />
<field name="meanZHP" format="I1" scaling="1e-1" unit="cm" />
<field name="RES3" format="U1" />
<field name="meanACC" format="U4" scaling="1e-1" unit="mm" />
<field name="OBS" format="U4" />
<field name="Valid" format="U1" />
<field name="Active" format="U1" />
<field name="RES4" format="U2" />
</message>


</msg_class>

Expand Down Expand Up @@ -191,7 +262,54 @@
<field name="res3" format="U4"/>
<field name="res4" format="U4"/>
</message>

<message name="NAV5_HPG" ID="0x24" length="36">
<field name="mask" format="U2"/>
<field name="dynModel" format="U1"/>
<field name="fixModel" format="U1"/>
<field name="fixedAlt" format="I4"/>
<field name="fixedAltVar" format="U4"/>
<field name="minElev" format="I1"/>
<field name="drLimit" format="U1"/>
<field name="dDop" format="U2"/>
<field name="tDop" format="U2"/>
<field name="pAcc" format="U2"/>
<field name="tAcc" format="U2"/>
<field name="staticHoldThresh" format="U1"/>
<field name="dgnssTimeout" format="U1" unit="s"/>
<field name="cnoThreshNumSVs" format="U1"/>
<field name="cnoThresh" format="U1" unit="dBHz"/>
<field name="res1" format="U2"/>
<field name="staticHoldMaxDist" format="U2" unit="m"/>
<field name="utcStandard" format="U1"/>
<field name="res2" format="U4"/>
<field name="res3" format="U1"/>
</message>

<message name="DGNSS" ID="0x70" length="4">
<field name="dgnssMode" format="U1"/>
<field name="RES1" format="U2"/>
<field name="RES2" format="U1"/>
</message>

<message name="TMODE3" ID="0x71" length="40">
<field name="version" format="U1"/>
<field name="RES1" format="U1"/>
<field name="flags" format="U2"/>
<field name="EcecfXOrLat" format="I4"/>
<field name="EcecfYOrLon" format="I4"/>
<field name="EcecfZOrAlt" format="I4" unit="cm"/>
<field name="EcecfXOrLatHP" format="I1"/>
<field name="EcecfYOrLonHP" format="I1"/>
<field name="EcecfZOrAltHP" format="I1" scaling="1e-1" unit="mm" />
<field name="RES2" format="U1"/>
<field name="FixedPosACC" format="U4" scaling="1e-1" unit="mm"/>
<field name="SvinMinDur" format="U4" unit="s" />
<field name="SvinAcclimit" format="U4" scaling="1e-1" unit="mm" />
<field name="RES3" format="U4"/>
<field name="RES4" format="U4"/>
</message>

</msg_class>

<msg_class name="ACK" ID="0x05">
Expand Down Expand Up @@ -249,6 +367,13 @@ supported.
<field name="dwrd9" format="U4"/>
</message>

<message name="RTCM" ID="0x32" length="8">
<field name="version" format="U1"/>
<field name="flags" format="U1"/>
<field name="reserved1" format="U2"/>
<field name="refStation" format="U2"/>
<field name="msgType" format="U2"/>
</message>
</msg_class>

<msg_class name="MON" ID="0x0A">
Expand Down
44 changes: 40 additions & 4 deletions sw/airborne/modules/gps/gps_ubx_ucenter.c
View file
Expand Up @@ -367,17 +367,25 @@ static inline void gps_ubx_ucenter_config_nav(void)
{
// New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available
// If version message couldn't be fetched, default to NAV5
DEBUG_PRINT("sw_ver: %i, hw_ver: %i\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.hw_ver_h);
if (gps_ubx_ucenter.sw_ver_h < 5 && gps_ubx_ucenter.hw_ver_h < 6 &&
gps_ubx_ucenter.sw_ver_h != 0 && gps_ubx_ucenter.hw_ver_h != 0) {
UbxSend_CFG_NAV(gps_ubx_ucenter.dev,
NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C,
0x3C, 0x14, 0x03E8 , 0x0000, 0x0, 0x17, 0x00FA, 0x00FA,
0x0064, 0x012C, 0x000F, 0x00, 0x00);
} else {
#if USE_GPS_UBX_RTCM
UbxSend_CFG_NAV5_HPG(gps_ubx_ucenter.dev,
NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED,
NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC,
NAV5_DEFAULT_STATIC_HOLD_THRES, 125, 0, 0, RESERVED, 0, 0, RESERVED, RESERVED);
#else
UbxSend_CFG_NAV5(gps_ubx_ucenter.dev,
NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED,
NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC,
NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED);
#endif
}
}

Expand All @@ -396,6 +404,7 @@ static inline void gps_ubx_ucenter_config_nav(void)
#define UBX_PROTO_MASK 0x0001
#define NMEA_PROTO_MASK 0x0002
#define RTCM_PROTO_MASK 0x0004
#define RTCM3_PROTO_MASK 0x0020

#define GPS_PORT_DDC 0x00
#define GPS_PORT_UART1 0x01
Expand Down Expand Up @@ -423,9 +432,10 @@ static inline void gps_ubx_ucenter_config_port(void)
case GPS_PORT_UART1:
case GPS_PORT_UART2:
UbxSend_CFG_PRT(gps_ubx_ucenter.dev,
gps_ubx_ucenter.port_id, 0x0, 0x0,
UBX_UART_MODE_MASK, UART_SPEED(gps_ubx_ucenter.baud_target), UBX_PROTO_MASK | NMEA_PROTO_MASK,
UBX_PROTO_MASK| NMEA_PROTO_MASK, 0x0, 0x0);
gps_ubx_ucenter.port_id, RESERVED, RESERVED,
UBX_UART_MODE_MASK, UART_SPEED(gps_ubx_ucenter.baud_target), UBX_PROTO_MASK | NMEA_PROTO_MASK | RTCM3_PROTO_MASK,
UBX_PROTO_MASK, 0x0, 0x0);
// On bebop, enable NMEA
break;
// USB Interface
case GPS_PORT_USB:
Expand Down Expand Up @@ -538,8 +548,10 @@ static bool gps_ubx_ucenter_configure(uint8_t nr)
#endif
break;
case 12:
#if ! USE_GPS_UBX_RXM_RAW
// Disable UTM on old Lea4P
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0);
#endif
break;
case 13:
// SBAS Configuration
Expand All @@ -561,11 +573,29 @@ static bool gps_ubx_ucenter_configure(uint8_t nr)
gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_SFRB_ID, 1);
#endif
break;
#if USE_GPS_UBX_RTCM
case 17:
DEBUG_PRINT("CFG_DGNSS\n");
UbxSend_CFG_DGNSS(gps_ubx_ucenter.dev, 0x03, RESERVED, RESERVED);
break;
case 18:
DEBUG_PRINT("Enable RELPOSNED\n");
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_RELPOSNED_ID, 1);
break;
case 19:
DEBUG_PRINT("Enable HPPPOSLLH\n");
gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_HPPOSLLH_ID, 1);
break;
case 20:
DEBUG_PRINT("Enable RXM_RTCM\n");
gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_RTCM_ID, 1);
break;
#endif
case 21:
// Try to save on non-ROM devices...
UbxSend_CFG_CFG(gps_ubx_ucenter.dev, 0x00000000, 0xffffffff, 0x00000000);
break;
case 18:
case 22:
#if DEBUG_GPS_UBX_UCENTER
// Debug Downlink the result of all configuration steps: see messages
// To view, enable DEBUG message in your telemetry configuration .xml
Expand All @@ -582,3 +612,9 @@ static bool gps_ubx_ucenter_configure(uint8_t nr)
gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE;
return true; // Continue, except for the last case
}

int gps_ubx_ucenter_get_status(void)
{
return gps_ubx_ucenter.status;
}

4 changes: 3 additions & 1 deletion sw/airborne/modules/gps/gps_ubx_ucenter.h
View file
Expand Up @@ -32,7 +32,7 @@
#include "pprzlink/pprzlink_device.h"

/** U-Center Variables */
#define GPS_UBX_UCENTER_CONFIG_STEPS 19
#define GPS_UBX_UCENTER_CONFIG_STEPS 23

struct gps_ubx_ucenter_struct {
uint8_t status;
Expand Down Expand Up @@ -64,6 +64,8 @@ extern void gps_ubx_ucenter_init(void);
extern void gps_ubx_ucenter_periodic(void);
extern void gps_ubx_ucenter_event(void);

extern int gps_ubx_ucenter_get_status(void);

#endif


11 changes: 11 additions & 0 deletions sw/airborne/subsystems/datalink/datalink.c
View file
Expand Up @@ -127,6 +127,17 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *
);
}
break;
#if USE_GPS_UBX_RTCM
case DL_RTCM_INJECT : {
// GPS parse data
gps_inject_data(
DL_RTCM_INJECT_packet_id(buf),
DL_RTCM_INJECT_data_length(buf),
DL_RTCM_INJECT_data(buf)
);
}
break;
#endif // USE_GPS_UBX_RTCM
#endif // USE_GPS

default:
Expand Down
39 changes: 39 additions & 0 deletions sw/airborne/subsystems/gps.c
View file
Expand Up @@ -75,6 +75,8 @@ 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 = 0;
Expand Down Expand Up @@ -140,6 +142,31 @@ static void send_gps(struct transport_tx *trans, struct link_device *dev)
send_svinfo_available(trans, dev);
}

static void send_gps_rtk(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GPS_RTK(trans, dev, AC_ID,
&gps_relposned.iTOW,
&gps_relposned.refStationId,
&gps_relposned.relPosN, &gps_relposned.relPosE, &gps_relposned.relPosD,
&gps_relposned.relPosHPN, &gps_relposned.relPosHPE, &gps_relposned.relPosHPD,
&gps_relposned.accN, &gps_relposned.accE, &gps_relposned.accD,
&gps_relposned.carrSoln,
&gps_relposned.relPosValid,
&gps_relposned.diffSoln,
&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)
{
pprz_msg_send_GPS_INT(trans, dev, AC_ID,
Expand Down Expand Up @@ -282,6 +309,18 @@ void gps_init(void)
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GPS_LLA, send_gps_lla);
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
#if USE_GPS_UBX_RTCM
// 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;
#endif
}

Expand Down

0 comments on commit a00eace

Please sign in to comment.