diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-27 00:27:11 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-27 00:27:11 +0400 |
commit | 83da4ae02dfc61d6a7f80ae40660826fbbca81be (patch) | |
tree | 22a932c0da0ea382bad423a6c47551cd2db7bef9 /src/modules/navigator/navigator_main.cpp | |
parent | e2305d93bd52fb86fde24fb331552483bb25dd7b (diff) | |
download | px4-firmware-83da4ae02dfc61d6a7f80ae40660826fbbca81be.tar.gz px4-firmware-83da4ae02dfc61d6a7f80ae40660826fbbca81be.tar.bz2 px4-firmware-83da4ae02dfc61d6a7f80ae40660826fbbca81be.zip |
'vehicle_global_position' topic updated: removed baro_alt and XXX_valid flags.
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 17 |
1 files changed, 11 insertions, 6 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..ef7201790 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -177,7 +177,7 @@ private: class Mission _mission; bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _global_pos_valid; /**< track changes of global_position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -817,13 +817,11 @@ Navigator::task_main() if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + if (myState == NAV_STATE_LAND && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } - _global_pos_valid = _global_pos.global_valid; - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { @@ -846,8 +844,15 @@ Navigator::task_main() /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } + + _global_pos_valid = true; + + } else { + /* assume that global position is valid if updated in last 20ms */ + _global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000; } + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; @@ -893,9 +898,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); + warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - if (_global_pos.global_valid) { + if (_global_pos_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); |