Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 18:47:21 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

navigator: run immediately on vehicle_status updates

Browse Source 
- this allows navigator to process any nav_state changes as soon as possible
This commit is contained in:
Daniel Agar
2019-11-20 10:31:50 -05:00
parent 4ce03dfc1e
commit 3f05a7724d
2 changed files with 8 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator.h
+1 -1
View File
@@ -316,6 +316,7 @@ private:
)
int _local_pos_sub{-1}; /**< local position subscription */
int _vehicle_status_sub{-1}; /**< local position subscription */
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */
@@ -325,7 +326,6 @@ private:
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
uORB::Subscription _vstatus_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
src/modules/navigator/navigator_main.cpp
+7 -2
View File
@@ -107,6 +107,7 @@ Navigator::Navigator() :
_handle_reverse_delay = param_find("VT_B_REV_DEL");
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
reset_triplets();
}
@@ -114,6 +115,7 @@ Navigator::Navigator() :
Navigator::~Navigator()
{
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_vehicle_status_sub);
}
void
@@ -147,11 +149,13 @@ Navigator::run()
params_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
px4_pollfd_struct_t fds[2] {};
/* Setup of loop */
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
fds[1].fd = _vehicle_status_sub;
fds[1].events = POLLIN;
/* rate-limit position subscription to 20 Hz / 50 ms */
orb_set_interval(_local_pos_sub, 50);
@@ -181,6 +185,8 @@ Navigator::run()
perf_begin(_loop_perf);
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus);
/* gps updated */
if (_gps_pos_sub.updated()) {
_gps_pos_sub.copy(&_gps_pos);
@@ -209,7 +215,6 @@ Navigator::run()
params_update();
}
_vstatus_sub.update(&_vstatus);
_land_detected_sub.update(&_land_detected);
_position_controller_status_sub.update();
_home_pos_sub.update(&_home_pos);