fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 11:27:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

RTL: Keep only the selected RTL type in memory.

Browse Source
This commit is contained in:
Konrad
2023-08-16 14:35:40 +02:00
committed by Silvan Fuhrer
parent d4ea106f9e
commit 5c021d8fa4
11 changed files with 192 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+91 -67
View File
@@ -49,13 +49,12 @@
using namespace time_literals;
using namespace math;
static constexpr float MIN_DIST_THRESHOLD = 2.f;
RTL::RTL(Navigator *navigator) :
NavigatorMode(navigator),
ModuleParams(navigator),
_rtl_direct(navigator),
_rtl_direct_mission_land(navigator),
_rtl_mission(navigator),
_rtl_mission_reverse(navigator)
_rtl_direct(navigator)
{
}
@@ -162,22 +161,16 @@ void RTL::updateDatamanCache()
void RTL::on_inactivation()
{
switch (_rtl_type) {
case RtlType::RTL_MISSION_FAST:
_rtl_mission.on_inactivation();
break;
case RtlType::RTL_MISSION_FAST_REVERSE:
_rtl_mission_reverse.on_inactivation();
case RtlType::RTL_MISSION_FAST: // Fall through
case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_mission_type_handle->on_inactivation();
break;
case RtlType::RTL_DIRECT:
_rtl_direct.on_inactivation();
break;
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_direct_mission_land.on_inactivation();
break;
default:
break;
}
@@ -194,10 +187,20 @@ void RTL::on_inactive()
parameters_update();
_rtl_mission.on_inactive();
_rtl_mission_reverse.on_inactive();
_rtl_direct.on_inactive();
_rtl_direct_mission_land.on_inactive();
switch (_rtl_type) {
case RtlType::RTL_MISSION_FAST:
case RtlType::RTL_MISSION_FAST_REVERSE:
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_mission_type_handle->on_inactive();
break;
case RtlType::RTL_DIRECT:
_rtl_direct.on_inactive();
break;
default:
break;
}
// Limit inactive calculation to 1Hz
hrt_abstime now{hrt_absolute_time()};
@@ -214,16 +217,14 @@ void RTL::on_inactive()
if (_navigator->home_global_position_valid() && global_position_recently_updated) {
switch (_rtl_type) {
case RtlType::RTL_DIRECT: estimated_time = _rtl_direct.calc_rtl_time_estimate();
case RtlType::RTL_DIRECT:
estimated_time = _rtl_direct.calc_rtl_time_estimate();
break;
case RtlType::RTL_DIRECT_MISSION_LAND: estimated_time = _rtl_direct_mission_land.calc_rtl_time_estimate();
break;
case RtlType::RTL_MISSION_FAST: estimated_time = _rtl_mission.calc_rtl_time_estimate();
break;
case RtlType::RTL_MISSION_FAST_REVERSE: estimated_time = _rtl_mission_reverse.calc_rtl_time_estimate();
case RtlType::RTL_DIRECT_MISSION_LAND:
case RtlType::RTL_MISSION_FAST:
case RtlType::RTL_MISSION_FAST_REVERSE:
estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate();
break;
default: break;
@@ -239,21 +240,18 @@ void RTL::on_activation()
setRtlTypeAndDestination();
switch (_rtl_type) {
case RtlType::RTL_MISSION_FAST:
_rtl_mission.on_activation();
break;
case RtlType::RTL_DIRECT_MISSION_LAND: // Fall through
case RtlType::RTL_MISSION_FAST: // Fall through
case RtlType::RTL_MISSION_FAST_REVERSE:
_rtl_mission_reverse.on_activation();
_rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt);
_rtl_mission_type_handle->on_activation();
break;
case RtlType::RTL_DIRECT:
_rtl_direct.on_activation(_enforce_rtl_alt);
_rtl_direct.setReturnAltMin(_enforce_rtl_alt);
_rtl_direct.on_activation();
break;
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_direct_mission_land.on_activation(_enforce_rtl_alt);
break;
default:
break;
@@ -271,31 +269,13 @@ void RTL::on_active()
switch (_rtl_type) {
case RtlType::RTL_MISSION_FAST:
_rtl_mission.on_active();
_rtl_mission_reverse.on_inactive();
_rtl_direct.on_inactive();
_rtl_direct_mission_land.on_inactive();
break;
case RtlType::RTL_MISSION_FAST_REVERSE:
_rtl_mission_reverse.on_active();
_rtl_mission.on_inactive();
_rtl_direct.on_inactive();
_rtl_direct_mission_land.on_inactive();
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_mission_type_handle->on_active();
break;
case RtlType::RTL_DIRECT:
_rtl_direct.on_active();
_rtl_direct_mission_land.on_inactive();
_rtl_mission_reverse.on_inactive();
_rtl_mission.on_inactive();
break;
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_direct_mission_land.on_active();
_rtl_mission_reverse.on_inactive();
_rtl_mission.on_inactive();
_rtl_direct.on_inactive();
break;
default:
@@ -305,15 +285,10 @@ void RTL::on_active()
void RTL::setRtlTypeAndDestination()
{
if (_param_rtl_type.get() == 2) {
if (hasMissionLandStart()) {
_rtl_type = RtlType::RTL_MISSION_FAST;
} else {
_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;
}
init_rtl_mission_type();
} else {
if (_param_rtl_type.get() != 2) {
// check the closest allowed destination.
bool isMissionLanding{false};
RtlDirect::RtlPosition rtl_position;
@@ -321,13 +296,13 @@ void RTL::setRtlTypeAndDestination()
findRtlDestination(isMissionLanding, rtl_position, rtl_alt);
if (isMissionLanding) {
_rtl_direct_mission_land.setRtlAlt(rtl_alt);
_rtl_type = RtlType::RTL_DIRECT_MISSION_LAND;
_rtl_mission_type_handle->setRtlAlt(rtl_alt);
} else {
_rtl_type = RtlType::RTL_DIRECT;
_rtl_direct.setRtlAlt(rtl_alt);
_rtl_direct.setRtlPosition(rtl_position);
_rtl_type = RtlType::RTL_DIRECT;
}
}
}
@@ -372,7 +347,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)};
if (dist < min_dist) {
if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
min_dist = dist;
setLandPosAsDestination(rtl_position, land_mission_item);
isMissionLanding = true;
@@ -395,7 +370,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl
float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)};
if (dist < min_dist) {
if ((dist + MIN_DIST_THRESHOLD) < min_dist) {
min_dist = dist;
setSafepointAsDestination(rtl_position, mission_safe_point);
isMissionLanding = false;
@@ -485,6 +460,53 @@ float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPositio
return max(return_altitude_amsl, _global_pos_sub.get().alt);
}
void RTL::init_rtl_mission_type()
{
RtlType new_rtl_mission_type{RtlType::RTL_DIRECT_MISSION_LAND};
if (_param_rtl_type.get() == 2) {
if (hasMissionLandStart()) {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST;
} else {
new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
}
}
if (_set_rtl_mission_type == new_rtl_mission_type) {
return;
}
if (_rtl_mission_type_handle) {
delete _rtl_mission_type_handle;
_rtl_mission_type_handle = nullptr;
_set_rtl_mission_type = RtlType::NONE;
}
switch (new_rtl_mission_type) {
case RtlType::RTL_DIRECT_MISSION_LAND:
_rtl_mission_type_handle = new RtlDirectMissionLand(_navigator);
_set_rtl_mission_type = RtlType::RTL_DIRECT_MISSION_LAND;
// RTL type is either direct or mission land have to set it later.
break;
case RtlType::RTL_MISSION_FAST:
_rtl_mission_type_handle = new RtlMissionFast(_navigator);
_set_rtl_mission_type = RtlType::RTL_MISSION_FAST;
_rtl_type = RtlType::RTL_MISSION_FAST;
break;
case RtlType::RTL_MISSION_FAST_REVERSE:
_rtl_mission_type_handle = new RtlMissionFastReverse(_navigator);
_set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
_rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;;
break;
default:
break;
}
}
void RTL::parameters_update()
{
if (_parameter_update_sub.updated()) {
@@ -495,7 +517,9 @@ void RTL::parameters_update()
// this class attributes need updating (and do so).
updateParams();
setRtlTypeAndDestination();
if (!isActive()) {
setRtlTypeAndDestination();
}
}
}
src/modules/navigator/rtl.h
+19 -9
View File
@@ -45,6 +45,7 @@
#include "navigator_mode.h"
#include <dataman_client/DatamanClient.hpp>
#include "rtl_base.h"
#include "rtl_direct.h"
#include "rtl_direct_mission_land.h"
#include "rtl_mission_fast.h"
@@ -68,6 +69,7 @@ public:
~RTL() = default;
enum class RtlType {
NONE,
RTL_DIRECT,
RTL_DIRECT_MISSION_LAND,
RTL_MISSION_FAST,
@@ -115,13 +117,24 @@ private:
void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point);
/**
* @brief
* @brief calculate return altitude from cone half angle
*
* @param cone_half_angle_deg
* @return float
* @param[in] rtl_position landing position of the rtl
* @param[in] cone_half_angle_deg half angle of the cone [deg]
* @return return altitude
*/
float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg);
/**
* @brief initialize RTL mission type
*
*/
void init_rtl_mission_type();
/**
* @brief Update parameters
*
*/
void parameters_update();
enum class DatamanState {
@@ -134,6 +147,9 @@ private:
hrt_abstime _destination_check_time{0};
RtlBase *_rtl_mission_type_handle{nullptr};
RtlType _set_rtl_mission_type{RtlType::NONE};
RtlType _rtl_type{RtlType::RTL_DIRECT};
DatamanState _dataman_state{DatamanState::UpdateRequestWait};
@@ -150,12 +166,6 @@ private:
RtlDirect _rtl_direct;
RtlDirectMissionLand _rtl_direct_mission_land;
RtlMissionFast _rtl_mission;
RtlMissionFastReverse _rtl_mission_reverse;
bool _enforce_rtl_alt{false};
DEFINE_PARAMETERS(
src/modules/navigator/rtl_base.h
+57
View File
@@ -0,0 +1,57 @@
/***************************************************************************
*
* Copyright (c) 2023 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 rtl_base.h
*
* Helper class for RTL modes using the mission
*
*/
#pragma once
#include "mission_base.h"
#include <uORB/topics/rtl_time_estimate.h>
class RtlBase : public MissionBase
{
public:
RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed):
MissionBase(navigator, dataman_cache_size_signed) {};
virtual ~RtlBase() = default;
virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0;
virtual void setReturnAltMin(bool min) { (void)min;};
virtual void setRtlAlt(float alt) { (void)alt;};
};
src/modules/navigator/rtl_direct.cpp
+2 -2
View File
@@ -74,7 +74,7 @@ void RtlDirect::on_inactivation()
}
}
void RtlDirect::on_activation(bool enforce_rtl_alt)
void RtlDirect::on_activation()
{
_global_pos_sub.update();
_local_pos_sub.update();
@@ -87,7 +87,7 @@ void RtlDirect::on_activation(bool enforce_rtl_alt)
// For safety reasons don't go into RTL if landed.
_rtl_state = RTL_STATE_LANDED;
} else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || enforce_rtl_alt) {
} else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || _enforce_rtl_alt) {
// If lower than return altitude, climb up first.
// If rtl_alt_min is true then forcing altitude change even if above.
src/modules/navigator/rtl_direct.h
+5 -3
View File
@@ -88,12 +88,11 @@ public:
void on_inactivation() override;
/**
* @brief On activation.
* @brief on activation.
* Initialize the return to launch calculations.
*
* @param[in] enforce_rtl_alt boolean if the minimal return to launch altitude should be enforced at the beginning of the return, even when the current vehicle altitude is above.
*/
void on_activation(bool enforce_rtl_alt);
void on_activation() override;
/**
* @brief on active
@@ -109,6 +108,7 @@ public:
*/
rtl_time_estimate_s calc_rtl_time_estimate();
void setReturnAltMin(bool min) { _enforce_rtl_alt = min; }
void setRtlAlt(float alt) {_rtl_alt = alt;};
void setRtlPosition(RtlPosition position) {_destination = position;};
@@ -204,6 +204,8 @@ private:
/** Current state in the state machine.*/
RTLState _rtl_state{RTL_STATE_NONE};
bool _enforce_rtl_alt{false};
RtlPosition _destination{}; ///< the RTL position to fly to
float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position
src/modules/navigator/rtl_direct_mission_land.cpp
+3 -3
View File
@@ -47,12 +47,12 @@
static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5;
RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) :
MissionBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE)
RtlBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE)
{
}
void RtlDirectMissionLand::on_activation(bool enforce_rtl_alt)
void RtlDirectMissionLand::on_activation()
{
_land_detected_sub.update();
_global_pos_sub.update();
@@ -62,7 +62,7 @@ void RtlDirectMissionLand::on_activation(bool enforce_rtl_alt)
if (hasMissionLandStart()) {
_is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK);
if ((_global_pos_sub.get().alt < _rtl_alt) || enforce_rtl_alt) {
if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) {
// If lower than return altitude, climb up first.
// If enforce_rtl_alt is true then forcing altitude change even if above.
src/modules/navigator/rtl_direct_mission_land.h
+7 -5
View File
@@ -41,7 +41,7 @@
#pragma once
#include "mission.h"
#include "rtl_base.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
@@ -49,17 +49,18 @@
class Navigator;
class RtlDirectMissionLand : public MissionBase
class RtlDirectMissionLand : public RtlBase
{
public:
RtlDirectMissionLand(Navigator *navigator);
~RtlDirectMissionLand() = default;
void on_activation(bool enforce_rtl_alt);
void on_activation() override;
rtl_time_estimate_s calc_rtl_time_estimate();
rtl_time_estimate_s calc_rtl_time_estimate() override;
void setRtlAlt(float alt) {_rtl_alt = alt;};
void setReturnAltMin(bool min) override { _enforce_rtl_alt = min; };
void setRtlAlt(float alt) override {_rtl_alt = alt;};
private:
bool setNextMissionItem() override;
@@ -68,5 +69,6 @@ private:
bool do_need_move_to_land();
bool _needs_climbing{false}; //< Flag if climbing is required at the start
bool _enforce_rtl_alt{false};
float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position
};
src/modules/navigator/rtl_mission_fast.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5;
RtlMissionFast::RtlMissionFast(Navigator *navigator) :
MissionBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE)
RtlBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE)
{
}
src/modules/navigator/rtl_mission_fast.h
+3 -3
View File
@@ -41,7 +41,7 @@
#pragma once
#include "mission_base.h"
#include "rtl_base.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
@@ -49,7 +49,7 @@
class Navigator;
class RtlMissionFast : public MissionBase
class RtlMissionFast : public RtlBase
{
public:
RtlMissionFast(Navigator *navigator);
@@ -59,7 +59,7 @@ public:
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate();
rtl_time_estimate_s calc_rtl_time_estimate() override;
private:
bool setNextMissionItem() override;
src/modules/navigator/rtl_mission_fast_reverse.cpp
+1 -1
View File
@@ -47,7 +47,7 @@
static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5;
RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
MissionBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE)
RtlBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE)
{
}
src/modules/navigator/rtl_mission_fast_reverse.h
+3 -3
View File
@@ -41,7 +41,7 @@
#pragma once
#include "mission_base.h"
#include "rtl_base.h"
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
@@ -49,7 +49,7 @@
class Navigator;
class RtlMissionFastReverse : public MissionBase
class RtlMissionFastReverse : public RtlBase
{
public:
RtlMissionFastReverse(Navigator *navigator);
@@ -59,7 +59,7 @@ public:
void on_active() override;
void on_inactive() override;
rtl_time_estimate_s calc_rtl_time_estimate();
rtl_time_estimate_s calc_rtl_time_estimate() override;
private:
bool setNextMissionItem() override;