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

Navigator resurrect FW GPS failure navigation (#7762)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-08 12:18:59 -04:00
committed by GitHub
parent e15afcca7a
commit 7a42424411
6 changed files with 138 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+13 -56
View File
@@ -57,14 +57,9 @@
#include <px4_tasks.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <drivers/device/device.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
@@ -267,13 +262,11 @@ Navigator::task_main()
px4_pollfd_struct_t fds[1] = {};
/* Setup of loop */
fds[0].fd = _global_pos_sub;
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
bool global_pos_available_once = false;
/* rate-limit global pos subscription to 20 Hz / 50 ms */
orb_set_interval(_global_pos_sub, 49);
/* rate-limit position subscription to 20 Hz / 50 ms */
orb_set_interval(_local_pos_sub, 50);
while (!_task_should_exit) {
@@ -281,11 +274,6 @@ Navigator::task_main()
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret == 0) {
/* timed out - periodic check for _task_should_exit, etc. */
if (global_pos_available_once) {
global_pos_available_once = false;
}
/* Let the loop run anyway, don't do `continue` here. */
} else if (pret < 0) {
@@ -295,16 +283,9 @@ Navigator::task_main()
continue;
} else {
if (fds[0].revents & POLLIN) {
/* success, global pos is available */
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
global_pos_available_once = true;
/* success, local pos is available */
local_position_update();
}
}
@@ -323,11 +304,15 @@ Navigator::task_main()
}
}
/* local position updated */
orb_check(_local_pos_sub, &updated);
/* global position updated */
orb_check(_global_pos_sub, &updated);
if (updated) {
local_position_update();
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
have_geofence_position_data = true;
}
}
/* sensors combined updated */
@@ -372,6 +357,7 @@ Navigator::task_main()
home_position_update();
}
/* vehicle_command updated */
orb_check(_vehicle_command_sub, &updated);
if (updated) {
@@ -509,9 +495,6 @@ Navigator::task_main()
_mission.set_current_offboard_mission_index(cmd.param1);
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) {
warnx("navigator: got pause/continue command");
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
if (cmd.param2 > FLT_EPSILON) {
// XXX not differentiating ground and airspeed yet
@@ -680,29 +663,17 @@ Navigator::task_main()
}
orb_unsubscribe(_global_pos_sub);
_global_pos_sub = -1;
orb_unsubscribe(_local_pos_sub);
_local_pos_sub = -1;
orb_unsubscribe(_gps_pos_sub);
_gps_pos_sub = -1;
orb_unsubscribe(_sensor_combined_sub);
_sensor_combined_sub = -1;
orb_unsubscribe(_fw_pos_ctrl_status_sub);
_fw_pos_ctrl_status_sub = -1;
orb_unsubscribe(_vstatus_sub);
_vstatus_sub = -1;
orb_unsubscribe(_land_detected_sub);
_land_detected_sub = -1;
orb_unsubscribe(_home_pos_sub);
_home_pos_sub = -1;
orb_unsubscribe(_onboard_mission_sub);
_onboard_mission_sub = -1;
orb_unsubscribe(_offboard_mission_sub);
_offboard_mission_sub = -1;
orb_unsubscribe(_param_update_sub);
_param_update_sub = -1;
orb_unsubscribe(_vehicle_command_sub);
_vehicle_command_sub = -1;
PX4_INFO("exiting");
@@ -985,20 +956,6 @@ Navigator::publish_geofence_result()
}
}
void
Navigator::publish_att_sp()
{
/* lazily publish the attitude sp only once available */
if (_att_sp_pub != nullptr) {
/* publish att sp*/
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
/* advertise and publish */
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
void
Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
{