diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 165d92b989..0daba4a4ac 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -5,4 +5,6 @@ uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL uint8 GF_ACTION_TERMINATE = 4 # flight termination bool geofence_violated # true if the geofence is violated -uint8 geofence_action # action to take when geofence is violated \ No newline at end of file +uint8 geofence_action # action to take when geofence is violated + +bool home_required # true if the geofence requires a valid home position diff --git a/msg/mission_result.msg b/msg/mission_result.msg index 8eaf4d2773..5f6d5bcb14 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -1,14 +1,18 @@ -uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified +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 uint32 seq_total # Total number of mission items + 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 failure # true if the mission cannot continue or be completed for some reason + 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 -bool mission_failure # true if the mission cannot continue or be completed for some reason diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b11b62f751..aca3e2d104 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2477,8 +2477,8 @@ int commander_thread_main(int argc, char *argv[]) status_flags.condition_auto_mission_available = _mission_result.valid && !_mission_result.finished; - if (status.mission_failure != _mission_result.mission_failure) { - status.mission_failure = _mission_result.mission_failure; + if (status.mission_failure != _mission_result.failure) { + status.mission_failure = _mission_result.failure; status_changed = true; if (status.mission_failure) { @@ -2625,6 +2625,7 @@ int commander_thread_main(int argc, char *argv[]) if (status_flags.condition_home_position_valid && (hrt_elapsed_time(&_home.timestamp) > 2000000) && _last_mission_instance != _mission_result.instance_count) { + if (!_mission_result.valid) { /* the mission is invalid */ tune_mission_fail(true); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fa16129cb0..1013d77c3f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1525,8 +1525,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons // continuously reset launch detection and runway takeoff until armed if (!_control_mode.flag_armed) { - _runway_takeoff.reset(); - _launchDetector.reset(); _launch_detection_state = LAUNCHDETECTION_RES_NONE; _launch_detection_notify = 0; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 664eae0352..931ab8173a 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -38,54 +38,30 @@ * @author Thomas Gubler */ #include "geofence.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include "navigator.h" +#include + +#include +#include +#include +#include + #define GEOFENCE_RANGE_WARNING_LIMIT 5000000 Geofence::Geofence(Navigator *navigator) : SuperBlock(navigator, "GF"), _navigator(navigator), - _fence_pub(nullptr), - _home_pos{}, - _home_pos_set(false), - _last_horizontal_range_warning(0), - _last_vertical_range_warning(0), - _altitude_min(0), - _altitude_max(0), - _vertices_count(0), _param_action(this, "GF_ACTION", false), _param_altitude_mode(this, "GF_ALTMODE", false), _param_source(this, "GF_SOURCE", false), _param_counter_threshold(this, "GF_COUNT", false), _param_max_hor_distance(this, "GF_MAX_HOR_DIST", false), - _param_max_ver_distance(this, "GF_MAX_VER_DIST", false), - _outside_counter(0) + _param_max_ver_distance(this, "GF_MAX_VER_DIST", false) { - /* Load initial params */ updateParams(); } -Geofence::~Geofence() -{ - -} - - bool Geofence::inside(const struct vehicle_global_position_s &global_position) { return inside(global_position.lat, global_position.lon, global_position.alt); @@ -96,14 +72,9 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } - bool Geofence::inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, - const struct home_position_s home_pos, bool home_position_set) + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl) { - _home_pos = home_pos; - _home_pos_set = home_position_set; - if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); @@ -133,38 +104,38 @@ bool Geofence::inside(double lat, double lon, float altitude) { bool inside_fence = true; - float max_horizontal_distance = _param_max_hor_distance.get(); - float max_vertical_distance = _param_max_ver_distance.get(); + if (isHomeRequired() && _navigator->home_position_valid()) { - if (max_horizontal_distance > 1.0f || max_vertical_distance > 1.0f) { - if (_home_pos_set) { - float dist_xy = -1.0f; - float dist_z = -1.0f; - get_distance_to_point_global_wgs84(lat, lon, altitude, - _home_pos.lat, _home_pos.lon, _home_pos.alt, - &dist_xy, &dist_z); + const float max_horizontal_distance = _param_max_hor_distance.get(); + const float max_vertical_distance = _param_max_ver_distance.get(); - if (max_vertical_distance > 1.0f && (dist_z > max_vertical_distance)) { - if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Maximum altitude above home exceeded by %.1f m", - (double)(dist_z - max_vertical_distance)); - _last_vertical_range_warning = hrt_absolute_time(); - } + const double home_lat = _navigator->get_home_position()->lat; + const double home_lon = _navigator->get_home_position()->lon; + const double home_alt = _navigator->get_home_position()->alt; - inside_fence = false; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + get_distance_to_point_global_wgs84(lat, lon, altitude, home_lat, home_lon, home_alt, &dist_xy, &dist_z); + + if (max_vertical_distance > FLT_EPSILON && (dist_z > max_vertical_distance)) { + if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum altitude above home exceeded by %.1f m", + (double)(dist_z - max_vertical_distance)); + _last_vertical_range_warning = hrt_absolute_time(); } - if (max_horizontal_distance > 1.0f && (dist_xy > max_horizontal_distance)) { - if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Maximum distance from home exceeded by %.1f m", - (double)(dist_xy - max_horizontal_distance)); - _last_horizontal_range_warning = hrt_absolute_time(); - } + inside_fence = false; + } - inside_fence = false; + if (max_horizontal_distance > FLT_EPSILON && (dist_xy > max_horizontal_distance)) { + if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Maximum distance from home exceeded by %.1f m", + (double)(dist_xy - max_horizontal_distance)); + _last_horizontal_range_warning = hrt_absolute_time(); } + + inside_fence = false; } } @@ -188,11 +159,9 @@ bool Geofence::inside(double lat, double lon, float altitude) } } - bool Geofence::inside_polygon(double lat, double lon, float altitude) { if (valid()) { - if (!isEmpty()) { /* Vertical check */ if (altitude > _altitude_max || altitude < _altitude_min) { @@ -426,3 +395,12 @@ int Geofence::clearDm() dm_clear(DM_KEY_FENCE_POINTS); return PX4_OK; } + +bool Geofence::isHomeRequired() +{ + bool max_horizontal_enabled = (_param_max_hor_distance.get() > FLT_EPSILON); + bool max_vertical_enabled = (_param_max_ver_distance.get() > FLT_EPSILON); + bool geofence_action_rtl = (getGeofenceAction() == geofence_result_s::GF_ACTION_RTL); + + return max_horizontal_enabled || max_vertical_enabled || geofence_action_rtl; +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index f7c295a50f..25ba9b2a8a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -41,15 +41,16 @@ #ifndef GEOFENCE_H_ #define GEOFENCE_H_ -#include -#include -#include -#include -#include -#include +#include + #include +#include #include #include +#include +#include +#include +#include #define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt" @@ -59,11 +60,9 @@ class Geofence : public control::SuperBlock { public: Geofence(Navigator *navigator); - Geofence(const Geofence &) = delete; Geofence &operator=(const Geofence &) = delete; - - ~Geofence(); + ~Geofence() = default; /* Altitude mode, corresponding to the param GF_ALTMODE */ enum { @@ -85,8 +84,7 @@ public: * @return true: system is inside fence, false: system is outside fence */ bool inside(const struct vehicle_global_position_s &global_position, - const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl, - const struct home_position_s home_pos, bool home_position_set); + const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl); bool inside(const struct mission_item_s &mission_item); @@ -108,26 +106,23 @@ public: bool isEmpty() {return _vertices_count == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } - int getSource() { return _param_source.get(); } - int getGeofenceAction() { return _param_action.get(); } + bool isHomeRequired(); + private: - Navigator *_navigator; + Navigator *_navigator{nullptr}; - orb_advert_t _fence_pub; /**< publish fence topic */ + orb_advert_t _fence_pub{nullptr}; /**< publish fence topic */ - home_position_s _home_pos; - bool _home_pos_set; + hrt_abstime _last_horizontal_range_warning{0}; + hrt_abstime _last_vertical_range_warning{0}; - hrt_abstime _last_horizontal_range_warning; - hrt_abstime _last_vertical_range_warning; + float _altitude_min{0.0f}; + float _altitude_max{0.0f}; - float _altitude_min; - float _altitude_max; - - unsigned _vertices_count; + unsigned _vertices_count{0}; /* Params */ control::BlockParamInt _param_action; @@ -137,12 +132,11 @@ private: control::BlockParamFloat _param_max_hor_distance; control::BlockParamFloat _param_max_ver_distance; - int _outside_counter; + int _outside_counter{0}; bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); }; - #endif /* GEOFENCE_H_ */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9a44771d4e..c6e31fad3a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,7 +67,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_yawmode(this, "MIS_YAWMODE", false), _param_force_vtol(this, "NAV_FORCE_VT", false), _param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false), - _missionFeasibilityChecker() + _missionFeasibilityChecker(navigator) { updateParams(); } @@ -304,7 +304,7 @@ Mission::update_onboard_mission() // XXX check validity here as well _navigator->get_mission_result()->valid = true; /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->mission_failure = false; + _navigator->get_mission_result()->failure = false; /* reset reached info as well */ _navigator->get_mission_result()->reached = false; @@ -355,7 +355,7 @@ Mission::update_offboard_mission() if (!failed) { /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->mission_failure = false; + _navigator->get_mission_result()->failure = false; /* reset reached info as well */ _navigator->get_mission_result()->reached = false; @@ -1374,21 +1374,8 @@ Mission::check_mission_valid(bool force) { if ((!_home_inited && _navigator->home_position_valid()) || force) { - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _navigator->get_mission_result()->valid = - _missionFeasibilityChecker.checkMissionFeasible( - _navigator->get_mavlink_log_pub(), - (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol), - dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _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_default_acceptance_radius(), - _navigator->get_land_detected()->landed); + _missionFeasibilityChecker.checkMissionFeasible(_offboard_mission, _param_dist_1wp.get(), false); _navigator->get_mission_result()->seq_total = _offboard_mission.count; _navigator->increment_mission_instance_count(); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8bed8a52a1..86d16b8206 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -474,29 +474,20 @@ MissionBlock::get_time_inside(const struct mission_item_s &item) bool MissionBlock::item_contains_position(const struct mission_item_s *item) { - // XXX: maybe extend that check onto item properties - if (item->nav_cmd == NAV_CMD_DO_JUMP || - item->nav_cmd == NAV_CMD_DO_CHANGE_SPEED || - item->nav_cmd == NAV_CMD_DO_SET_SERVO || - item->nav_cmd == NAV_CMD_DO_LAND_START || - item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || - item->nav_cmd == NAV_CMD_IMAGE_START_CAPTURE || - item->nav_cmd == NAV_CMD_IMAGE_STOP_CAPTURE || - item->nav_cmd == NAV_CMD_VIDEO_START_CAPTURE || - item->nav_cmd == NAV_CMD_VIDEO_STOP_CAPTURE || - item->nav_cmd == NAV_CMD_DO_MOUNT_CONFIGURE || - item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL || - item->nav_cmd == NAV_CMD_DO_SET_ROI || - item->nav_cmd == NAV_CMD_ROI || - item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST || - item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || - item->nav_cmd == NAV_CMD_DELAY || - item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (item->nav_cmd == NAV_CMD_WAYPOINT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED || + item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LAND || + item->nav_cmd == NAV_CMD_TAKEOFF || + item->nav_cmd == NAV_CMD_LOITER_TO_ALT || + item->nav_cmd == NAV_CMD_VTOL_TAKEOFF || + item->nav_cmd == NAV_CMD_VTOL_LAND) { + + return true; - return false; } - return true; + return false; } void diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 134b9fa443..56f18ac173 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -66,7 +66,6 @@ public: MissionBlock(const MissionBlock &) = delete; MissionBlock &operator=(const MissionBlock &) = delete; - /* TODO: move this to a helper class in navigator */ static bool item_contains_position(const struct mission_item_s *item); protected: diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index ca92078b01..fe0a6375f3 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2017 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 @@ -42,73 +42,79 @@ #include "mission_feasibility_checker.h" #include "mission_block.h" +#include "navigator.h" + +#include +#include #include -#include #include #include -#include -#include -#include -#include -#include #include +#include +#include +#include -MissionFeasibilityChecker::MissionFeasibilityChecker() : - _mavlink_log_pub(nullptr), - _fw_pos_ctrl_status_sub(-1), - _initDone(false), - _dist_1wp_ok(false) +bool +MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float max_waypoint_distance, + bool land_start_req) { - _fw_pos_ctrl_status = {}; -} + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id); + const size_t nMissionItems = mission.count; + + const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol); + + Geofence &geofence = _navigator->get_geofence(); + fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status(); + const float home_alt = _navigator->get_home_position()->alt; + const bool home_valid = _navigator->home_position_valid(); -bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, 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, - float default_acceptance_rad, - bool condition_landed) -{ + + bool &warning_issued = _navigator->get_mission_result()->warning; + const float default_acceptance_rad = _navigator->get_default_acceptance_radius(); + const bool landed = _navigator->get_land_detected()->landed; + bool failed = false; bool warned = false; - /* Init if not done yet */ - init(); - - _mavlink_log_pub = mavlink_log_pub; // first check if we have a valid position - if (!home_valid /* can later use global / local pos for finer granularity */) { + if (!home_valid) { failed = true; warned = true; - mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock."); } else { + const double lat = _navigator->get_home_position()->lat; + const double lon = _navigator->get_home_position()->lon; + failed = failed - || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + || !check_dist_1wp(dm_current, nMissionItems, lat, lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported - failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed); - failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed); + failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { failed = failed - || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); + || !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_acceptance_rad); } else { - failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed + || !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_valid, + default_acceptance_rad, land_start_req); } return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, - Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) +bool +MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, float default_acceptance_rad) { - /* 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++) { - struct mission_item_s missionitem; + struct mission_item_s missionitem = {}; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { @@ -120,9 +126,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air - float takeoff_alt = missionitem.altitude_is_relative - ? missionitem.altitude - : missionitem.altitude - home_alt; + float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt; + // check if we should use default acceptance radius float acceptance_radius = default_acceptance_rad; @@ -131,7 +136,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr } if (takeoff_alt - 1.0f < acceptance_radius) { - mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!"); return false; } } @@ -141,26 +146,33 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr return true; } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, - Geofence &geofence, float home_alt, bool home_valid) +bool +MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems, + fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_valid, + float default_acceptance_rad, bool land_start_req) { - /* Update fixed wing navigation capabilites */ - updateNavigationCapabilities(); - /* Perform checks and issue feedback to the user for all checks */ - bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_valid, land_start_req); + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, fw_pos_ctrl_status, land_start_req); /* Mission is only marked as feasible if all checks return true */ - return resLanding; + return (resTakeoff && resLanding); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt) +bool +MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, + bool home_valid) { + + if (geofence.isHomeRequired() && !home_valid) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position"); + return false; + } + /* Check if all mission items are inside the geofence (if we have a valid geofence) */ if (geofence.valid()) { for (size_t i = 0; i < nMissionItems; i++) { - struct mission_item_s missionitem; + struct mission_item_s missionitem = {}; const ssize_t len = sizeof(missionitem); if (dm_read(dm_current, i, &missionitem, len) != len) { @@ -168,15 +180,17 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } + if (missionitem.altitude_is_relative && !home_valid) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position"); + return false; + } + // Geofence function checks against home altitude amsl - missionitem.altitude = missionitem.altitude_is_relative - ? missionitem.altitude + home_alt - : missionitem.altitude; + missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude; - if (MissionBlock::item_contains_position(&missionitem) && - !geofence.inside(missionitem)) { + if (MissionBlock::item_contains_position(&missionitem) && !geofence.inside(missionitem)) { - mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1); return false; } } @@ -185,12 +199,13 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, +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 waypoints are above the home altitude, only return false if bool throw_error = true */ + /* Check if all waypoints are above the home altitude */ for (size_t i = 0; i < nMissionItems; i++) { - struct mission_item_s missionitem; + struct mission_item_s missionitem = {}; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { @@ -200,16 +215,16 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } /* reject relative alt without home set */ - if (missionitem.altitude_is_relative && !home_valid && isPositionCommand(missionitem.nav_cmd)) { + if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(&missionitem)) { warning_issued = true; if (throw_error) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %d uses rel alt", i + 1); return false; } else { - mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: No home pos, WP %d uses rel alt", i + 1); return true; } } @@ -217,16 +232,16 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, /* calculate the global waypoint altitude */ float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; - if (home_alt > wp_alt && isPositionCommand(missionitem.nav_cmd)) { + if ((home_alt > wp_alt) && MissionBlock::item_contains_position(&missionitem)) { warning_issued = true; if (throw_error) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Waypoint %d below home", i + 1); return false; } else { - mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i + 1); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %d below home", i + 1); return true; } } @@ -235,8 +250,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } -bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, - bool condition_landed) +bool +MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool landed) { // do not allow mission if we find unsupported item for (size_t i = 0; i < nMissionItems; i++) { @@ -245,7 +260,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s if (dm_read(dm_current, i, &missionitem, len) != len) { // not supposed to happen unless the datamanager can't access the SD card, etc. - mavlink_log_critical(_mavlink_log_pub, "Rejecting Mission: Cannot access SD card"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card"); return false; } @@ -277,31 +292,93 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST && missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i + 1), + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1), (int)missionitem.nav_cmd); return false; } - // check if the mission starts with a land command while the vehicle is landed - if (missionitem.nav_cmd == NAV_CMD_LAND && - i == 0 && - condition_landed) { + /* Check non navigation item */ + if (missionitem.nav_cmd == NAV_CMD_DO_SET_SERVO) { - mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed."); - return false; + /* check actuator number */ + if (missionitem.params[0] < 0 || missionitem.params[0] > 5) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5", + (int)missionitem.params[0]); + return false; + } + + /* check actuator value */ + if (missionitem.params[1] < -PWM_DEFAULT_MAX || missionitem.params[1] > PWM_DEFAULT_MAX) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), + "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX", (int)missionitem.params[1]); + return false; + } } + // check if the mission starts with a land command while the vehicle is landed + if (missionitem.nav_cmd == NAV_CMD_LAND && i == 0 && landed) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing"); + return false; + } } return true; } -bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) +bool +MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, float default_acceptance_rad) +{ + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem = {}; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // look for a takeoff waypoint + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air + + float takeoff_alt = missionitem.altitude_is_relative + ? missionitem.altitude + : missionitem.altitude - home_alt; + + // check if we should use default acceptance radius + float acceptance_radius = default_acceptance_rad; + + if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = missionitem.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!"); + return false; + } + } + } + + // all checks have passed + return true; +} + +bool +MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, + fw_pos_ctrl_status_s *fw_pos_ctrl_status, bool land_start_req) { /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ + bool landing_valid = false; + + bool land_start_found = false; + size_t do_land_start_index = 0; + size_t landing_approach_index = 0; + for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; const ssize_t len = sizeof(missionitem); @@ -311,64 +388,92 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return false; } - if (missionitem.nav_cmd == NAV_CMD_LAND) { - struct mission_item_s missionitem_previous; + // if DO_LAND_START found then require valid landing AFTER + if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { + if (land_start_found) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start"); + return false; - if (i != 0) { - if (dm_read(dm_current, i - 1, &missionitem_previous, len) != len) { + } else { + land_start_found = true; + do_land_start_index = i; + } + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + mission_item_s missionitem_previous {}; + + if (i > 0) { + landing_approach_index = i - 1; + + if (dm_read(dm_current, landing_approach_index, &missionitem_previous, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } - float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat, - missionitem.lon); - float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, - _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); - float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, - _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad); - float delta_altitude = missionitem.altitude - missionitem_previous.altitude; -// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", -// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); -// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", -// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); + if (MissionBlock::item_contains_position(&missionitem_previous)) { + float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat, + missionitem.lon); - if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) { - /* Last wp is before flare region */ + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, + fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad); - if (delta_altitude < 0) { - if (missionitem_previous.altitude <= slope_alt_req) { - /* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */ - return true; + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, + fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad); + + if (wp_distance > fw_pos_ctrl_status->landing_flare_length) { + /* Last wp is before flare region */ + + const float delta_altitude = missionitem.altitude - missionitem_previous.altitude; + + if (delta_altitude < 0) { + if (missionitem_previous.altitude > slope_alt_req) { + /* Landing waypoint is above altitude of slope at the given waypoint distance */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %.1fm or move further away by %.1fm", + (double)(slope_alt_req - missionitem_previous.altitude), + (double)(wp_distance_req - wp_distance)); + + return false; + } } else { - /* Landing waypoint is above altitude of slope at the given waypoint distance */ - mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close"); - mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm", - (double)(slope_alt_req), - (double)(wp_distance_req - wp_distance)); + /* Landing waypoint is above last waypoint */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint"); return false; } } else { - /* Landing waypoint is above last waypoint */ - mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint"); + /* Last wp is in flare region */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare"); return false; } + landing_valid = true; + } else { - /* Last wp is in flare region */ - //xxx give recommendations - mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint"); + // mission item before land doesn't have a position + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach"); return false; } } else { - mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint"); return false; } } } + if (land_start_req && !land_start_found) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: land start required"); + return false; + } + + if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start"); + return false; + } + /* No landing waypoints or no waypoints */ return true; } @@ -377,46 +482,23 @@ 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) { - /* check if first waypoint is not too far from home */ if (dist_first_wp > 0.0f) { - struct mission_item_s mission_item; + 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 non navigation item */ - if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) { - - /* check actuator number */ - if (mission_item.params[0] < 0 || mission_item.params[0] > 5) { - mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]); - warning_issued = true; - return false; - } - - /* check actuator value */ - if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) { - mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]); - warning_issued = true; - return false; - } - } - - /* check only items with valid lat/lon */ - else if (isPositionCommand(mission_item.nav_cmd)) { + for (size_t i = 0; i < nMissionItems; i++) { + if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + if (MissionBlock::item_contains_position(&mission_item)) { + /* check only items with valid lat/lon */ /* 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); + 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_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); warning_issued = true; } @@ -424,7 +506,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } else { /* item is too far from home */ - mavlink_log_critical(_mavlink_log_pub, "First waypoint too far: %d m > %d, refusing mission", + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "First waypoint too far: %d m > %d, refusing mission", (int)dist_to_1wp, (int)dist_first_wp); warning_issued = true; return false; @@ -433,51 +515,15 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } else { /* error reading, mission is invalid */ - mavlink_log_info(_mavlink_log_pub, "error reading offboard mission"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "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; } } - -bool -MissionFeasibilityChecker::isPositionCommand(unsigned cmd) -{ - if (cmd == NAV_CMD_WAYPOINT || - cmd == NAV_CMD_LOITER_UNLIMITED || - cmd == NAV_CMD_LOITER_TIME_LIMIT || - cmd == NAV_CMD_LAND || - cmd == NAV_CMD_TAKEOFF || - cmd == NAV_CMD_LOITER_TO_ALT || - cmd == NAV_CMD_VTOL_TAKEOFF || - cmd == NAV_CMD_VTOL_LAND) { - - return true; - - } else { - return false; - - } -} - -void MissionFeasibilityChecker::updateNavigationCapabilities() -{ - (void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); -} - -void MissionFeasibilityChecker::init() -{ - if (!_initDone) { - - _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); - - _initDone = true; - } -} diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 71df42d5fa..a35df8f4b8 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2017 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 @@ -42,61 +42,54 @@ #ifndef MISSION_FEASIBILITY_CHECKER_H_ #define MISSION_FEASIBILITY_CHECKER_H_ -#include +#include #include #include -#include -#include "geofence.h" +class Geofence; +class Navigator; class MissionFeasibilityChecker { private: - orb_advert_t *_mavlink_log_pub; - - int _fw_pos_ctrl_status_sub; - struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; - - bool _initDone; - bool _dist_1wp_ok; - void init(); + Navigator *_navigator{nullptr}; /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); + 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 condition_landed); - 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); - bool isPositionCommand(unsigned cmd); + + 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); - bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); - void updateNavigationCapabilities(); + bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status, + float home_alt, bool home_valid, float default_acceptance_rad, bool land_start_req); + + bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, + float default_acceptance_rad); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status, + bool land_start_req); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, - bool home_valid, float default_acceptance_rad); -public: + bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, float default_acceptance_rad); - MissionFeasibilityChecker(); +public: + MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}; + ~MissionFeasibilityChecker() = default; MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete; MissionFeasibilityChecker &operator=(const MissionFeasibilityChecker &) = delete; - ~MissionFeasibilityChecker() {} - /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(orb_advert_t *mavlink_log_pub, 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, float default_acceptance_rad, - bool condition_landed); + bool checkMissionFeasible(const mission_s &mission, float max_waypoint_distance, bool land_start_req); }; - #endif /* MISSION_FEASIBILITY_CHECKER_H_ */ diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 046293e223..660360130c 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1); * * Failsafe check to prevent running mission stored from previous flight at a new takeoff location. * Set a value of zero or less to disable. The mission will not be started if the current - * waypoint is more distant than MIS_DIS_1WP from the current position. + * waypoint is more distant than MIS_DIS_1WP from the home position. * * @unit m * @min 0 diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 3f619c6879..15f53df2ea 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -304,7 +304,7 @@ private: float _mission_throttle{-1.0f}; // update subscriptions - void fw_pos_ctrl_status_update(); + void fw_pos_ctrl_status_update(bool force = false); void global_position_update(); void gps_position_update(); void home_position_update(bool force = false); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a10121686b..f9de1c947e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -180,12 +180,12 @@ Navigator::home_position_update(bool force) } void -Navigator::fw_pos_ctrl_status_update() +Navigator::fw_pos_ctrl_status_update(bool force) { bool updated = false; orb_check(_fw_pos_ctrl_status_sub, &updated); - if (updated) { + if (updated || force) { orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status); } } @@ -265,7 +265,7 @@ Navigator::task_main() gps_position_update(); sensor_combined_update(); home_position_update(true); - fw_pos_ctrl_status_update(); + fw_pos_ctrl_status_update(true); params_update(); /* wakeup source(s) */ @@ -531,13 +531,13 @@ Navigator::task_main() (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { - bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, - home_position_valid()); + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.timestamp = hrt_absolute_time(); _geofence_result.geofence_action = _geofence.getGeofenceAction(); + _geofence_result.home_required = _geofence.isHomeRequired(); if (!inside) { /* inform other apps via the mission result */ @@ -969,8 +969,8 @@ Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd) void Navigator::set_mission_failure(const char *reason) { - if (!_mission_result.mission_failure) { - _mission_result.mission_failure = true; + if (!_mission_result.failure) { + _mission_result.failure = true; set_mission_result_updated(); mavlink_log_critical(&_mavlink_log_pub, "%s", reason); }