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

navigator: param enhancements

Browse Source
This commit is contained in:
Julian Oes
2014-06-06 18:13:45 +02:00
parent d78c3a2242
commit 85d7fdc741
7 changed files with 93 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+22 -4
View File
@@ -92,8 +92,7 @@ Navigator *g_navigator;
}
Navigator::Navigator() :
/* state machine transition table */
SuperBlock(NULL, "NAV"),
_task_should_exit(false),
_navigator_task(-1),
_mavlink_fd(-1),
@@ -122,8 +121,10 @@ Navigator::Navigator() :
_mission(this, "MIS"),
_loiter(this, "LOI"),
_rtl(this, "RTL"),
_update_triplet(false)
_update_triplet(false),
_param_loiter_radius(this, "LOITER_RAD")
{
updateParams();
}
Navigator::~Navigator()
@@ -188,6 +189,13 @@ Navigator::vehicle_control_mode_update()
}
}
void
Navigator::params_update()
{
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
}
void
Navigator::task_main_trampoline(int argc, char *argv[])
{
@@ -226,6 +234,7 @@ Navigator::task_main()
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_param_update_sub - orb_subscribe(ORB_ID(parameter_update));
/* copy all topics first time */
vehicle_status_update();
@@ -233,6 +242,7 @@ Navigator::task_main()
global_position_update();
home_position_update();
navigation_capabilities_update();
params_update();
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -241,7 +251,7 @@ Navigator::task_main()
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
struct pollfd fds[5];
struct pollfd fds[6];
/* Setup of loop */
fds[0].fd = _global_pos_sub;
@@ -254,6 +264,8 @@ Navigator::task_main()
fds[3].events = POLLIN;
fds[4].fd = _control_mode_sub;
fds[4].events = POLLIN;
fds[5].fd = _param_update_sub;
fds[5].events = POLLIN;
while (!_task_should_exit) {
@@ -278,6 +290,12 @@ Navigator::task_main()
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
/* parameters updated */
if (fds[5].revents & POLLIN) {
params_update();
updateParams();
}
/* vehicle control mode updated */
if (fds[4].revents & POLLIN) {
vehicle_control_mode_update();