forked from Archive/PX4-Autopilot
mission require valid landing after DO_LAND_START
This commit is contained in:
parent
56b028148b
commit
ed1b442065
|
@ -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
|
||||
uint8 geofence_action # action to take when geofence is violated
|
||||
|
||||
bool home_required # true if the geofence requires a valid home position
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -38,54 +38,30 @@
|
|||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
#include "geofence.h"
|
||||
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <string.h>
|
||||
#include <dataman/dataman.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <ctype.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_defines.h>
|
||||
#include <unistd.h>
|
||||
#include <geo/geo.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "navigator.h"
|
||||
|
||||
#include <ctype.h>
|
||||
|
||||
#include <dataman/dataman.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <geo/geo.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
|
@ -41,15 +41,16 @@
|
|||
#ifndef GEOFENCE_H_
|
||||
#define GEOFENCE_H_
|
||||
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <cfloat>
|
||||
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_defines.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
#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_ */
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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 <drivers/drv_pwm_output.h>
|
||||
#include <fw_pos_control_l1/landingslope.h>
|
||||
#include <geo/geo.h>
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <systemlib/mavlink_log.h>
|
||||
#include <fw_pos_control_l1/landingslope.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/fw_pos_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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 <unistd.h>
|
||||
#include <dataman/dataman.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fw_pos_ctrl_status.h>
|
||||
#include <dataman/dataman.h>
|
||||
#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_ */
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue