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

navigator: publish distance sensor mode change request when in RTL landing phase or during mission landing

Browse Source
This commit is contained in:
Konrad
2024-09-16 09:11:23 +02:00
committed by KonradRudin
parent 1755b8304e
commit aab2390e51
9 changed files with 78 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+31
View File
@@ -110,6 +110,11 @@ Navigator::Navigator() :
_mission_sub = orb_subscribe(ORB_ID(mission));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_distance_sensor_mode_change_request_pub.advertise();
_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
_distance_sensor_mode_change_request_pub.get().req_mode = distance_sensor_mode_change_request_s::MODE_DISABLED;
_distance_sensor_mode_change_request_pub.update();
// Update the timeout used in mission_block (which can't hold it's own parameters)
_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
@@ -898,6 +903,8 @@ void Navigator::run()
publish_navigator_status();
publish_distance_sensor_mode_request();
_geofence.run();
perf_end(_loop_perf);
@@ -1447,6 +1454,30 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
_vehicle_cmd_pub.publish(*vcmd);
}
void Navigator::publish_distance_sensor_mode_request()
{
// Send request to enable distance sensor when in the landing phase of a mission or RTL
if (((_navigation_mode == &_rtl) && _rtl.isLanding()) || ((_navigation_mode == &_mission) && _mission.isLanding())) {
if (_distance_sensor_mode_change_request_pub.get().req_mode !=
distance_sensor_mode_change_request_s::MODE_ENABLED) {
_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
_distance_sensor_mode_change_request_pub.get().req_mode =
distance_sensor_mode_change_request_s::MODE_ENABLED;
_distance_sensor_mode_change_request_pub.update();
}
} else if (_distance_sensor_mode_change_request_pub.get().req_mode !=
distance_sensor_mode_change_request_s::MODE_DISABLED) {
_distance_sensor_mode_change_request_pub.get().timestamp = hrt_absolute_time();
_distance_sensor_mode_change_request_pub.get().req_mode =
distance_sensor_mode_change_request_s::MODE_DISABLED;
_distance_sensor_mode_change_request_pub.update();
}
}
void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result)
{
vehicle_command_ack_s command_ack = {};