dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Mavlink: Include new UTM_DATA_AVAIL_FLAGS_RELATIVE_ALTITUDE_AVAILABLE flag

This commit is contained in:
Michael Schaeuble 2018-09-17 16:09:54 +02:00 committed by Lorenz Meier
parent 15439fcc0b
commit ae4654f36a
1 changed files with 25 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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;
}