From 2b060a000018a568f822f36314bd0b72b353ef3a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 26 Oct 2021 17:41:19 -0400 Subject: [PATCH] gps: ublox sensor_gps_heading message --- msg/CMakeLists.txt | 1 + msg/sensor_gps_heading.msg | 10 ++++++++++ src/drivers/gps/definitions.h | 1 + src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 18 ++++++++++++++++-- src/modules/logger/logged_topics.cpp | 1 + 6 files changed, 30 insertions(+), 3 deletions(-) create mode 100644 msg/sensor_gps_heading.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3bee66456a..3bb5ea3fea 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/sensor_gps_heading.msg b/msg/sensor_gps_heading.msg new file mode 100644 index 0000000000..e11daaaaaa --- /dev/null +++ b/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 diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index fee9bfb6f2..18841e3896 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 6fcf068949..8594714049 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 6fcf06894973240d45dc49d3b31565917dc8f2f6 +Subproject commit 8594714049927ca0f2e1cd246cee58d7a316c072 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4be1a57176..9826e89f2f 100644 --- a/src/drivers/gps/gps.cpp +++ b/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 #include #include +#include #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 _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position + uORB::PublicationMulti _sensor_gps_heading_pub{ORB_ID(sensor_gps_heading)}; uORB::PublicationMulti _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; + } } } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 6378916c9a..8c51443708 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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);