forked from Archive/PX4-Autopilot
Navigator: add MissionFeasibilityChecker class; performs validation of landing waypoint set-up for fixed wing for now, but can be extended for other checks (e.g. check mission against geofence)
This commit is contained in:
Thomas Gubler
parent
d07cc95339
commit
b02b48290f
|
|
@ -311,6 +311,11 @@ private:
|
|||
*/
|
||||
void vehicle_setpoint_poll();
|
||||
|
||||
/**
|
||||
* Publish navigation capabilities
|
||||
*/
|
||||
void navigation_capabilities_publish();
|
||||
|
||||
/**
|
||||
* Control position.
|
||||
*/
|
||||
|
|
@ -538,6 +543,12 @@ FixedwingPositionControl::parameters_update()
|
|||
/* Update the landing slope */
|
||||
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
|
||||
|
||||
/* Update and publish the navigation capabilities */
|
||||
_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
|
||||
_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
|
||||
_nav_capabilities.landing_flare_length = landingslope.flare_length();
|
||||
navigation_capabilities_publish();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
@ -709,6 +720,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu
|
|||
}
|
||||
}
|
||||
|
||||
void FixedwingPositionControl::navigation_capabilities_publish()
|
||||
{
|
||||
if (_nav_capabilities_pub > 0) {
|
||||
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
|
||||
} else {
|
||||
_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed,
|
||||
const struct mission_item_triplet_s &mission_item_triplet)
|
||||
|
|
@ -875,7 +895,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
|||
|
||||
}
|
||||
|
||||
float flare_curve_alt = landingslope.getFlarceCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
|
||||
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
|
||||
|
||||
/* avoid climbout */
|
||||
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
|
||||
|
|
@ -1214,11 +1234,8 @@ FixedwingPositionControl::task_main()
|
|||
/* set new turn distance */
|
||||
_nav_capabilities.turn_distance = turn_distance;
|
||||
|
||||
if (_nav_capabilities_pub > 0) {
|
||||
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
|
||||
} else {
|
||||
_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
|
||||
}
|
||||
navigation_capabilities_publish();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,9 +1,7 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -37,7 +35,6 @@
|
|||
/*
|
||||
* @file: landingslope.cpp
|
||||
*
|
||||
* @author: Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include "landingslope.h"
|
||||
|
|
@ -74,11 +71,12 @@ void Landingslope::calculateSlopeValues()
|
|||
|
||||
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
|
||||
{
|
||||
return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
|
||||
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
|
||||
}
|
||||
|
||||
float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude)
|
||||
float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
|
||||
{
|
||||
return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
|
||||
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,9 +1,7 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -37,15 +35,18 @@
|
|||
/*
|
||||
* @file: landingslope.h
|
||||
*
|
||||
* @author: Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef LANDINGSLOPE_H_
|
||||
#define LANDINGSLOPE_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
class Landingslope
|
||||
{
|
||||
private:
|
||||
//xxx: documentation of landing pending
|
||||
float _landing_slope_angle_rad;
|
||||
float _flare_relative_alt;
|
||||
float _motor_lim_horizontal_distance;
|
||||
|
|
@ -62,8 +63,29 @@ public:
|
|||
Landingslope() {}
|
||||
~Landingslope() {}
|
||||
|
||||
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
|
||||
float getFlarceCurveAltitude(float wp_distance, float wp_altitude);
|
||||
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
|
||||
|
||||
/**
|
||||
*
|
||||
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
|
||||
*/
|
||||
__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
|
||||
{
|
||||
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
|
||||
*/
|
||||
__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
|
||||
{
|
||||
return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
|
||||
|
||||
}
|
||||
|
||||
|
||||
float getFlareCurveAltitude(float wp_distance, float wp_altitude);
|
||||
|
||||
void update(float landing_slope_angle_rad,
|
||||
float flare_relative_alt,
|
||||
|
|
|
|||
|
|
@ -62,7 +62,7 @@ orb_advert_t mission_pub = -1;
|
|||
struct mission_s mission;
|
||||
|
||||
//#define MAVLINK_WPM_NO_PRINTF
|
||||
#define MAVLINK_WPM_VERBOSE 1
|
||||
#define MAVLINK_WPM_VERBOSE 0
|
||||
|
||||
uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,184 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file mission_feasibility_checker.cpp
|
||||
* Provides checks if mission is feasible given the navigation capabilities
|
||||
*/
|
||||
|
||||
#include "mission_feasibility_checker.h"
|
||||
|
||||
#include <geo/geo.h>
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <fw_pos_control_l1/landingslope.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
|
||||
{
|
||||
_nav_caps = {0};
|
||||
}
|
||||
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
|
||||
{
|
||||
/* Init if not done yet */
|
||||
init();
|
||||
|
||||
/* Open mavlink fd */
|
||||
if (_mavlink_fd < 0) {
|
||||
/* try to open the mavlink log device every once in a while */
|
||||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
|
||||
|
||||
if (isRotarywing)
|
||||
return checkMissionFeasibleRotarywing(dm_current, nItems);
|
||||
else
|
||||
return checkMissionFeasibleFixedwing(dm_current, nItems);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
|
||||
{
|
||||
|
||||
return checkGeofence(dm_current, nItems);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
|
||||
{
|
||||
/* Update fixed wing navigation capabilites */
|
||||
updateNavigationCapabilities();
|
||||
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
||||
|
||||
return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems));
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
|
||||
{
|
||||
//xxx: check geofence
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
|
||||
{
|
||||
/* 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 */
|
||||
|
||||
|
||||
for (size_t i = 0; i < nItems; i++) {
|
||||
static 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;
|
||||
}
|
||||
|
||||
if (missionitem.nav_cmd == NAV_CMD_LAND) {
|
||||
struct mission_item_s missionitem_previous;
|
||||
if (i != 0) {
|
||||
if (dm_read(dm_current, i-1, &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, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
|
||||
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.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 > _nav_caps.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;
|
||||
} else {
|
||||
/* Landing waypoint is above altitude of slope at the given waypoint distance */
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close");
|
||||
mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
|
||||
(double)(slope_alt_req),
|
||||
(double)(wp_distance_req - wp_distance));
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
/* Landing waypoint is above last waypoint */
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
/* Last wp is in flare region */
|
||||
//xxx give recommendations
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region");
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
||||
{
|
||||
int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::init()
|
||||
{
|
||||
if (!_initDone) {
|
||||
|
||||
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
|
||||
|
||||
_initDone = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,82 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file mission_feasibility_checker.h
|
||||
* Provides checks if mission is feasible given the navigation capabilities
|
||||
*/
|
||||
#ifndef MISSION_FEASIBILITY_CHECKER_H_
|
||||
#define MISSION_FEASIBILITY_CHECKER_H_
|
||||
|
||||
#include <unistd.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <dataman/dataman.h>
|
||||
|
||||
|
||||
class MissionFeasibilityChecker
|
||||
{
|
||||
private:
|
||||
int _mavlink_fd;
|
||||
|
||||
int _capabilities_sub;
|
||||
struct navigation_capabilities_s _nav_caps;
|
||||
|
||||
bool _initDone;
|
||||
void init();
|
||||
|
||||
/* Checks for all airframes */
|
||||
bool checkGeofence(dm_item_t dm_current, size_t nItems);
|
||||
|
||||
/* Checks specific to fixedwing airframes */
|
||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems);
|
||||
bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems);
|
||||
void updateNavigationCapabilities();
|
||||
|
||||
/* Checks specific to rotarywing airframes */
|
||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems);
|
||||
public:
|
||||
|
||||
MissionFeasibilityChecker();
|
||||
~MissionFeasibilityChecker() {}
|
||||
|
||||
/*
|
||||
* Returns true if mission is feasible and false otherwise
|
||||
*/
|
||||
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif /* MISSION_FEASIBILITY_CHECKER_H_ */
|
||||
|
|
@ -39,6 +39,7 @@ MODULE_COMMAND = navigator
|
|||
|
||||
SRCS = navigator_main.cpp \
|
||||
navigator_params.c \
|
||||
navigator_mission.cpp
|
||||
navigator_mission.cpp \
|
||||
mission_feasibility_checker.cpp
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "navigator_mission.h"
|
||||
#include "mission_feasibility_checker.h"
|
||||
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
|
|
@ -165,6 +166,8 @@ private:
|
|||
bool _waypoint_yaw_reached;
|
||||
uint64_t _time_first_inside_orbit;
|
||||
|
||||
MissionFeasibilityChecker missionFeasiblityChecker;
|
||||
|
||||
struct {
|
||||
float min_altitude;
|
||||
float loiter_radius;
|
||||
|
|
@ -228,7 +231,7 @@ private:
|
|||
/**
|
||||
* Retrieve offboard mission.
|
||||
*/
|
||||
void offboard_mission_update();
|
||||
void offboard_mission_update(bool isrotaryWing);
|
||||
|
||||
/**
|
||||
* Retrieve onboard mission.
|
||||
|
|
@ -435,11 +438,21 @@ Navigator::navigation_capabilities_update()
|
|||
|
||||
|
||||
void
|
||||
Navigator::offboard_mission_update()
|
||||
Navigator::offboard_mission_update(bool isrotaryWing)
|
||||
{
|
||||
struct mission_s offboard_mission;
|
||||
if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
|
||||
|
||||
/* Check mission feasibility, for now do not handle the return value,
|
||||
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
|
||||
dm_item_t dm_current;
|
||||
if (offboard_mission.dataman_id == 0) {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
||||
} else {
|
||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||
}
|
||||
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count);
|
||||
|
||||
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
|
||||
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
|
||||
_mission.set_offboard_mission_count(offboard_mission.count);
|
||||
|
|
@ -503,13 +516,14 @@ Navigator::task_main()
|
|||
|
||||
|
||||
/* copy all topics first time */
|
||||
vehicle_status_update();
|
||||
parameters_update();
|
||||
global_position_update();
|
||||
home_position_update();
|
||||
navigation_capabilities_update();
|
||||
offboard_mission_update();
|
||||
offboard_mission_update(_vstatus.is_rotary_wing);
|
||||
onboard_mission_update();
|
||||
vehicle_status_update();
|
||||
|
||||
|
||||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vstatus_sub, 200);
|
||||
|
|
@ -632,7 +646,7 @@ Navigator::task_main()
|
|||
}
|
||||
|
||||
if (fds[4].revents & POLLIN) {
|
||||
offboard_mission_update();
|
||||
offboard_mission_update(_vstatus.is_rotary_wing);
|
||||
// XXX check if mission really changed
|
||||
dispatch(EVENT_MISSION_CHANGED);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -53,6 +53,11 @@
|
|||
*/
|
||||
struct navigation_capabilities_s {
|
||||
float turn_distance; /**< the optimal distance to a waypoint to switch to the next */
|
||||
|
||||
/* Landing parameters: see fw_pos_control_l1/landingslope.h */
|
||||
float landing_horizontal_slope_displacement;
|
||||
float landing_slope_angle_rad;
|
||||
float landing_flare_length;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
Loading…
Reference in New Issue