dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

- 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-10-25 11:56:09 +02:00 committed by Roman Bapst
parent b77487d69c
commit f02786d112
15 changed files with 59 additions and 459 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/vehicle_status.msg
View File

@ -30,9 +30,9 @@ uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure
uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down)
uint8 NAVIGATION_STATE_UNUSED = 9 # Free slot
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot
uint8 NAVIGATION_STATE_UNUSED1 = 11 # Free slot
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
uint8 NAVIGATION_STATE_OFFBOARD = 14

1
src/drivers/osd/atxxxx/atxxxx.cpp
View File

@ -409,7 +409,6 @@ OSDatxxxx::get_flight_mode(uint8_t nav_state)
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
flight_mode = "FAILURE";
break;

1
src/drivers/rc_input/crsf_telemetry.cpp
View File

@ -163,7 +163,6 @@ bool CRSFTelemetry::send_flight_mode()
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
flight_mode = "Failure";
break;

20
src/modules/commander/Commander.cpp
View File

@ -3304,8 +3304,8 @@ Commander::update_control_mode()
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
@ -3321,14 +3321,8 @@ Commander::update_control_mode()
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
@ -3337,7 +3331,7 @@ Commander::update_control_mode()
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_vehicle_control_mode.flag_control_auto_enabled = false;
_vehicle_control_mode.flag_control_auto_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
@ -3390,8 +3384,8 @@ Commander::update_control_mode()
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
break;
default:

5
src/modules/commander/ManualControl.cpp
View File

@ -69,9 +69,8 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT)
&& vehicle_control_mode.flag_control_offboard_enabled;
// in Descend and LandGPSFail manual override is enbaled independently of COM_RC_OVERRIDE
const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
// in Descend manual override is enbaled independently of COM_RC_OVERRIDE
const bool override_landing = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND);
if (_rc_available && (override_auto_mode || override_offboard_mode || override_landing)) {

43
src/modules/commander/state_machine_helper.cpp
View File

@ -119,7 +119,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"6: unallocated",
"7: unallocated",
"AUTO_LANDENGFAIL",
"AUTO_LANDGPSFAIL",
"ACRO",
"11: UNUSED",
"DESCEND",
@ -870,13 +869,8 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@ -934,21 +928,14 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
return;
} else {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
if (status_flags.condition_local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return;
if (status_flags.condition_local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
return;
}
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
return;
} else if (status_flags.condition_local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
}
return;
}
// FALLTHROUGH
@ -1016,13 +1003,7 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm
// If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) {
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@ -1095,13 +1076,7 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &
// If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) {
//TODO: Add case for rover
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
// TODO: FW position controller doesn't run without condition_global_position_valid
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
}
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;

40
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -215,26 +215,6 @@ void FlightModeManager::start_flight_task()
_task_failure_count = 0;
}
} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
// Auto related maneuvers
should_disable_task = false;
FlightTaskError error = FlightTaskError::NoError;
error = switchTask(FlightTaskIndex::AutoLineSmoothVel);
if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {
PX4_WARN("Auto activation failed with error: %s", errorToString(error));
}
task_failure = true;
_task_failure_count++;
} else {
// we want to be in this mode, reset the failure count
_task_failure_count = 0;
}
} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
// Emergency descend
@ -256,6 +236,26 @@ void FlightModeManager::start_flight_task()
_task_failure_count = 0;
}
} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
// Auto related maneuvers
should_disable_task = false;
FlightTaskError error = FlightTaskError::NoError;
error = switchTask(FlightTaskIndex::AutoLineSmoothVel);
if (error != FlightTaskError::NoError) {
if (prev_failure_count == 0) {
PX4_WARN("Auto activation failed with error: %s", errorToString(error));
}
task_failure = true;
_task_failure_count++;
} else {
// we want to be in this mode, reset the failure count
_task_failure_count = 0;
}
}
// manual position control

5
src/modules/mavlink/mavlink_messages.cpp
View File

@ -261,11 +261,6 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;

3
src/modules/mavlink/streams/HEARTBEAT.hpp
View File

@ -134,9 +134,6 @@ private:
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
system_status = MAV_STATE_EMERGENCY;
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL) {
system_status = MAV_STATE_EMERGENCY;
} else if (vehicle_status_flags.condition_calibration_enabled) {
system_status = MAV_STATE_CALIBRATING;
}

1
src/modules/navigator/CMakeLists.txt
View File

@ -49,7 +49,6 @@ px4_add_module(
mission_feasibility_checker.cpp
geofence.cpp
enginefailure.cpp
gpsfailure.cpp
follow_target.cpp
DEPENDS
geo

176
src/modules/navigator/gpsfailure.cpp
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;
}
}

90
src/modules/navigator/gpsfailure.h
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();
};

96
src/modules/navigator/gpsfailure_params.c
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);

4
src/modules/navigator/navigator.h
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 */

29
src/modules/navigator/navigator_main.cpp
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;