From 3511f8abfb77d0178740b6dac99807a21cb1babf Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 9 Nov 2016 19:38:38 -0500 Subject: [PATCH] Add position ground truth. (#5819) Have confirmed this works with gazebo sitl. --- msg/vehicle_global_position.msg | 2 + msg/vehicle_local_position.msg | 4 +- src/modules/logger/logger.cpp | 2 + src/modules/simulator/simulator.h | 19 ++++++ src/modules/simulator/simulator_mavlink.cpp | 69 +++++++++++++++++++++ 5 files changed, 95 insertions(+), 1 deletion(-) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index 39b815015a..685ba2b7c2 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -18,3 +18,5 @@ float32 terrain_alt # Terrain altitude WGS84, (metres) bool terrain_alt_valid # Terrain altitude estimate is valid bool dead_reckoning # True if this position is estimated through dead-reckoning float32 pressure_alt # Pressure altitude AMSL, (metres) + +# TOPICS vehicle_global_position vehicle_global_position_groundtruth diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg index bf107056e2..31efaeb778 100644 --- a/msg/vehicle_local_position.msg +++ b/msg/vehicle_local_position.msg @@ -32,4 +32,6 @@ float32 dist_bottom_rate # Rate of change of distance from bottom surface to gr uint64 surface_bottom_timestamp # Time when new bottom surface found since system start, (microseconds) bool dist_bottom_valid # true if distance to bottom surface is valid float32 eph # Standard deviation of horizontal position error, (metres) -float32 epv # Standard deviation of vertical position error, (metres) \ No newline at end of file +float32 epv # Standard deviation of vertical position error, (metres) + +# TOPICS vehicle_local_position vehicle_local_position_groundtruth diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index abc042c681..7209078b22 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -510,6 +510,8 @@ void Logger::add_default_topics() { #ifdef CONFIG_ARCH_BOARD_SITL add_topic("vehicle_attitude_groundtruth", 10); + add_topic("vehicle_global_position_groundtruth", 100); + add_topic("vehicle_local_position_groundtruth", 100); #endif // Note: try to avoid setting the interval where possible, as it increases RAM usage diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 0e755f4b66..1c4c3ec7d9 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -58,6 +59,7 @@ #include #include #include +#include namespace simulator { @@ -247,10 +249,17 @@ private: , _rc_channels_pub(nullptr), _attitude_pub(nullptr), + _gpos_pub(nullptr), _actuator_outputs_sub{}, _vehicle_attitude_sub(-1), _manual_sub(-1), _vehicle_status_sub(-1), + _hil_local_proj_ref(), + _hil_local_proj_inited(false), + _hil_ref_lat(0), + _hil_ref_lon(0), + _hil_ref_alt(0), + _hil_ref_timestamp(0), _rc_input{}, _actuators{}, _attitude{}, @@ -317,6 +326,8 @@ private: // uORB publisher handlers orb_advert_t _rc_channels_pub; orb_advert_t _attitude_pub; + orb_advert_t _gpos_pub; + orb_advert_t _lpos_pub; // uORB subscription handlers int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES]; @@ -324,6 +335,14 @@ private: int _manual_sub; int _vehicle_status_sub; + // hil map_ref data + struct map_projection_reference_s _hil_local_proj_ref; + bool _hil_local_proj_inited; + double _hil_ref_lat; + double _hil_ref_lon; + float _hil_ref_alt; + uint64_t _hil_ref_timestamp; + // uORB data containers struct rc_input_values _rc_input; struct actuator_outputs_s _actuators[ORB_MULTI_MAX_INSTANCES]; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index b8556ca0c6..91361d593f 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -397,6 +397,75 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) int hilstate_multi; orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH); } + + /* global position */ + struct vehicle_global_position_s hil_gpos = {}; + { + hil_gpos.timestamp = timestamp; + + hil_gpos.time_utc_usec = timestamp; + hil_gpos.lat = hil_state.lat; + hil_gpos.lon = hil_state.lon; + hil_gpos.alt = hil_state.alt; + + hil_gpos.vel_n = hil_state.vx; + hil_gpos.vel_e = hil_state.vy; + hil_gpos.vel_d = hil_state.vz; + + // always publish ground truth attitude message + int hil_gpos_multi; + orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi, + ORB_PRIO_HIGH); + } + + /* local position */ + struct vehicle_local_position_s hil_lpos = {}; + { + hil_lpos.timestamp = timestamp; + + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + + if (!_hil_local_proj_inited) { + _hil_local_proj_inited = true; + map_projection_init(&_hil_local_proj_ref, lat, lon); + _hil_ref_timestamp = timestamp; + _hil_ref_lat = lat; + _hil_ref_lon = lon; + _hil_ref_alt = hil_state.alt / 1000.0f;; + } + + float x; + float y; + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); + hil_lpos.timestamp = timestamp; + hil_lpos.xy_valid = true; + hil_lpos.z_valid = true; + hil_lpos.v_xy_valid = true; + hil_lpos.v_z_valid = true; + hil_lpos.x = x; + hil_lpos.y = y; + hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f; + hil_lpos.vx = hil_state.vx / 100.0f; + hil_lpos.vy = hil_state.vy / 100.0f; + hil_lpos.vz = hil_state.vz / 100.0f; + matrix::Eulerf euler = matrix::Quatf(hil_attitude.q); + hil_lpos.yaw = euler.psi(); + hil_lpos.xy_global = true; + hil_lpos.z_global = true; + hil_lpos.ref_lat = _hil_ref_lat; + hil_lpos.ref_lon = _hil_ref_lon; + hil_lpos.ref_alt = _hil_ref_alt; + hil_lpos.ref_timestamp = _hil_ref_timestamp; + + // always publish ground truth attitude message + int hil_lpos_multi; + orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi, + ORB_PRIO_HIGH); + } + + + break; }