Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

RTL direct: publish NavigatorMissionItem

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-08-06 22:15:25 +02:00
parent b01c179eed
commit 588c4a04c8
2 changed files with 39 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl_direct.cpp
+31
View File
@@ -337,6 +337,8 @@ void RtlDirect::set_rtl_item()
_navigator->set_position_setpoint_triplet_updated();
}
}
publish_rtl_direct_navigator_mission_item(); // for logging
}
RtlDirect::RTLState RtlDirect::getActivationLandState()
@@ -515,3 +517,32 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con
return sanitized_land_approach;
}
void RtlDirect::publish_rtl_direct_navigator_mission_item()
{
navigator_mission_item_s navigator_mission_item{};
navigator_mission_item.sequence_current = static_cast<uint16_t>(_rtl_state);
navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
navigator_mission_item.latitude = _mission_item.lat;
navigator_mission_item.longitude = _mission_item.lon;
navigator_mission_item.altitude = _mission_item.altitude;
navigator_mission_item.time_inside = get_time_inside(_mission_item);
navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius;
navigator_mission_item.loiter_radius = _mission_item.loiter_radius;
navigator_mission_item.yaw = _mission_item.yaw;
navigator_mission_item.frame = _mission_item.frame;
navigator_mission_item.frame = _mission_item.origin;
navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack;
navigator_mission_item.force_heading = _mission_item.force_heading;
navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative;
navigator_mission_item.autocontinue = _mission_item.autocontinue;
navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition;
navigator_mission_item.timestamp = hrt_absolute_time();
_navigator_mission_item_pub.publish(navigator_mission_item);
}
src/modules/navigator/rtl_direct.h
+8
View File
@@ -47,6 +47,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/navigator_mission_item.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -137,6 +138,12 @@ private:
*/
void parameters_update();
/**
* @brief Publish navigator mission item
*
*/
void publish_rtl_direct_navigator_mission_item();
RTLState getActivationLandState();
void setLoiterPosition();
@@ -167,4 +174,5 @@ private:
uORB::SubscriptionData<vehicle_land_detected_s> _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::SubscriptionData<vehicle_status_s> _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::SubscriptionData<wind_s> _wind_sub{ORB_ID(wind)};
uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/
};