From e96fd83565cf712e1085f99b99c1d83cfeccd6b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 20 Nov 2015 11:14:51 +0100 Subject: [PATCH] Navigator: Support takeoff command --- src/modules/navigator/CMakeLists.txt | 1 + src/modules/navigator/navigator.h | 5 +- src/modules/navigator/navigator_main.cpp | 21 ++++++ src/modules/navigator/takeoff.cpp | 93 ++++++++++++++++++++++++ src/modules/navigator/takeoff.h | 67 +++++++++++++++++ 5 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/takeoff.cpp create mode 100644 src/modules/navigator/takeoff.h diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index d8399ccafd..6a5c4c15d9 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( mission.cpp loiter.cpp rtl.cpp + takeoff.cpp mission_feasibility_checker.cpp geofence.cpp datalinkloss.cpp diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 6ecf73a606..6facdd9081 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -61,6 +61,7 @@ #include "navigator_mode.h" #include "mission.h" #include "loiter.h" +#include "takeoff.h" #include "rtl.h" #include "datalinkloss.h" #include "enginefailure.h" @@ -71,7 +72,7 @@ /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 7 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public control::SuperBlock { @@ -183,6 +184,7 @@ private: int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ int _param_update_sub; /**< param update subscription */ + int _vehicle_command_sub; /**< vehicle commands (onboard and offboard) */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; @@ -218,6 +220,7 @@ private: NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ + Takeoff _takeoff; /**< class for handling takeoff commands */ RTL _rtl; /**< class that handles RTL */ RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b056e3fd99..e65b434f4b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include @@ -111,6 +112,7 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _offboard_mission_sub(-1), _param_update_sub(-1), + _vehicle_command_sub(-1), _pos_sp_triplet_pub(nullptr), _mission_result_pub(nullptr), _geofence_result_pub(nullptr), @@ -135,6 +137,7 @@ Navigator::Navigator() : _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), + _takeoff(this, "TKF"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), @@ -157,6 +160,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_engineFailure; _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; + _navigation_mode_array[7] = &_takeoff; updateParams(); } @@ -286,6 +290,7 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); + _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); @@ -398,6 +403,18 @@ Navigator::task_main() } } + bool updated; + orb_check(_vehicle_command_sub, &updated); + + if (updated) { + vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { + warnx("navigator: got takeoff coordinates"); + } + } + /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && @@ -466,6 +483,10 @@ Navigator::task_main() _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: + _pos_sp_triplet_published_invalid_once = false; + _navigation_mode = &_takeoff; + break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp new file mode 100644 index 0000000000..76f58c11b9 --- /dev/null +++ b/src/modules/navigator/takeoff.cpp @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 + * 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 Takeoff.cpp + * + * Helper class to Takeoff + * + * @author Lorenz Meier +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "Takeoff.h" +#include "navigator.h" + +Takeoff::Takeoff(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) +{ + /* load initial params */ + updateParams(); +} + +Takeoff::~Takeoff() +{ +} + +void +Takeoff::on_inactive() +{ +} + +void +Takeoff::on_activation() +{ + /* set current mission item to Takeoff */ + set_loiter_item(&_mission_item, _param_min_alt.get()); + + /* convert mission item to current setpoint */ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + pos_sp_triplet->previous.valid = false; + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +Takeoff::on_active() +{ +} diff --git a/src/modules/navigator/takeoff.h b/src/modules/navigator/takeoff.h new file mode 100644 index 0000000000..2ac50b8748 --- /dev/null +++ b/src/modules/navigator/takeoff.h @@ -0,0 +1,67 @@ +/*************************************************************************** + * + * Copyright (c) 2014 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 + * 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 Takeoff.h + * + * Helper class to take off + * + * @author Lorenz Meier + */ + +#ifndef NAVIGATOR_TAKEOFF_H +#define NAVIGATOR_TAKEOFF_H + +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Takeoff : public MissionBlock +{ +public: + Takeoff(Navigator *navigator, const char *name); + + ~Takeoff(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; +}; + +#endif