aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp29
-rw-r--r--src/modules/fw_pos_control_l1/landingslope.cpp12
-rw-r--r--src/modules/fw_pos_control_l1/landingslope.h34
-rw-r--r--src/modules/mavlink/waypoints.c2
-rw-r--r--src/modules/navigator/mission_feasibility_checker.cpp184
-rw-r--r--src/modules/navigator/mission_feasibility_checker.h82
-rw-r--r--src/modules/navigator/module.mk3
-rw-r--r--src/modules/navigator/navigator_main.cpp26
-rw-r--r--src/modules/uORB/topics/navigation_capabilities.h5
9 files changed, 350 insertions, 27 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 2be25517d..3784337ac 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -312,6 +312,11 @@ private:
void vehicle_setpoint_poll();
/**
+ * Publish navigation capabilities
+ */
+ void navigation_capabilities_publish();
+
+ /**
* Control position.
*/
bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
@@ -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 &current_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 &current_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();
+
}
}
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
index a2d8525b9..b139a6397 100644
--- a/src/modules/fw_pos_control_l1/landingslope.cpp
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -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;
+
}
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
index b77252e6e..8ff431509 100644
--- a/src/modules/fw_pos_control_l1/landingslope.h
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -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,
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 45e891434..2ac05039f 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -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;
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
new file mode 100644
index 000000000..25b2636bb
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -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;
+ }
+}
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
new file mode 100644
index 000000000..7d1cc2f8a
--- /dev/null
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -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_ */
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index fc59c956a..6be4e87a0 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -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
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 48f828ff7..c7ac885b4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -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);
}
@@ -1311,4 +1325,4 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item)
new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
new_mission_item->radius = 50.0f; // TODO: get rid of magic number
}
-} \ No newline at end of file
+}
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
index 6a3e811e3..391756f99 100644
--- a/src/modules/uORB/topics/navigation_capabilities.h
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -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;
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-01 04:50:22 +0000