fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 14:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

failure_detector: add motor/ESC failure detection

Browse Source
This commit is contained in:
Alessandro Simovic
2022-05-12 07:58:56 +02:00
committed by Beat Küng
parent dfd934fbdb
commit fb71e7587c
25 changed files with 338 additions and 441 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/CMakeLists.txt
-1
View File
@@ -48,7 +48,6 @@ px4_add_module(
precland.cpp
mission_feasibility_checker.cpp
geofence.cpp
enginefailure.cpp
follow_target.cpp
vtol_takeoff.cpp
DEPENDS
src/modules/navigator/enginefailure.cpp
-140
View File
@@ -1,140 +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 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 <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <lib/geo/geo.h>
#include <navigator/navigation.h>
#include <px4_platform_common/events.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include "navigator.h"
#include "enginefailure.h"
EngineFailure::EngineFailure(Navigator *navigator) :
MissionBlock(navigator),
_ef_state(EF_STATE_NONE)
{
}
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();
pos_sp_triplet->previous = pos_sp_triplet->current;
_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.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_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;
}
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
EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down\t");
events::send(events::ID("enginefailure_loitering"), events::Log::Emergency, "Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
break;
}
}
src/modules/navigator/enginefailure.h
-73
View File
@@ -1,73 +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 enginefailure.h
* Helper class for a fixedwing engine failure mode
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class EngineFailure : public MissionBlock
{
public:
EngineFailure(Navigator *navigator);
~EngineFailure() = default;
void on_inactive() override;
void on_activation() override;
void on_active() override;
private:
enum EFState {
EF_STATE_NONE = 0,
EF_STATE_LOITERDOWN = 1,
} _ef_state{EF_STATE_NONE};
/**
* Set the DLL item
*/
void set_ef_item();
/**
* Move to next EF item
*/
void advance_ef();
};
src/modules/navigator/navigator.h
+1 -3
View File
@@ -41,7 +41,6 @@
#pragma once
#include "enginefailure.h"
#include "follow_target.h"
#include "geofence.h"
#include "land.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
{
@@ -393,7 +392,6 @@ private:
Land _land; /**< class for handling land commands */
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!) */
FollowTarget _follow_target;
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
src/modules/navigator/navigator_main.cpp
+5 -12
View File
@@ -78,19 +78,17 @@ Navigator::Navigator() :
_land(this),
_precland(this),
_rtl(this),
_engineFailure(this),
_follow_target(this)
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
_navigation_mode_array[1] = &_loiter;
_navigation_mode_array[2] = &_rtl;
_navigation_mode_array[3] = &_engineFailure;
_navigation_mode_array[4] = &_takeoff;
_navigation_mode_array[5] = &_land;
_navigation_mode_array[6] = &_precland;
_navigation_mode_array[7] = &_vtol_takeoff;
_navigation_mode_array[8] = &_follow_target;
_navigation_mode_array[3] = &_takeoff;
_navigation_mode_array[4] = &_land;
_navigation_mode_array[5] = &_precland;
_navigation_mode_array[6] = &_vtol_takeoff;
_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");
@@ -697,11 +695,6 @@ void Navigator::run()
_precland.set_mode(PrecLandMode::Required);
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_follow_target;