gps: ublox sensor_gps_heading message

msg/CMakeLists.txt

@@ -138,6 +138,7 @@ set(msg_files
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gps.msg
+	sensor_gps_heading.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg

msg/sensor_gps_heading.msg

@@ -0,0 +1,10 @@
+uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
+float32 heading
+float32 heading_accuracy                 # [1e-5 deg] Accuracy of the heading of the relative position vector
+
+float32[3] relative_position    # relative position vector
+float32[3] relative_position_accuracy

src/drivers/gps/definitions.h

@@ -43,6 +43,7 @@
 #include <px4_platform_common/defines.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gps_heading.h>
 
 #define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
 #define GPS_WARN(...) PX4_WARN(__VA_ARGS__)

src/drivers/gps/gps.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -63,6 +63,7 @@
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gps_heading.h>
 
 #ifndef CONSTRAINED_FLASH
 # include "devices/src/ashtech.h"
@@ -180,7 +181,11 @@ private:
 	sensor_gps_s			_report_gps_pos{};				///< uORB topic for gps position
 	satellite_info_s		*_p_report_sat_info{nullptr};			///< pointer to uORB topic for satellite info
 
+	sensor_gps_heading_s            _sensor_gps_heading{};
+	hrt_abstime                     _sensor_gps_heading_timestamp_last{0};
+
 	uORB::PublicationMulti<sensor_gps_s>	_report_gps_pos_pub{ORB_ID(sensor_gps)};	///< uORB pub for gps position
+	uORB::PublicationMulti<sensor_gps_heading_s> _sensor_gps_heading_pub{ORB_ID(sensor_gps_heading)};
 	uORB::PublicationMulti<satellite_info_s>	_report_sat_info_pub{ORB_ID(satellite_info)};		///< uORB pub for satellite info
 
 	float				_rate{0.0f};					///< position update rate
@@ -775,7 +780,7 @@ GPS::run()
 
 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
-			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
+			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, &_sensor_gps_heading, _p_report_sat_info,
 						   gps_ubx_dynmodel, heading_offset, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
@@ -1099,6 +1104,15 @@ GPS::publish()
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
 		_report_gps_pos.heading = NAN;
 		_is_gps_main_advertised.store(true);
+
+		if (_sensor_gps_heading.timestamp_sample > _sensor_gps_heading_timestamp_last) {
+
+			_sensor_gps_heading.device_id = get_device_id();
+			_sensor_gps_heading.timestamp = hrt_absolute_time();
+			_sensor_gps_heading_pub.publish(_sensor_gps_heading);
+
+			_sensor_gps_heading_timestamp_last = _sensor_gps_heading.timestamp_sample;
+		}
 	}
 }
 

src/modules/logger/logged_topics.cpp

@@ -154,6 +154,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("sensor_accel", 1000, 4);
 	add_topic_multi("sensor_baro", 1000, 4);
 	add_topic_multi("sensor_gps", 1000, 2);
+	add_topic_multi("sensor_gps_heading", 1000, 1);
 	add_topic_multi("sensor_gyro", 1000, 4);
 	add_topic_multi("sensor_mag", 1000, 4);
 	add_topic_multi("vehicle_imu", 500, 4);