From 2ba8ac44382d7e88cc22b73471733c2d01d9305e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 17:31:31 +0200 Subject: [PATCH 01/10] Move mission result to generated topics --- msg/mission_result.msg | 11 ++++ src/modules/uORB/topics/mission_result.h | 75 ------------------------ 2 files changed, 11 insertions(+), 75 deletions(-) create mode 100644 msg/mission_result.msg delete mode 100644 src/modules/uORB/topics/mission_result.h diff --git a/msg/mission_result.msg b/msg/mission_result.msg new file mode 100644 index 0000000000..532db6f73d --- /dev/null +++ b/msg/mission_result.msg @@ -0,0 +1,11 @@ +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified +uint32 seq_reached # Sequence of the mission item which has been reached +uint32 seq_current # Sequence of the current mission item +bool valid # true if mission is valid +bool reached # true if mission has been reached +bool finished # true if mission has been completed +bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode +bool flight_termination # true if the navigator demands a flight termination from the commander app +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint32 item_changed_index # indicate which item has changed +uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h deleted file mode 100644 index 16e7f2f126..0000000000 --- a/src/modules/uORB/topics/mission_result.h +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mission_result.h - * Mission results that navigator needs to pass on to commander and mavlink. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * @author Ban Siesta - */ - -#ifndef TOPIC_MISSION_RESULT_H -#define TOPIC_MISSION_RESULT_H - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct mission_result_s { - unsigned seq_reached; /**< Sequence of the mission item which has been reached */ - unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ - bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ - unsigned item_changed_index; /**< indicate which item has changed */ - unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(mission_result); - -#endif From 2cf10a5e999ce1e5c2579a202755346a6ad18046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 17:31:58 +0200 Subject: [PATCH 02/10] Navigator: Publish mission validity in mission result --- src/modules/navigator/mission.cpp | 9 +++++++++ src/modules/navigator/navigator.h | 3 +++ src/modules/navigator/navigator_main.cpp | 4 ++++ 3 files changed, 16 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a74e042a91..3848d16e5d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -197,6 +197,11 @@ Mission::update_onboard_mission() /* otherwise, just leave it */ } + // XXX check validity here as well + _navigator->get_mission_result()->valid = true; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { _onboard_mission.count = 0; _onboard_mission.current_seq = 0; @@ -234,6 +239,10 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _navigator->get_mission_result()->valid = !failed; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { warnx("offboard mission update failed"); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 782a297fbb..093e1be3c2 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -163,6 +163,8 @@ public: float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } + void increment_mission_instance_count() { _mission_instance_count++; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -205,6 +207,7 @@ private: bool _home_position_set; bool _mission_item_valid; /**< flags if the current mission item is valid */ + int _mission_instance_count; /**< instance count for the current mission */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8cfce50879..1460972cc2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,7 @@ Navigator::Navigator() : _att_sp{}, _home_position_set(false), _mission_item_valid(false), + _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), @@ -663,6 +664,8 @@ int navigator_main(int argc, char *argv[]) void Navigator::publish_mission_result() { + _mission_result.instance_count = _mission_instance_count; + /* lazily publish the mission result only once available */ if (_mission_result_pub > 0) { /* publish mission result */ @@ -679,6 +682,7 @@ Navigator::publish_mission_result() _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; + _mission_result.valid = true; } void From a4b238946070dfa2094b87d8fff61a987a6bec03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 18:37:19 +0200 Subject: [PATCH 03/10] Commander: Support new mission status --- src/modules/commander/commander.cpp | 26 ++++++++++++++++- src/modules/commander/commander_helper.cpp | 33 ++++++++++++++++++++++ src/modules/commander/commander_helper.h | 3 ++ 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73550e41e9..edeb31e888 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -190,6 +190,8 @@ static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; static struct home_position_s _home; +static unsigned _last_mission_instance = 0; + /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -839,7 +841,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & //Play tune first time we initialize HOME if (!status.condition_home_position_valid) { - tune_positive(true); + tune_home_set(true); } /* mark home position as set */ @@ -1764,6 +1766,28 @@ int commander_thread_main(int argc, char *argv[]) } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + /* Only evaluate mission state if home is set, + * this prevents false positives for the mission + * rejection. Back off 2 seconds to not overlay + * home tune. + */ + if (status.condition_home_position_valid && + (hrt_elapsed_time(&_home.timestamp) > 2000000) && + _last_mission_instance != mission_result.instance_count) { + if (mission_result.valid) { + /* the mission is valid */ + tune_mission_ok(true); + warnx("mission ok"); + } else { + /* the mission is not valid */ + tune_mission_fail(true); + warnx("mission fail"); + } + + /* prevent further feedback until the mission changes */ + _last_mission_instance = mission_result.instance_count; + } + /* RC input check */ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c0f8561fda..cbf11de1b0 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -172,6 +172,39 @@ void set_tune(int tune) } } +void tune_home_set(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + +void tune_mission_ok(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + +void tune_mission_fail(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + /** * Blink green LED and play positive tune (if use_buzzer == true). */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d2aace2a40..d2ab41f887 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -58,6 +58,9 @@ void buzzer_deinit(void); void set_tune_override(int tune); void set_tune(int tune); +void tune_home_set(bool use_buzzer); +void tune_mission_ok(bool use_buzzer); +void tune_mission_fail(bool use_buzzer); void tune_positive(bool use_buzzer); void tune_neutral(bool use_buzzer); void tune_negative(bool use_buzzer); From 174f4d27f3e65454808e073023ee14833f3d7ff2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 18:37:32 +0200 Subject: [PATCH 04/10] Navigator: output new mission status --- src/modules/navigator/mission.cpp | 18 ++++++++++++++++++ src/modules/navigator/mission.h | 1 + 2 files changed, 19 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3848d16e5d..be27e6208a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -78,6 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _takeoff(false), _mission_type(MISSION_TYPE_NONE), _inited(false), + _home_inited(false), _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), @@ -110,6 +111,22 @@ Mission::on_inactive() update_offboard_mission(); } + /* check if the home position became valid in the meantime */ + if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) && + !_home_inited && _navigator->home_position_valid()) { + + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + + _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt, _navigator->home_position_valid()); + + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + + _home_inited = true; + } + } else { /* read mission topics on initialization */ _inited = true; @@ -176,6 +193,7 @@ Mission::on_active() && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } + } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index bc9a2c6c82..6cfae49598 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -186,6 +186,7 @@ private: } _mission_type; bool _inited; + bool _home_inited; bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ From 21ca431131e5c50854a75d55655d4c87b268cea8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:12:17 +0200 Subject: [PATCH 05/10] Tone alarm: Add home set tune --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 31aee266e1..3506d832d0 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -152,6 +152,7 @@ enum { TONE_EKF_WARNING_TUNE, TONE_BARO_WARNING_TUNE, TONE_SINGLE_BEEP_TUNE, + TONE_HOME_SET, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a18b54981f..bf8418bf9f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning _default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning _default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep + _default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4"; _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -354,6 +355,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep + _tune_names[TONE_HOME_SET] = "home_set"; } ToneAlarm::~ToneAlarm() From b5a79bbc0b22a83c7a4b3cefaf7f1195f5b05f32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:12:35 +0200 Subject: [PATCH 06/10] commander: Use distinct tunes for home set and mission ok / failed --- src/modules/commander/commander_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index cbf11de1b0..362a707c03 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -179,7 +179,7 @@ void tune_home_set(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_HOME_SET); } } @@ -190,7 +190,7 @@ void tune_mission_ok(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } } @@ -201,7 +201,7 @@ void tune_mission_fail(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } } From eb3cc8b41ab587f7cd6240abb9dcba918888218c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:02:55 +0200 Subject: [PATCH 07/10] mission result topic: Add warnings --- msg/mission_result.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/mission_result.msg b/msg/mission_result.msg index 532db6f73d..ac4d32f559 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -2,6 +2,7 @@ uint32 instance_count # Instance count of this mission. Increments monotonically uint32 seq_reached # Sequence of the mission item which has been reached uint32 seq_current # Sequence of the current mission item bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings bool reached # true if mission has been reached bool finished # true if mission has been completed bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode From b11e13331882272c6b6bd5d200ec938070ade632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:12 +0200 Subject: [PATCH 08/10] Evaluate warning field from mission result --- src/modules/commander/commander.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edeb31e888..5ac8e9ca8b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1774,14 +1774,18 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_home_position_valid && (hrt_elapsed_time(&_home.timestamp) > 2000000) && _last_mission_instance != mission_result.instance_count) { - if (mission_result.valid) { + if (!mission_result.valid) { + /* the mission is invalid */ + tune_mission_fail(true); + warnx("mission fail"); + } else if (mission_result.warning) { + /* the mission has a warning */ + tune_mission_fail(true); + warnx("mission warning"); + } else { /* the mission is valid */ tune_mission_ok(true); warnx("mission ok"); - } else { - /* the mission is not valid */ - tune_mission_fail(true); - warnx("mission fail"); } /* prevent further feedback until the mission changes */ From 41f535ae262d7a6e5f4a2fe043b66d86dde9993e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:34 +0200 Subject: [PATCH 09/10] navigator: Include distance to first waypoint in mission check, provide warning feedback --- src/modules/navigator/mission.cpp | 82 ++----------- src/modules/navigator/mission.h | 1 - .../navigator/mission_feasibility_checker.cpp | 113 ++++++++++++++---- .../navigator/mission_feasibility_checker.h | 9 +- 4 files changed, 107 insertions(+), 98 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index be27e6208a..4944ebe789 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -79,7 +79,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), - _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), @@ -119,7 +118,9 @@ Mission::on_inactive() _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); @@ -255,7 +256,9 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -308,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) } } -bool -Mission::check_dist_1wp() -{ - if (_dist_1wp_ok) { - /* always return true after at least one successful check */ - return true; - } - - /* check if first waypoint is not too far from home */ - if (_param_dist_1wp.get() > 0.0f) { - if (_navigator->get_vstatus()->condition_home_position_valid) { - struct mission_item_s mission_item; - - /* find first waypoint (with lat/lon) item in datamanager */ - for (unsigned i = 0; i < _offboard_mission.count; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, - &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { - - /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || - mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - mission_item.nav_cmd == NAV_CMD_TAKEOFF || - mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - if (dist_to_1wp < _param_dist_1wp.get()) { - _dist_1wp_ok = true; - if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { - /* allow at 2/3 distance, but warn */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); - } - return true; - - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); - return false; - } - } - - } else { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); - return false; - } - } - - /* no waypoints found in mission, then we will not fly far away */ - _dist_1wp_ok = true; - return true; - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); - return false; - } - - } else { - return true; - } -} - void Mission::set_mission_items() { @@ -394,10 +330,8 @@ Mission::set_mission_items() _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } - /* get home distance state */ - bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = !home_dist_ok; + bool user_feedback_done = false; /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { @@ -409,7 +343,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6cfae49598..d77f461574 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -187,7 +187,6 @@ private: bool _inited; bool _home_inited; - bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 9d1dc7c7e6..05019bf8aa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -57,23 +57,40 @@ #endif static const int ERROR = -1; -MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +MissionFeasibilityChecker::MissionFeasibilityChecker() : + _mavlink_fd(-1), + _capabilities_sub(-1), + _initDone(false), + _dist_1wp_ok(false) { _nav_caps = {0}; } -bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, + dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) { bool failed = false; + bool warned = false; /* Init if not done yet */ init(); _mavlink_fd = mavlink_fd; + // first check if we have a valid position + if (!home_valid /* can later use global / local pos for finer granularity */) { + failed = true; + warned = true; + mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); + } else { + failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + } + // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - + failed |= !checkGeofence(dm_current, nMissionItems, geofence); + failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); @@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { - - /* Perform checks and issue feedback to the user for all checks */ - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); - - /* Mission is only marked as feasible if all checks return true */ - return (resGeofence && resHomeAltitude); + /* no custom rotary wing checks yet */ + return true; } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); -// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ - return (resLanding && resGeofence && resHomeAltitude); + return resLanding; } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -137,7 +146,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, bool &warning_issued, bool throw_error) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { + warning_issued = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - if (throw_error) { - return false; - } else { - return true; - } + return false; } /* always reject relative alt without home set */ if (missionitem.altitude_is_relative && !home_valid) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + warning_issued = true; return false; } @@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { + + warning_issued = true; + if (throw_error) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; @@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } +bool +MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued) +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + if (dist_first_wp > 0.0f) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (unsigned i = 0; i < nMissionItems; i++) { + if (dm_read(dm_current, i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, curr_lat, curr_lon); + + if (dist_to_1wp < dist_first_wp) { + _dist_1wp_ok = true; + if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + warning_issued = true; + } + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + warning_issued = true; + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_mavlink_fd, "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + return true; + } +} + void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9c9511be3d..4586f75a47 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,12 +57,14 @@ private: struct navigation_capabilities_s _nav_caps; bool _initDone; + bool _dist_1wp_ok; void init(); /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); - bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); @@ -79,8 +81,9 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid); + bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, + size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); }; From 677aef6673e96f7227db981a2e464898c86290ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 21:55:02 +0200 Subject: [PATCH 10/10] navigator: Fixed bitwise or --- .../navigator/mission_feasibility_checker.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 05019bf8aa..c57a12aefb 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,18 +84,18 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota warned = true; mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); } else { - failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported - failed |= !checkMissionItemValidity(dm_current, nMissionItems); - failed |= !checkGeofence(dm_current, nMissionItems, geofence); - failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); + failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); } else { - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } if (!failed) {