From ae4654f36a05357c2d60036115bcaa043f7800d4 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Mon, 17 Sep 2018 16:09:54 +0200 Subject: [PATCH] Mavlink: Include new UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE flag --- src/modules/mavlink/mavlink_messages.cpp | 49 ++++++++++++------------ 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index f5ec96944b..7e4a1c588a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1719,12 +1719,13 @@ public: } private: - MavlinkOrbSubscription *_global_pos_sub; - uint64_t _global_pos_time = 0; - MavlinkOrbSubscription *_local_pos_sub; uint64_t _local_pos_time = 0; + MavlinkOrbSubscription *_global_pos_sub; + uint64_t _global_pos_time = 0; + vehicle_global_position_s _global_position = {}; + MavlinkOrbSubscription *_position_setpoint_triplet_sub; uint64_t _setpoint_triplet_time = 0; position_setpoint_triplet_s _setpoint_triplet = {}; @@ -1743,8 +1744,8 @@ private: protected: explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink), - _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _position_setpoint_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _vehicle_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _land_detected_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))) @@ -1752,13 +1753,13 @@ protected: bool send(const hrt_abstime t) { - vehicle_global_position_s global_position = {}; vehicle_local_position_s local_position = {}; - bool global_updated = _global_pos_sub->update(&_global_pos_time, &global_position); bool local_updated = _local_pos_sub->update(&_local_pos_time, &local_position); - // Check if new setpoint, vehicle status and land detected are available otherwise use the last received + // Check if new global position, setpoint, vehicle status and land detected are available + // otherwise use the last received + _global_pos_sub->update(&_global_pos_time, &_global_position); _position_setpoint_triplet_sub->update(&_setpoint_triplet_time, &_setpoint_triplet); _vehicle_status_sub->update(&_vehicle_status_time, &_vehicle_status); _land_detected_sub->update(&_land_detected_time, &_land_detected); @@ -1780,22 +1781,22 @@ protected: memset(&msg.uas_id[0], 0, sizeof(msg.uas_id)); //msg.flags |= UTM_DATA_AVAIL_FLAGS_UAS_ID_AVAILABLE; - // Handle global position update - if (global_updated) { - msg.lat = global_position.lat * 1e7; - msg.lon = global_position.lon * 1e7; - msg.alt = global_position.alt_ellipsoid * 1000.0f; - - msg.h_acc = global_position.eph * 1000.0f; - msg.v_acc = global_position.epv * 1000.0f; - - msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; - msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; - } - // Handle local position update if (local_updated) { + if (_global_pos_time > 0) { + // Handle global position update + msg.lat = _global_position.lat * 1e7; + msg.lon = _global_position.lon * 1e7; + msg.alt = _global_position.alt_ellipsoid * 1000.0f; + + msg.h_acc = _global_position.eph * 1000.0f; + msg.v_acc = _global_position.epv * 1000.0f; + + msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; + msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + } + if (local_position.v_xy_valid) { msg.vx = local_position.vx * 100.0f; msg.vy = local_position.vy * 100.0f; @@ -1809,17 +1810,17 @@ protected: } if (local_position.dist_bottom_valid) { - msg.relative_alt = local_position.dist_bottom * 1000.0f; // TODO Check if terrain height is correct field - msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; + msg.relative_alt = local_position.dist_bottom * 1000.0f; + msg.flags |= UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE; } // Handle next waypoint if it is valid - if (_setpoint_triplet_time > 0 && _setpoint_triplet.next.valid) { + if (_setpoint_triplet_time > 0 && _setpoint_triplet.current.valid) { msg.next_lat = _setpoint_triplet.current.lat * 1e7; msg.next_lon = _setpoint_triplet.current.lon * 1e7; // HACK We assume that the offset between AMSL and WGS84 is constant between the current // vehicle position and the the target waypoint. - msg.next_alt = (_setpoint_triplet.current.alt + (global_position.alt_ellipsoid - global_position.alt)) * 1000.0f; + msg.next_alt = (_setpoint_triplet.current.alt + (_global_position.alt_ellipsoid - _global_position.alt)) * 1000.0f; msg.flags |= UTM_DATA_AVAIL_FLAGS_NEXT_WAYPOINT_AVAILABLE; }