From 43c2970fb92df360a4c7a14f7118ab2ba8ff87cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 16 Jul 2018 09:54:19 +0200 Subject: [PATCH] vehicle_gps_position: add heading field --- msg/vehicle_gps_position.msg | 2 ++ src/drivers/gps/gps.cpp | 4 ++++ src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/simulator/gpssim/gpssim.cpp | 1 + src/modules/uavcan/sensors/gnss.cpp | 2 ++ 5 files changed, 11 insertions(+) diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index c57a078033..48ff70ebee 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -29,3 +29,5 @@ int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of th 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 uint8 satellites_used # Number of satellites used + +float32 heading # heading in NED. Set to NaN if not set (used for dual antenna GPS), (rad, [-PI, PI]) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5da86301e9..c6fe470c47 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -281,6 +281,8 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac /* enforce null termination */ _port[sizeof(_port) - 1] = '\0'; + _report_gps_pos.heading = NAN; + /* create satellite info data object if requested */ if (enable_sat_info) { _sat_info = new GPS_Sat_Info(); @@ -650,6 +652,7 @@ GPS::run() _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; _report_gps_pos.satellites_used = 10; + _report_gps_pos.heading = NAN; /* no time and satellite information simulated */ @@ -700,6 +703,7 @@ GPS::run() /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.heading = NAN; if (_mode == GPS_DRIVER_MODE_UBX) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c96cb8317b..4aa1d5b1cb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2139,6 +2139,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used? + hil_gps.heading = NAN; + if (_gps_pub == nullptr) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); diff --git a/src/modules/simulator/gpssim/gpssim.cpp b/src/modules/simulator/gpssim/gpssim.cpp index 013e95e418..735496886c 100644 --- a/src/modules/simulator/gpssim/gpssim.cpp +++ b/src/modules/simulator/gpssim/gpssim.cpp @@ -178,6 +178,7 @@ GPSSIM::GPSSIM(const char *uart_path, bool fake_gps, bool enable_sat_info, { /* we need this potentially before it could be set in task_main */ g_dev = this; + _report_gps_pos.heading = NAN; /* create satellite info data object if requested */ if (enable_sat_info) { diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index b48ed25e67..299caa2248 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -379,6 +379,8 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.hdop = msg.pdop; report.vdop = msg.pdop; + report.heading = NAN; + // Publish to a multi-topic int32_t gps_orb_instance; orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,