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

RTL: publish a status message on currently chosen RTL point

This commit is contained in:
Konrad 2024-02-14 10:40:15 +01:00 committed by Silvan Fuhrer
parent 74303a79e1
commit 68100650da
6 changed files with 62 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -173,6 +173,7 @@ set(msg_files
RegisterExtComponentReply.msg
RegisterExtComponentRequest.msg
Rpm.msg
RtlStatus.msg
RtlTimeEstimate.msg
SatelliteInfo.msg
SensorAccel.msg

15
msg/RtlStatus.msg Normal file
View File

@ -0,0 +1,15 @@
uint64 timestamp # time since system start (microseconds)
uint32 safe_points_id # unique ID of active set of safe_point_items
bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting
uint8 rtl_type # Type of RTL chosen
uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position

1
src/modules/logger/logged_topics.cpp
View File

@ -102,6 +102,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("px4io_status");
add_topic("radio_status");
add_topic("rtl_time_estimate", 1000);
add_optional_topic("rtl_status", 5000);
add_optional_topic("sensor_airflow", 100);
add_topic("sensor_combined");
add_optional_topic("sensor_correction");

2
src/modules/navigator/navigator_main.cpp
View File

@ -208,7 +208,7 @@ void Navigator::run()
if (mission.safe_points_id != safe_points_id) {
safe_points_id = mission.safe_points_id;
_rtl.updateSafePoints();
_rtl.updateSafePoints(safe_points_id);
}
}

39
src/modules/navigator/rtl.cpp
View File

@ -294,12 +294,14 @@ void RTL::setRtlTypeAndDestination()
init_rtl_mission_type();
uint8_t safe_point_index{0U};
if (_param_rtl_type.get() != 2) {
// check the closest allowed destination.
DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
PositionYawSetpoint rtl_position;
float rtl_alt;
findRtlDestination(destination_type, rtl_position, rtl_alt);
findRtlDestination(destination_type, rtl_position, rtl_alt, safe_point_index);
switch (destination_type) {
case DestinationType::DESTINATION_TYPE_MISSION_LAND:
@ -331,9 +333,29 @@ void RTL::setRtlTypeAndDestination()
break;
}
}
// Publish rtl status
_rtl_status_pub.get().timestamp = hrt_absolute_time();
_rtl_status_pub.get().safe_points_id = _safe_points_id;
_rtl_status_pub.get().is_evaluation_pending = _dataman_state != DatamanState::UpdateRequestWait;
_rtl_status_pub.get().has_vtol_approach = false;
if ((_param_rtl_type.get() == 0) || (_param_rtl_type.get() == 3)) {
_rtl_status_pub.get().has_vtol_approach = _home_has_land_approach || _one_rally_point_has_land_approach;
} else if (_param_rtl_type.get() == 1) {
_rtl_status_pub.get().has_vtol_approach = _one_rally_point_has_land_approach;
}
_rtl_status_pub.get().rtl_type = static_cast<uint8_t>(_rtl_type);
_rtl_status_pub.get().safe_point_index = safe_point_index;
_rtl_status_pub.update();
}
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt)
void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
uint8_t &safe_point_index)
{
// set destination to home per default, then check if other valid landing spot is closer
rtl_position.alt = _home_pos_sub.get().alt;
@ -352,8 +374,10 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)};
float min_dist;
_home_has_land_approach = hasVtolLandApproach(rtl_position);
if (((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) || (vtol_in_fw_mode && (_param_rtl_approach_force.get() == 1)
&& !hasVtolLandApproach(rtl_position))) {
&& !_home_has_land_approach)) {
// Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode or we forces approach landing for vtol in fw and it is not defined for home.
min_dist = FLT_MAX;
@ -394,6 +418,8 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
if (_safe_points_updated) {
_one_rally_point_has_land_approach = false;
for (int current_seq = 0; current_seq < _dataman_cache_safepoint.size(); ++current_seq) {
mission_item_s mission_safe_point;
@ -416,11 +442,16 @@ void RTL::findRtlDestination(DestinationType &destination_type, PositionYawSetpo
PositionYawSetpoint safepoint_position;
setSafepointAsDestination(safepoint_position, mission_safe_point);
bool current_safe_point_has_approaches{hasVtolLandApproach(safepoint_position)};
_one_rally_point_has_land_approach |= current_safe_point_has_approaches;
if (((dist + MIN_DIST_THRESHOLD) < min_dist) && (!vtol_in_fw_mode || (_param_rtl_approach_force.get() == 0)
|| hasVtolLandApproach(safepoint_position))) {
|| current_safe_point_has_approaches)) {
min_dist = dist;
rtl_position = safepoint_position;
destination_type = DestinationType::DESTINATION_TYPE_SAFE_POINT;
safe_point_index = current_seq;
}
}
}

11
src/modules/navigator/rtl.h
View File

@ -58,6 +58,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rtl_status.h>
#include <uORB/topics/rtl_time_estimate.h>
class Navigator;
@ -86,7 +87,7 @@ public:
void set_return_alt_min(bool min) { _enforce_rtl_alt = min; }
void updateSafePoints() { _initiate_safe_points_updated = true; }
void updateSafePoints(uint32_t new_safe_point_id) { _initiate_safe_points_updated = true; _safe_points_id = new_safe_point_id; }
private:
enum class DestinationType {
@ -109,7 +110,8 @@ private:
* @brief Find RTL destination.
*
*/
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt);
void findRtlDestination(DestinationType &destination_type, PositionYawSetpoint &rtl_position, float &rtl_alt,
uint8_t &safe_point_index);
/**
* @brief Set the position of the land start marker in the planned mission as destination.
@ -188,6 +190,9 @@ private:
RtlType _rtl_type{RtlType::RTL_DIRECT};
bool _home_has_land_approach; ///< Flag if the home position has a land approach defined
bool _one_rally_point_has_land_approach; ///< Flag if a rally point has a land approach defined
DatamanState _dataman_state{DatamanState::UpdateRequestWait};
DatamanState _error_state{DatamanState::UpdateRequestWait};
uint32_t _opaque_id{0}; ///< dataman safepoint id: if it does not match, safe points data was updated
@ -197,6 +202,7 @@ private:
bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed
mutable DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2};
uint32_t _mission_id = 0u;
uint32_t _safe_points_id = 0u;
mission_stats_entry_s _stats;
@ -222,4 +228,5 @@ private:
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
uORB::Publication<rtl_time_estimate_s> _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)};
uORB::PublicationData<rtl_status_s> _rtl_status_pub{ORB_ID(rtl_status)};
};