dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mission require valid landing after DO_LAND_START

This commit is contained in:
Daniel Agar 2017-04-16 21:16:47 -04:00
parent 56b028148b
commit ed1b442065
14 changed files with 343 additions and 350 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/geofence_result.msg
View File

@ -6,3 +6,5 @@ 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
bool home_required # true if the geofence requires a valid home position

6
msg/mission_result.msg
View File

@ -1,14 +1,18 @@
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

5
src/modules/commander/commander.cpp
View File

@ -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);

2
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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;

86
src/modules/navigator/geofence.cpp
View File

@ -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,21 +104,23 @@ 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()) {
const float max_horizontal_distance = _param_max_hor_distance.get();
const float max_vertical_distance = _param_max_ver_distance.get();
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;
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);
if (max_vertical_distance > 1.0f && (dist_z > max_vertical_distance)) {
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",
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();
}
@ -155,10 +128,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
inside_fence = false;
}
if (max_horizontal_distance > 1.0f && (dist_xy > max_horizontal_distance)) {
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",
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();
}
@ -166,7 +138,6 @@ bool Geofence::inside(double lat, double lon, float altitude)
inside_fence = false;
}
}
}
// to be inside the geofence both fences have to report being inside
// as they both report being inside when not enabled
@ -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;
}

44
src/modules/navigator/geofence.h
View File

@ -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_ */

21
src/modules/navigator/mission.cpp
View File

@ -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();

33
src/modules/navigator/mission_block.cpp
View File

@ -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) {
return false;
}
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;
}
void

1
src/modules/navigator/mission_block.h
View File

@ -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:

380
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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.");
/* 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;
}
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);
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 (wp_distance > _fw_pos_ctrl_status.landing_flare_length) {
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);
if (wp_distance > fw_pos_ctrl_status->landing_flare_length) {
/* Last wp is before flare region */
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;
const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
} else {
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(_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),
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 last waypoint */
mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare");
return false;
}
landing_valid = true;
} else {
// 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;
}
}
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 */
else if (isPositionCommand(mission_item.nav_cmd)) {
/* 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;
}
}

55
src/modules/navigator/mission_feasibility_checker.h
View File

@ -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_ */

2
src/modules/navigator/mission_params.c
View File

@ -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

2
src/modules/navigator/navigator.h
View File

@ -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);

14
src/modules/navigator/navigator_main.cpp
View File

@ -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);
}