From fd3746a233b0ef16758e0171da0ee7e71ff58887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:06:14 +0200 Subject: [PATCH] add OBC RC loss mode to navigator --- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 13 +- src/modules/navigator/rcloss.cpp | 172 +++++++++++++++++++++++ src/modules/navigator/rcloss.h | 88 ++++++++++++ src/modules/navigator/rcloss_params.c | 59 ++++++++ 7 files changed, 339 insertions(+), 7 deletions(-) create mode 100644 src/modules/navigator/rcloss.cpp create mode 100644 src/modules/navigator/rcloss.h create mode 100644 src/modules/navigator/rcloss_params.c diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 02e766ffba..f0bbbcf1cb 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("request flight termination"); + warnx("gps fail: request flight termination"); } default: break; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 9e4ad053c9..c075903b75 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,8 +51,10 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ - enginefailure.cpp \ datalinkloss_params.c \ + rcloss.cpp \ + rcloss_params.c \ + enginefailure.cpp \ gpsfailure.cpp \ gpsfailure_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ec6e538e34..709e3e7242 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -63,12 +63,13 @@ #include "datalinkloss.h" #include "enginefailure.h" #include "gpsfailure.h" +#include "rcloss.h" #include "geofence.h" /** * 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 { @@ -109,7 +110,7 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); - + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -193,6 +194,8 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + RCLoss _rcLoss; /**< class that handles RTL according to + OBC rules (rc loss mode) */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode @@ -207,6 +210,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ + control::BlockParamInt _param_rcloss_obc; /**< if true: obc mode on rc loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0913bb575..c569ee7b54 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -131,6 +131,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), + _rcLoss(this, "RCL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), @@ -139,7 +140,8 @@ Navigator::Navigator() : _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), - _param_datalinkloss_obc(this, "DLL_OBC") + _param_datalinkloss_obc(this, "DLL_OBC"), + _param_rcloss_obc(this, "RCL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -149,6 +151,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; _navigation_mode_array[6] = &_gpsFailure; + _navigation_mode_array[7] = &_rcLoss; updateParams(); } @@ -413,7 +416,11 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + if (_param_rcloss_obc.get() != 0) { + _navigation_mode = &_rcLoss; + } else { + _navigation_mode = &_rtl; + } break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param @@ -421,7 +428,7 @@ Navigator::task_main() if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { - _navigation_mode = &_rtl; /* TODO: change this to something else */ + _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 0000000000..b8b659efe0 --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * 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 rcloss.cpp + * Helper class for RC Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +RCLoss::RCLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _rcl_state(RCL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RCLoss::~RCLoss() +{ +} + +void +RCLoss::on_inactive() +{ + /* reset RCL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rcl_state = RCL_STATE_NONE; + } +} + +void +RCLoss::on_activation() +{ + _rcl_state = RCL_STATE_NONE; + updateParams(); + advance_rcl(); + set_rcl_item(); +} + +void +RCLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_rcl(); + set_rcl_item(); + } +} + +void +RCLoss::set_rcl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rcl_state) { + case RCL_STATE_LOITER: { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case RCL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("gps fail: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +RCLoss::advance_rcl() +{ + switch (_rcl_state) { + case RCL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + _rcl_state = RCL_STATE_LOITER; + break; + case RCL_STATE_LOITER: + _rcl_state = RCL_STATE_TERMINATE; + warnx("time is up, no RC regain, terminating"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 0000000000..bcb74d8778 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -0,0 +1,88 @@ +/*************************************************************************** + * + * 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 rcloss.h + * Helper class for RC Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RCLoss : public MissionBlock +{ +public: + RCLoss(Navigator *navigator, const char *name); + + ~RCLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + + enum RCLState { + RCL_STATE_NONE = 0, + RCL_STATE_LOITER = 1, + RCL_STATE_TERMINATE = 2, + RCL_STATE_END = 3 + } _rcl_state; + + /** + * Set the RCL item + */ + void set_rcl_item(); + + /** + * Move to next RCL item + */ + void advance_rcl(); + +}; +#endif diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 0000000000..83d23cf499 --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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 rcloss_params.c + * + * Parameters for RC Loss (OBC) + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * OBC RC Loss mode parameters, accessible via MAVLink + */ + +/** + * Loiter Time + * + * The amount of time in seconds the system should loiter at current position before termination + * + * @unit seconds + * @min 0.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);