diff options
Diffstat (limited to 'src/modules/navigator')
| -rw-r--r-- | src/modules/navigator/datalinkloss.cpp | 227 | ||||
| -rw-r--r-- | src/modules/navigator/datalinkloss.h | 98 | ||||
| -rw-r--r-- | src/modules/navigator/datalinkloss_params.c | 126 | ||||
| -rw-r--r-- | src/modules/navigator/enginefailure.cpp | 149 | ||||
| -rw-r--r-- | src/modules/navigator/enginefailure.h | 83 | ||||
| -rw-r--r-- | src/modules/navigator/gpsfailure.cpp | 178 | ||||
| -rw-r--r-- | src/modules/navigator/gpsfailure.h | 97 | ||||
| -rw-r--r-- | src/modules/navigator/gpsfailure_params.c | 97 | ||||
| -rw-r--r-- | src/modules/navigator/rcloss.cpp | 182 | ||||
| -rw-r--r-- | src/modules/navigator/rcloss.h | 88 | ||||
| -rw-r--r-- | src/modules/navigator/rcloss_params.c | 60 |
11 files changed, 1385 insertions, 0 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 000000000..66f1c8c73 --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * 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 datalinkloss.cpp + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "NAV_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_AH_LON", false), + _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), + _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), + _dll_state(DLL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +DataLinkLoss::~DataLinkLoss() +{ +} + +void +DataLinkLoss::on_inactive() +{ + /* reset DLL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _dll_state = DLL_STATE_NONE; + } +} + +void +DataLinkLoss::on_activation() +{ + _dll_state = DLL_STATE_NONE; + updateParams(); + advance_dll(); + set_dll_item(); +} + +void +DataLinkLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_dll(); + set_dll_item(); + } +} + +void +DataLinkLoss::set_dll_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 (_dll_state) { + case DLL_STATE_FLYTOCOMMSHOLDWP: { + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_commsholdalt.get(); + _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_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.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 DLL_STATE_FLYTOAIRFIELDHOMEWP: { + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _param_airfieldhomealt.get(); + _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.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + 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 +DataLinkLoss::advance_dll() +{ + switch (_dll_state) { + case DLL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } + } + break; + case DLL_STATE_FLYTOCOMMSHOLDWP: + warnx("fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + warnx("time is up, state should have been changed manually by now"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 000000000..31e0e3907 --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -0,0 +1,98 @@ +/*************************************************************************** + * + * 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 datalinkloss.h + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef NAVIGATOR_DATALINKLOSS_H +#define NAVIGATOR_DATALINKLOSS_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/Subscription.hpp> + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class DataLinkLoss : public MissionBlock +{ +public: + DataLinkLoss(Navigator *navigator, const char *name); + + ~DataLinkLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; + control::BlockParamInt _param_numberdatalinklosses; + control::BlockParamInt _param_skipcommshold; + + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 + } _dll_state; + + /** + * Set the DLL item + */ + void set_dll_item(); + + /** + * Move to next DLL item + */ + void advance_dll(); + +}; +#endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 000000000..6780c0c88 --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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 datalinkloss_params.c + * + * Parameters for DLL + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * Data Link Loss parameters, accessible via MAVLink + */ + +/** + * Comms hold wait time + * + * The amount of time in seconds the system should wait at the comms hold waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); + +/** + * Comms hold Lat + * + * Latitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); + +/** + * Comms hold Lon + * + * Longitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); + +/** + * Comms hold alt + * + * Altitude of comms hold waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); + +/** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + +/** + * Number of allowed Datalink timeouts + * + * After more than this number of data link timeouts the aircraft returns home directly + * + * @group DLL + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(NAV_DLL_N, 2); + +/** + * Skip comms hold wp + * + * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to + * airfield home + * + * @group DLL + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 000000000..e007b208b --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * 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 enginefailure.cpp + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> + +#include "navigator.h" +#include "enginefailure.h" + +#define DELAY_SIGMA 0.01f + +EngineFailure::EngineFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _ef_state(EF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +EngineFailure::~EngineFailure() +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_NONE; + advance_ef(); + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + 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 +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); + _ef_state = EF_STATE_LOITERDOWN; + break; + default: + break; + } +} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h new file mode 100644 index 000000000..2c48c2fce --- /dev/null +++ b/src/modules/navigator/enginefailure.h @@ -0,0 +1,83 @@ +/*************************************************************************** + * + * 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 enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/Subscription.hpp> + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator, const char *name); + + ~EngineFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; +#endif diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 000000000..cd55f60b0 --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> + +#include "navigator.h" +#include "gpsfailure.h" + +#define DELAY_SIGMA 0.01f + +GpsFailure::GpsFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _param_openlooploiter_roll(this, "R"), + _param_openlooploiter_pitch(this, "P"), + _param_openlooploiter_thrust(this, "TR"), + _gpsf_state(GPSF_STATE_NONE), + _timestamp_activation(0) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +GpsFailure::~GpsFailure() +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); + updateParams(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); + + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + break; + } + case GPSF_STATE_TERMINATE: + set_gpsf_item(); + advance_gpsf(); + break; + default: + break; + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + + switch (_gpsf_state) { + case GPSF_STATE_TERMINATE: { + /* Request flight termination from the commander */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("gps fail: request flight termination"); + } + default: + break; + } + + reset_mission_item_reached(); + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + updateParams(); + + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + warnx("gpsf loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); + break; + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + warnx("gpsf terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + warnx("mavlink sent"); + break; + case GPSF_STATE_TERMINATE: + warnx("gpsf end"); + _gpsf_state = GPSF_STATE_END; + default: + break; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 000000000..e9e72babf --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -0,0 +1,97 @@ +/*************************************************************************** + * + * 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 gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#ifndef NAVIGATOR_GPSFAILURE_H +#define NAVIGATOR_GPSFAILURE_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/uORB.h> +#include <uORB/Publication.hpp> +#include <uORB/topics/vehicle_attitude_setpoint.h> + +#include <drivers/drv_hrt.h> + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class GpsFailure : public MissionBlock +{ +public: + GpsFailure(Navigator *navigator, const char *name); + + ~GpsFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + control::BlockParamFloat _param_openlooploiter_roll; + control::BlockParamFloat _param_openlooploiter_pitch; + control::BlockParamFloat _param_openlooploiter_thrust; + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, + } _gpsf_state; + + hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */ + + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; +#endif diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 000000000..39d179eed --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * GPS Failure Navigation Mode parameters, accessible via MAVLink + */ + +/** + * Loiter time + * + * The amount of time in seconds the system should do open loop loiter and wait for gps recovery + * before it goes into flight termination. + * + * @unit seconds + * @min 0.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); + +/** + * Open loop loiter roll + * + * Roll in degrees during the open loop loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Open loop loiter pitch + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Open loop loiter thrust + * + * Thrust value which is set during the open loop loiter + * + * @min 0.0 + * @max 1.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); + + diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 000000000..5564a1c42 --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * 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 <thomasgubler@gmail.com> + */ + +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> + +#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 = _navigator->get_global_position()->alt; + _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 */ + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); + warnx("rc not recovered: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } + 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: + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } + 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 000000000..bcb74d877 --- /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 <thomasgubler@gmail.com> + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/Subscription.hpp> + +#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 000000000..f1a01c73b --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * 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 + * Set to -1 to make the system skip loitering + * + * @unit seconds + * @min -1.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); |