diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 924fc7ed99..d74348b8f5 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -144,6 +144,7 @@ set(msg_files sensor_combined.msg sensor_correction.msg sensor_gps.msg + sensor_gnss_relative.msg sensor_gyro.msg sensor_gyro_fft.msg sensor_gyro_fifo.msg diff --git a/msg/sensor_gnss_relative.msg b/msg/sensor_gnss_relative.msg new file mode 100644 index 0000000000..0f1736f45f --- /dev/null +++ b/msg/sensor_gnss_relative.msg @@ -0,0 +1,30 @@ +# GNSS relative positioning information in NED frame. The NED frame is defined as the local topological system at the reference station. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint16 reference_station_id # Reference Station ID + +float32[3] position # GPS NED relative position vector (m) +float32[3] position_accuracy # Accuracy of relative position (m) + +float32 heading # Heading of the relative position vector (radians) +float32 heading_accuracy # Accuracy of heading of the relative position vector (radians) + +float32 position_length +float32 accuracy_length + +bool gnss_fix_ok # GNSS valid fix (i.e within DOP & accuracy masks) +bool differential_solution # differential corrections were applied +bool relative_position_valid +bool carrier_solution_floating # carrier phase range solution with floating ambiguities +bool carrier_solution_fixed # carrier phase range solution with fixed ambiguities +bool moving_base_mode # if the receiver is operating in moving base mode +bool reference_position_miss # extrapolated reference position was used to compute moving base solution this epoch +bool reference_observations_miss # extrapolated reference observations were used to compute moving base solution this epoch +bool heading_valid +bool relative_position_normalized # the components of the relative position vector (including the high-precision parts) are normalized diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index fee9bfb6f2..5aa4cbb177 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -43,6 +43,7 @@ #include #include #include +#include #define GPS_INFO(...) PX4_INFO(__VA_ARGS__) #define GPS_WARN(...) PX4_WARN(__VA_ARGS__) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index fa275c3993..ad1094aaf1 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit fa275c39935e00906d7a691770d2c10f1ea95d3c +Subproject commit ad1094aaf16fcc650b270431a1d0bdcf38e8d89a diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7f7fac775e..a4b3615657 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #ifndef CONSTRAINED_FLASH # include "devices/src/ashtech.h" @@ -181,6 +182,8 @@ private: satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position + uORB::PublicationMulti _sensor_gnss_relative_pub{ORB_ID(sensor_gnss_relative)}; + uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info float _rate{0.0f}; ///< position update rate @@ -220,6 +223,11 @@ private: */ void publishRTCMCorrections(uint8_t *data, size_t len); + /** + * Publish RTCM corrections + */ + void publishRelativePosition(sensor_gnss_relative_s &gnss_relative); + /** * This is an abstraction for the poll on serial used. * @@ -396,6 +404,13 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::RTCM, false); break; + case GPSCallbackType::gotRelativePositionMessage: + if (data1 && data2 == sizeof(sensor_gnss_relative_s)) { + gps->publishRelativePosition(*static_cast(data1)); + } + + break; + case GPSCallbackType::surveyInStatus: /* not used */ break; @@ -1184,6 +1199,14 @@ GPS::publishRTCMCorrections(uint8_t *data, size_t len) } } +void +GPS::publishRelativePosition(sensor_gnss_relative_s &gnss_relative) +{ + gnss_relative.device_id = get_device_id(); + gnss_relative.timestamp = hrt_absolute_time(); + _sensor_gnss_relative_pub.publish(gnss_relative); +} + int GPS::custom_command(int argc, char *argv[]) { diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 23f0126224..1fe3cfd6f7 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -176,6 +176,7 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("sensor_accel", 1000, 4); add_optional_topic_multi("sensor_baro", 1000, 4); add_topic_multi("sensor_gps", 1000, 2); + add_topic_multi("sensor_gnss_relative", 1000, 1); add_optional_topic("pps_capture", 1000); add_optional_topic_multi("sensor_gyro", 1000, 4); add_optional_topic_multi("sensor_mag", 1000, 4);