mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 02:13:34 +08:00
delete Outback Challenge (OBC) AUTO_RTGS (datalink loss) and AUTO_RCRECOVER (rc loss)
This commit is contained in:
Daniel Agar
@@ -46,8 +46,6 @@ px4_add_module(
|
||||
precland.cpp
|
||||
mission_feasibility_checker.cpp
|
||||
geofence.cpp
|
||||
datalinkloss.cpp
|
||||
rcloss.cpp
|
||||
enginefailure.cpp
|
||||
gpsfailure.cpp
|
||||
follow_target.cpp
|
||||
|
||||
@@ -1,217 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <systemlib/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <navigator/navigation.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "datalinkloss.h"
|
||||
|
||||
DataLinkLoss::DataLinkLoss(Navigator *navigator) :
|
||||
MissionBlock(navigator),
|
||||
ModuleParams(navigator),
|
||||
_dll_state(DLL_STATE_NONE)
|
||||
{
|
||||
}
|
||||
|
||||
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;
|
||||
advance_dll();
|
||||
set_dll_item();
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
advance_dll();
|
||||
set_dll_item();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
DataLinkLoss::set_dll_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
pos_sp_triplet->previous = pos_sp_triplet->current;
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
|
||||
switch (_dll_state) {
|
||||
case DLL_STATE_FLYTOCOMMSHOLDWP: {
|
||||
_mission_item.lat = (double)(_param_nav_dll_ch_lat.get()) * 1.0e-7;
|
||||
_mission_item.lon = (double)(_param_nav_dll_ch_lon.get()) * 1.0e-7;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _param_nav_dll_ch_alt.get();
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = _param_nav_dll_ch_t.get() < 0.0f ? 0.0f : _param_nav_dll_ch_t.get();
|
||||
_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_nav_ah_lat.get()) * 1.0e-7;
|
||||
_mission_item.lon = (double)(_param_nav_ah_lon.get()) * 1.0e-7;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.altitude = _param_nav_ah_alt.get();
|
||||
_mission_item.yaw = NAN;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.time_inside = _param_nav_dll_ah_t.get() < 0.0f ? 0.0f : _param_nav_dll_ah_t.get();
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_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->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
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_nav_dll_n.get()) {
|
||||
warnx("%d data link losses, limit is %d, fly to airfield home",
|
||||
_navigator->get_vstatus()->data_link_lost_counter, _param_nav_dll_n.get());
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
|
||||
} else {
|
||||
if (!_param_nav_dll_chsk.get()) {
|
||||
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "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_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home, comms hold skipped");
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case DLL_STATE_FLYTOCOMMSHOLDWP:
|
||||
warnx("fly to airfield home");
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home");
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
break;
|
||||
|
||||
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
|
||||
_dll_state = DLL_STATE_TERMINATE;
|
||||
warnx("time is up, state should have been changed manually by now");
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no manual control, terminating");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
break;
|
||||
|
||||
case DLL_STATE_TERMINATE:
|
||||
warnx("dll end");
|
||||
_dll_state = DLL_STATE_END;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -1,92 +0,0 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* 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>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class DataLinkLoss : public MissionBlock, public ModuleParams
|
||||
{
|
||||
public:
|
||||
DataLinkLoss(Navigator *navigator);
|
||||
|
||||
~DataLinkLoss() = default;
|
||||
|
||||
void on_inactive() override;
|
||||
void on_activation() override;
|
||||
void on_active() override;
|
||||
|
||||
private:
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::NAV_DLL_CH_T>) _param_nav_dll_ch_t,
|
||||
(ParamInt<px4::params::NAV_DLL_CH_LAT>) _param_nav_dll_ch_lat, // * 1e7
|
||||
(ParamInt<px4::params::NAV_DLL_CH_LON>) _param_nav_dll_ch_lon, // * 1e7
|
||||
(ParamFloat<px4::params::NAV_DLL_CH_ALT>) _param_nav_dll_ch_alt,
|
||||
(ParamInt<px4::params::NAV_AH_LAT>) _param_nav_ah_lat, // * 1e7
|
||||
(ParamInt<px4::params::NAV_AH_LON>) _param_nav_ah_lon, // * 1e7
|
||||
(ParamFloat<px4::params::NAV_AH_ALT>) _param_nav_ah_alt,
|
||||
(ParamFloat<px4::params::NAV_DLL_AH_T>) _param_nav_dll_ah_t,
|
||||
(ParamInt<px4::params::NAV_DLL_N>) _param_nav_dll_n,
|
||||
(ParamInt<px4::params::NAV_DLL_CHSK>) _param_nav_dll_chsk
|
||||
)
|
||||
|
||||
enum DLLState {
|
||||
DLL_STATE_NONE = 0,
|
||||
DLL_STATE_FLYTOCOMMSHOLDWP = 1,
|
||||
DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
|
||||
DLL_STATE_TERMINATE = 3,
|
||||
DLL_STATE_END = 4
|
||||
} _dll_state{DLL_STATE_NONE};
|
||||
|
||||
/**
|
||||
* Set the DLL item
|
||||
*/
|
||||
void set_dll_item();
|
||||
|
||||
/**
|
||||
* Move to next DLL item
|
||||
*/
|
||||
void advance_dll();
|
||||
|
||||
};
|
||||
@@ -1,132 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014, 2015 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 <thomas@px4.io>
|
||||
*/
|
||||
|
||||
/*
|
||||
* 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 s
|
||||
* @min 0.0
|
||||
* @max 3600.0
|
||||
* @decimal 0
|
||||
* @increment 1
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
|
||||
|
||||
/**
|
||||
* Comms hold Lat
|
||||
*
|
||||
* Latitude of comms hold waypoint
|
||||
*
|
||||
* @unit deg * 1e7
|
||||
* @min -900000000
|
||||
* @max 900000000
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
|
||||
|
||||
/**
|
||||
* Comms hold Lon
|
||||
*
|
||||
* Longitude of comms hold waypoint
|
||||
*
|
||||
* @unit deg * 1e7
|
||||
* @min -1800000000
|
||||
* @max 1800000000
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
|
||||
|
||||
/**
|
||||
* Comms hold alt
|
||||
*
|
||||
* Altitude of comms hold waypoint
|
||||
*
|
||||
* @unit m
|
||||
* @min -50
|
||||
* @max 30000
|
||||
* @decimal 1
|
||||
* @increment 0.5
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
|
||||
|
||||
/**
|
||||
* Airfield home wait time
|
||||
*
|
||||
* The amount of time in seconds the system should wait at the airfield home waypoint
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.0
|
||||
* @max 3600.0
|
||||
* @decimal 0
|
||||
* @increment 1
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
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
|
||||
*
|
||||
* @min 0
|
||||
* @max 1000
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
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
|
||||
*
|
||||
* @boolean
|
||||
* @group Data Link Loss
|
||||
*/
|
||||
PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);
|
||||
@@ -41,7 +41,6 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "datalinkloss.h"
|
||||
#include "enginefailure.h"
|
||||
#include "follow_target.h"
|
||||
#include "geofence.h"
|
||||
@@ -51,7 +50,6 @@
|
||||
#include "loiter.h"
|
||||
#include "mission.h"
|
||||
#include "navigator_mode.h"
|
||||
#include "rcloss.h"
|
||||
#include "rtl.h"
|
||||
#include "takeoff.h"
|
||||
|
||||
@@ -83,7 +81,7 @@
|
||||
/**
|
||||
* Number of navigation modes that need on_active/on_inactive calls
|
||||
*/
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 11
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 9
|
||||
|
||||
|
||||
class Navigator : public ModuleBase<Navigator>, public ModuleParams
|
||||
@@ -382,8 +380,6 @@ private:
|
||||
Land _land; /**< class for handling land commands */
|
||||
PrecLand _precland; /**< class for handling precision land commands */
|
||||
RTL _rtl; /**< class that handles RTL */
|
||||
RCLoss _rcLoss; /**< class that handles RTL according to OBC rules (rc loss mode) */
|
||||
DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */
|
||||
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
|
||||
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
|
||||
FollowTarget _follow_target;
|
||||
|
||||
@@ -84,8 +84,6 @@ Navigator::Navigator() :
|
||||
_land(this),
|
||||
_precland(this),
|
||||
_rtl(this),
|
||||
_rcLoss(this),
|
||||
_dataLinkLoss(this),
|
||||
_engineFailure(this),
|
||||
_gpsFailure(this),
|
||||
_follow_target(this)
|
||||
@@ -94,14 +92,12 @@ Navigator::Navigator() :
|
||||
_navigation_mode_array[0] = &_mission;
|
||||
_navigation_mode_array[1] = &_loiter;
|
||||
_navigation_mode_array[2] = &_rtl;
|
||||
_navigation_mode_array[3] = &_dataLinkLoss;
|
||||
_navigation_mode_array[4] = &_engineFailure;
|
||||
_navigation_mode_array[5] = &_gpsFailure;
|
||||
_navigation_mode_array[6] = &_rcLoss;
|
||||
_navigation_mode_array[7] = &_takeoff;
|
||||
_navigation_mode_array[8] = &_land;
|
||||
_navigation_mode_array[9] = &_precland;
|
||||
_navigation_mode_array[10] = &_follow_target;
|
||||
_navigation_mode_array[3] = &_engineFailure;
|
||||
_navigation_mode_array[4] = &_gpsFailure;
|
||||
_navigation_mode_array[5] = &_takeoff;
|
||||
_navigation_mode_array[6] = &_land;
|
||||
_navigation_mode_array[7] = &_precland;
|
||||
_navigation_mode_array[8] = &_follow_target;
|
||||
|
||||
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
|
||||
_handle_reverse_delay = param_find("VT_B_REV_DEL");
|
||||
@@ -493,11 +489,6 @@ Navigator::run()
|
||||
navigation_mode_new = &_loiter;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
|
||||
_pos_sp_triplet_published_invalid_once = false;
|
||||
navigation_mode_new = &_rcLoss;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
|
||||
_pos_sp_triplet_published_invalid_once = false;
|
||||
|
||||
@@ -613,11 +604,6 @@ Navigator::run()
|
||||
_precland.set_mode(PrecLandMode::Required);
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
|
||||
_pos_sp_triplet_published_invalid_once = false;
|
||||
navigation_mode_new = &_dataLinkLoss;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
|
||||
_pos_sp_triplet_published_invalid_once = false;
|
||||
navigation_mode_new = &_engineFailure;
|
||||
|
||||
@@ -1,178 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <systemlib/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <navigator/navigation.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "datalinkloss.h"
|
||||
|
||||
RCLoss::RCLoss(Navigator *navigator) :
|
||||
MissionBlock(navigator),
|
||||
ModuleParams(navigator),
|
||||
_rcl_state(RCL_STATE_NONE)
|
||||
{
|
||||
}
|
||||
|
||||
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;
|
||||
advance_rcl();
|
||||
set_rcl_item();
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::on_active()
|
||||
{
|
||||
if (is_mission_item_reached()) {
|
||||
advance_rcl();
|
||||
set_rcl_item();
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
RCLoss::set_rcl_item()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
pos_sp_triplet->previous = pos_sp_triplet->current;
|
||||
_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 = _navigator->get_global_position()->yaw;
|
||||
_mission_item.loiter_radius = _navigator->get_loiter_radius();
|
||||
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
|
||||
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
|
||||
_mission_item.time_inside = _param_nav_gpsf_lt.get() < 0.0f ? 0.0f : _param_nav_gpsf_lt.get();
|
||||
_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->set_mission_result_updated();
|
||||
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_apply_limitation(_mission_item);
|
||||
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_nav_gpsf_lt.get() > 0.0f) {
|
||||
warnx("RC loss, OBC mode, loiter");
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC loss, loitering");
|
||||
_rcl_state = RCL_STATE_LOITER;
|
||||
|
||||
} else {
|
||||
warnx("RC loss, OBC mode, slip loiter, terminate");
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC loss, terminating");
|
||||
_rcl_state = RCL_STATE_TERMINATE;
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case RCL_STATE_LOITER:
|
||||
_rcl_state = RCL_STATE_TERMINATE;
|
||||
warnx("time is up, no RC regain, terminating");
|
||||
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC not regained, terminating");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
break;
|
||||
|
||||
case RCL_STATE_TERMINATE:
|
||||
warnx("rcl end");
|
||||
_rcl_state = RCL_STATE_END;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -1,81 +0,0 @@
|
||||
/***************************************************************************
|
||||
*
|
||||
* 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>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
#include "mission_block.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class RCLoss : public MissionBlock, public ModuleParams
|
||||
{
|
||||
public:
|
||||
RCLoss(Navigator *navigator);
|
||||
~RCLoss() = default;
|
||||
|
||||
void on_inactive() override;
|
||||
void on_activation() override;
|
||||
void on_active() override;
|
||||
|
||||
private:
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::NAV_RCL_LT>) _param_nav_gpsf_lt
|
||||
)
|
||||
|
||||
enum RCLState {
|
||||
RCL_STATE_NONE = 0,
|
||||
RCL_STATE_LOITER = 1,
|
||||
RCL_STATE_TERMINATE = 2,
|
||||
RCL_STATE_END = 3
|
||||
} _rcl_state{RCL_STATE_NONE};
|
||||
|
||||
/**
|
||||
* Set the RCL item
|
||||
*/
|
||||
void set_rcl_item();
|
||||
|
||||
/**
|
||||
* Move to next RCL item
|
||||
*/
|
||||
void advance_rcl();
|
||||
|
||||
};
|
||||
@@ -1,59 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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>
|
||||
*/
|
||||
|
||||
/*
|
||||
* OBC RC Loss mode parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* RC Loss Loiter Time (CASA Outback Challenge rules)
|
||||
*
|
||||
* The amount of time in seconds the system should loiter at current position before termination.
|
||||
* Only applies if NAV_RCL_ACT is set to 2 (CASA Outback Challenge rules).
|
||||
* Set to -1 to make the system skip loitering.
|
||||
*
|
||||
* @unit s
|
||||
* @min -1.0
|
||||
* @decimal 1
|
||||
* @increment 0.1
|
||||
* @group Mission
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);
|
||||
Reference in New Issue
Block a user