fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 13:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator/Commander: make GPS failsafe consitent: switch to Descend also for FW and VTOL

Browse Source 
- remove GPS failsafe mode
- for VTOL: transition to hover in Descend (unless NAV_FORCE_VT is not set)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-05 12:09:39 +03:00
committed by Roman Bapst
parent b77487d69c
commit f02786d112
15 changed files with 59 additions and 459 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/CMakeLists.txt
-1
View File
@@ -49,7 +49,6 @@ px4_add_module(
mission_feasibility_checker.cpp
geofence.cpp
enginefailure.cpp
gpsfailure.cpp
follow_target.cpp
DEPENDS
geo
src/modules/navigator/gpsfailure.cpp
-176
View File
@@ -1,176 +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 gpsfailure.cpp
* Helper class for gpsfailure mode according to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "gpsfailure.h"
#include "navigator.h"
#include <systemlib/mavlink_log.h>
#include <lib/geo/geo.h>
#include <navigator/navigation.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <px4_platform_common/events.h>
#include <mathlib/mathlib.h>
using matrix::Eulerf;
using matrix::Quatf;
GpsFailure::GpsFailure(Navigator *navigator) :
MissionBlock(navigator),
ModuleParams(navigator)
{
}
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();
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 */
vehicle_attitude_setpoint_s att_sp = {};
att_sp.timestamp = hrt_absolute_time();
att_sp.roll_body = math::radians(_param_nav_gpsf_r.get());
att_sp.pitch_body = math::radians(_param_nav_gpsf_p.get());
att_sp.thrust_body[0] = _param_nav_gpsf_tr.get();
Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
q.copyTo(att_sp.q_d);
if (_navigator->get_vstatus()->is_vtol) {
_fw_virtual_att_sp_pub.publish(att_sp);
} else {
_att_sp_pub.publish(att_sp);
}
/* Measure time */
if ((_param_nav_gpsf_lt.get() > FLT_EPSILON) &&
(hrt_elapsed_time(&_timestamp_activation) > _param_nav_gpsf_lt.get() * 1e6f)) {
/* no recovery, advance the state machine */
PX4_WARN("GPS not recovered, switching to next failure 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 commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->set_mission_result_updated();
PX4_WARN("GPS failure: request flight termination");
}
break;
default:
break;
}
_navigator->set_position_setpoint_triplet_updated();
}
void
GpsFailure::advance_gpsf()
{
switch (_gpsf_state) {
case GPSF_STATE_NONE:
_gpsf_state = GPSF_STATE_LOITER;
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: loitering\t");
events::send(events::ID("navigator_gpsfailure_loitering"), events::Log::Error, "Global position failure: loitering");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no GPS recovery, terminating flight\t");
events::send(events::ID("navigator_gpsfailure_terminate"), events::Log::Emergency,
"No GPS recovery, terminating flight");
break;
case GPSF_STATE_TERMINATE:
PX4_WARN("terminate");
_gpsf_state = GPSF_STATE_END;
break;
default:
break;
}
}
src/modules/navigator/gpsfailure.h
-90
View File
@@ -1,90 +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 gpsfailure.h
* Helper class for Data Link Loss Mode according to the OBC rules
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include <px4_platform_common/module_params.h>
#include "mission_block.h"
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
class Navigator;
class GpsFailure : public MissionBlock, public ModuleParams
{
public:
GpsFailure(Navigator *navigator);
~GpsFailure() = default;
void on_inactive() override;
void on_activation() override;
void on_active() override;
private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_GPSF_LT>) _param_nav_gpsf_lt,
(ParamFloat<px4::params::NAV_GPSF_R>) _param_nav_gpsf_r,
(ParamFloat<px4::params::NAV_GPSF_P>) _param_nav_gpsf_p,
(ParamFloat<px4::params::NAV_GPSF_TR>) _param_nav_gpsf_tr
)
enum GPSFState {
GPSF_STATE_NONE = 0,
GPSF_STATE_LOITER = 1,
GPSF_STATE_TERMINATE = 2,
GPSF_STATE_END = 3,
} _gpsf_state{GPSF_STATE_NONE};
hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)};
/**
* Set the GPSF item
*/
void set_gpsf_item();
/**
* Move to next GPSF item
*/
void advance_gpsf();
};
src/modules/navigator/gpsfailure_params.c
-96
View File
@@ -1,96 +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 gpsfailure_params.c
*
* Parameters for GPSF navigation mode
*
*/
/**
* Loiter time
*
* The time in seconds the system should do open loop loiter and wait for GPS recovery
* before it goes into flight termination. Set to 0 to disable.
*
* @unit s
* @min 0.0
* @max 3600.0
* @decimal 0
* @increment 1
* @group GPS Failure Navigation
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 0.0f);
/**
* Fixed bank angle
*
* Roll in degrees during the loiter
*
* @unit deg
* @min 0.0
* @max 30.0
* @decimal 1
* @increment 0.5
* @group GPS Failure Navigation
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
/**
* Fixed pitch angle
*
* Pitch in degrees during the open loop loiter
*
* @unit deg
* @min -30.0
* @max 30.0
* @decimal 1
* @increment 0.5
* @group GPS Failure Navigation
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
/**
* Thrust
*
* Thrust value which is set during the open loop loiter
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.05
* @group GPS Failure Navigation
*/
PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.0f);
src/modules/navigator/navigator.h
+1 -3
View File
@@ -44,7 +44,6 @@
#include "enginefailure.h"
#include "follow_target.h"
#include "geofence.h"
#include "gpsfailure.h"
#include "land.h"
#include "precland.h"
#include "loiter.h"
@@ -86,7 +85,7 @@ using namespace time_literals;
/**
* Number of navigation modes that need on_active/on_inactive calls
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 9
#define NAVIGATOR_MODE_ARRAY_SIZE 8
class Navigator : public ModuleBase<Navigator>, public ModuleParams
{
@@ -403,7 +402,6 @@ private:
PrecLand _precland; /**< class for handling precision land commands */
RTL _rtl; /**< class that handles RTL */
EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */
GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */
FollowTarget _follow_target;
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
src/modules/navigator/navigator_main.cpp
+18 -11
View File
@@ -83,7 +83,6 @@ Navigator::Navigator() :
_precland(this),
_rtl(this),
_engineFailure(this),
_gpsFailure(this),
_follow_target(this)
{
/* Create a list of our possible navigation types */
@@ -91,11 +90,10 @@ Navigator::Navigator() :
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_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;
_navigation_mode_array[4] = &_takeoff;
_navigation_mode_array[5] = &_land;
_navigation_mode_array[6] = &_precland;
_navigation_mode_array[7] = &_follow_target;
_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -706,11 +704,6 @@ Navigator::run()
navigation_mode_new = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_follow_target;
@@ -750,6 +743,20 @@ Navigator::run()
navigation_mode_new == &_loiter)) {
reset_triplets();
}
// transition to hover in Descend mode
if (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND &&
_vstatus.is_vtol && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
force_vtol()) {
vehicle_command_s vcmd = {};
vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
vcmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
publish_vehicle_cmd(&vcmd);
mavlink_log_info(&_mavlink_log_pub, "Transition to hover mode and descend.\t");
events::send(events::ID("navigator_transition_descend"), events::Log::Critical,
"Transition to hover mode and descend");
}
}
_navigation_mode = navigation_mode_new;