From 5db1490a03a820f44845bb68861cbae5920da81f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 20:07:56 +0200 Subject: [PATCH 1/2] EKF: Only publish with low EPH / EPV if the position estimate is actually valid / in place. --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4588f3c901..078d06e092 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -932,9 +932,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() const float dtLastGoodGPS = static_cast(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f; - if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + if (!_local_pos.xy_global || + !_local_pos.v_xy_valid || + _gps.timestamp_position == 0 || + (dtLastGoodGPS >= POS_RESET_THRESHOLD)) { + _global_pos.eph = EPH_LARGE_VALUE; _global_pos.epv = EPV_LARGE_VALUE; + } else { _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; From ad1dff434d661dad7100e5eb7d2d7810d78a7c35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 26 Aug 2015 20:14:40 +0200 Subject: [PATCH 2/2] Navigator: Fix out of index warnings when empty offboard mission was read --- src/modules/navigator/mission.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b9e6e69439..350b05e39f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -624,26 +624,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s { /* select onboard/offboard mission */ int *mission_index_ptr; - struct mission_s *mission; dm_item_t dm_item; - int mission_index_next; + + struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission; + int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index; + + /* do not work on empty missions */ + if (mission->count == 0) { + return false; + } + + /* move to next item if there is one */ + if (mission_index_next < ((int)mission->count - 1)) { + mission_index_next++; + } if (onboard) { /* onboard mission */ - mission_index_next = _current_onboard_mission_index + 1; mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next; - - mission = &_onboard_mission; - dm_item = DM_KEY_WAYPOINTS_ONBOARD; } else { /* offboard mission */ - mission_index_next = _current_offboard_mission_index + 1; mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next; - - mission = &_offboard_mission; - dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); } @@ -653,7 +656,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) { /* mission item index out of bounds */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count); return false; } @@ -665,7 +668,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_fd(), + mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR waypoint could not be read"); return false; }