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

Mavlink and navigator: Disable some functions in mavlink that are taken over by navigator, introduce topic to report mission status from commander back to mavlink

Browse Source
This commit is contained in:
Julian Oes
2013-11-27 09:27:08 +01:00
parent 9c1a5be8e1
commit 3f25298798
8 changed files with 372 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+39
View File
@@ -58,6 +58,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission_item_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
@@ -144,11 +145,13 @@ private:
orb_advert_t _triplet_pub; /**< publish position setpoint triplet */
orb_advert_t _fence_pub; /**< publish fence topic */
orb_advert_t _mission_result_pub; /**< publish mission result topic */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct home_position_s _home_pos; /**< home position for RTL */
struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */
struct mission_result_s _mission_result; /**< mission result for commander/mavlink */
perf_counter_t _loop_perf; /**< loop performance counter */
@@ -215,12 +218,16 @@ private:
void publish_mission_item_triplet();
void publish_mission_result();
int advance_current_mission_item();
void reset_mission_item_reached();
void check_mission_item_reached();
void report_mission_reached();
void start_waypoint();
void start_loiter(mission_item_s *new_loiter_position);
@@ -266,6 +273,7 @@ Navigator::Navigator() :
/* publications */
_triplet_pub(-1),
_fence_pub(-1),
_mission_result_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -295,6 +303,8 @@ Navigator::Navigator() :
memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s));
memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s));
memset(&_mission_result, 0, sizeof(struct mission_result_s));
/* fetch initial values */
parameters_update();
}
@@ -586,6 +596,9 @@ Navigator::task_main()
check_mission_item_reached();
if (_mission_item_reached) {
report_mission_reached();
mavlink_log_info(_mavlink_fd, "[navigator] reached WP %d", _current_mission_index);
if (advance_current_mission_item() != OK) {
set_mode(NAVIGATION_MODE_LOITER_WAYPOINT);
@@ -896,6 +909,20 @@ Navigator::publish_mission_item_triplet()
}
}
void
Navigator::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish the mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
}
int
Navigator::advance_current_mission_item()
{
@@ -945,6 +972,9 @@ Navigator::reset_mission_item_reached()
_time_first_inside_orbit = 0;
_mission_item_reached = false;
_mission_result.mission_reached = false;
_mission_result.mission_index = 0;
}
void
@@ -1041,6 +1071,15 @@ Navigator::check_mission_item_reached()
}
void
Navigator::report_mission_reached()
{
_mission_result.mission_reached = true;
_mission_result.mission_index = _current_mission_index;
publish_mission_result();
}
void
Navigator::start_waypoint()
{