BlockByBlock / CorrectBase

Work in progress
Mozilla Public License 2.0
1 stars 1 forks source link

RTK capability: (base-station) receive RTCM (1005, 1077 and 1087) corrections #1

Closed AlexisTM closed 5 years ago

AlexisTM commented 6 years ago

Is it possible to use this library on the UBX M8P for RTK purpose (base-station)?

configure dynamic model get the RTCM3 corrections (1005, 1077 and 1087) If not, do you plan on supporting it, is this project open for contributions?

BlockByBlock commented 6 years ago

Hello Alexis, this base reader is for reading the m8p via serial. Configuration have to be done on the ublox GUI still. It does not support other functionalities, but serve as a simple serial reading tool for a fun project :)

AlexisTM commented 6 years ago

This is about reading as well; The configuration can be done from ublox Gui.

RTCM messages have to be forwarded one way or another to the rover to allow RTK localization.

BlockByBlock commented 6 years ago

I did a simple python serial read from the port. I highly recommend a more reliable way for sending correction data real time.

BlockByBlock commented 6 years ago

Nonetheless, u did get me interested to update this project. I have colleagues working on RTK correction. Perhaps I will work start work on this scratch build. Thanks for the inspiration!

AlexisTM commented 6 years ago

You are welcome ;) I agree that Python is not the "best in class" way to program it but it can become quite robust and it is specially fast to develop.

The RTK capability is already available on the PX4 Stack (UAV Firmware). They currently have a ground station (QGroundControl) reading/configurating the Ublox RTK device and sending corrections over TCP/UDP/Telemetry link (~350 bytes/s). Sadly, this makes it a complex setup when we are flying multiple copters. That's why I am currently trying to make the base_station node which is ROS integrated and could be started in a terminal and sent through a fleet of copters/UGVs.

The QGC related files: https://github.com/mavlink/qgroundcontrol/tree/master/src/GPS I currently struggle with the GPS_Drivers https://github.com/PX4/GpsDrivers/tree/d7854b7bcf1610bb42d89f4bae553744cbe4408c ;)

The client library to allow to receive the RTCM messages on a copter from ROS should be almost ready - http://github.com/mavlink/mavros/pull/1069

AlexisTM commented 5 years ago

It finally seems pretty simple, you can configure the messages via UBX-CFG-MSG, using:

#define UBX_CLASS_RTCM3 0xF5

/* UBX ID for RTCM3 output messages */
/* Minimal messages for RTK: 1005, 1077 + (1087 or 1127) */
/* Reduced message size using MSM4: 1005, 1074 + (1084 or 1124)  */
#define UBX_ID_RTCM3_1005   0x05    /**< Stationary RTK reference station ARP */
#define UBX_ID_RTCM3_1074   0x4A    /**< GPS MSM4 */
#define UBX_ID_RTCM3_1077   0x4D    /**< GPS MSM7 */
#define UBX_ID_RTCM3_1084   0x54    /**< GLONASS MSM4 */
#define UBX_ID_RTCM3_1087   0x57    /**< GLONASS MSM7 */
#define UBX_ID_RTCM3_1124   0x7C    /**< BeiDou MSM4 */
#define UBX_ID_RTCM3_1127   0x7F    /**< BeiDou MSM7 */
#define UBX_ID_RTCM3_1230   0xE6    /**< GLONASS code-phase biases */
#define UBX_ID_RTCM3_4072   0xFE    /**< Reference station PVT (u-blox proprietary RTCM Message) - Used for moving baseline */

#define UBX_MSG_RTCM3_1005  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1005 << 8)
#define UBX_MSG_RTCM3_1077  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1077 << 8)
#define UBX_MSG_RTCM3_1087  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1087 << 8)
#define UBX_MSG_RTCM3_1074  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1074 << 8)
#define UBX_MSG_RTCM3_1084  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1084 << 8)
#define UBX_MSG_RTCM3_1124  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1124 << 8)
#define UBX_MSG_RTCM3_1127  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1127 << 8)
#define UBX_MSG_RTCM3_1230  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_1230 << 8)
#define UBX_MSG_RTCM3_4072  ((UBX_CLASS_RTCM3) | UBX_ID_RTCM3_4072 << 8)

The important messages are: 1005 + 1077(GPS) + 1087(Glonass)/1127 (Beidou) + 4072 (for moving baseline)

I finally "just made it": https://github.com/AlexisTM/rtk_ros

BlockByBlock commented 5 years ago

Great work! Thanks for pulling out the RTCM3 messages!

BlockByBlock commented 5 years ago

Just a quick question, does the rtk_ros skips the ground control - QGroundControl/MissionPlanner? For configuration and reading

AlexisTM commented 5 years ago

Yes, this is a terminal application that runs on an embedded computer using ROS. The node is sending mavros_msgs/RTCM messages through ROS (WiFi or 4G with a VPN) to the copter, which are sent to the flight controller (which itself send it to the onboard GPS).

NOTE: It is using PX4/GpsDrivers to handle the GPS communications. Yet; as the serial library is different to QTSerial (to avoid that big dependency), the GpsDrivers library requires some modifications.

BlockByBlock commented 5 years ago

Great, setting up the M8P with RTKLIB is rather troublesome and it's huge at 127mb with functions that I do not need.

I am looking for something that could pull the ublox RTCM simply - seem like the ubx and gps_helper files in the GpsDrivers might do the trick. Will take reference from your GpsDrivers. Your ros package is amazing, I will adapt it for my fun project which is unfortunately not on mavros ros msg type and ROS as a middleware. Will credit your GpsDriver :+1:

P.S. I am not too sure about this, it seems that <rtk_ros/GpsDrivers/src/ashtech.h> is not required for rtk_ros.hpp, I can't find any GPSDriverAshtech object.

AlexisTM commented 5 years ago

GpsDrivers is not mine; It is part of the PX4 stack ;)

There are no occurrences indeed; I did not do a second pass of cleaning yet...

The rough architecture (imposed by GpsDrivers library) is as follows: The node creates a serial/SPI/anything link. The node instantiate a GpsHelper object giving a callback and few parameters. When the GpsHelper needs to access the device, it calls the callback with a type such as GpsCallbackType::readDeviceData or GpsCallbackType::writeDeviceData.

The main loop is the following:

https://github.com/AlexisTM/rtk_ros/blob/637cb2a647bab20d2ba1b2122329f33edfe253e7/rtk_ros/include/rtk_ros/rtk_node.hpp#L111-L130

As you can see there is a "max 3 fails/timeout".

If the following line is not there, the GPSDriverUBX::receive function times out continuously: The only change required in GpsDrivers: https://github.com/AlexisTM/GpsDrivers/blob/640b011c7996cd99234ab444bb16aa198c7a7919/src/ubx.cpp#L477

Timeout implementation that fires: https://github.com/AlexisTM/GpsDrivers/blob/640b011c7996cd99234ab444bb16aa198c7a7919/src/ubx.cpp#L482-L485

This is cause by the different behaviour of the read function:

Original behaviour: The function waits timeout time (provided by the callback) and return 0 if failed https://github.com/mavlink/qgroundcontrol/blob/16c7249a7de52dbc96ecb7df6ae830585759292d/src/GPS/GPSProvider.cc#L157-L164

This implementation: The function waits until data is received. https://github.com/AlexisTM/rtk_ros/blob/637cb2a647bab20d2ba1b2122329f33edfe253e7/rtk_ros/include/rtk_ros/rtk_node.hpp#L199-L208

With the original behavious, when it returns 0, the receive function will return because there is no more data to process. With the current implementation, you will always see data on the bus and tell the backend that data is available.

I might reimplement it to avoid this obscure architecture. (Example: RTCM data comes in a callback, GPS data comes depending on the returned data from the receive function)