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

Mavlink and navigator: handle current waypoint onboard

Browse Source
This commit is contained in:
Julian Oes
2014-02-15 11:29:36 +01:00
parent d15fc32097
commit 30dacbd154
6 changed files with 142 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+16 -9
View File
@@ -65,7 +65,6 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
@@ -288,9 +287,9 @@ private:
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main sensor collection task.
* Main task.
*/
void task_main() __attribute__((noreturn));
void task_main();
void publish_safepoints(unsigned points);
@@ -384,7 +383,6 @@ Navigator::Navigator() :
/* publications */
_pos_sp_triplet_pub(-1),
_mission_result_pub(-1),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -416,7 +414,6 @@ Navigator::Navigator() :
_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
memset(&_mission_result, 0, sizeof(struct mission_result_s));
memset(&_mission_item, 0, sizeof(struct mission_item_s));
memset(&nav_states_str, 0, sizeof(nav_states_str));
@@ -518,13 +515,16 @@ Navigator::offboard_mission_update(bool isrotaryWing)
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
_mission.set_offboard_mission_count(offboard_mission.count);
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
} else {
_mission.set_current_offboard_mission_index(0);
_mission.set_offboard_mission_count(0);
_mission.set_current_offboard_mission_index(0);
}
_mission.publish_mission_result();
}
void
@@ -534,12 +534,12 @@ Navigator::onboard_mission_update()
if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
} else {
_mission.set_current_onboard_mission_index(0);
_mission.set_onboard_mission_count(0);
_mission.set_current_onboard_mission_index(0);
}
}
@@ -1116,6 +1116,9 @@ Navigator::set_mission_item()
ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
if (ret == OK) {
_mission.report_current_mission_item();
/* reset time counter for new item */
_time_first_inside_orbit = 0;
@@ -1537,6 +1540,9 @@ void
Navigator::on_mission_item_reached()
{
if (myState == NAV_STATE_MISSION) {
_mission.report_mission_item_reached();
if (_do_takeoff) {
/* takeoff completed */
_do_takeoff = false;
@@ -1589,6 +1595,7 @@ Navigator::on_mission_item_reached()
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
_mission.publish_mission_result();
}
void
src/modules/navigator/navigator_mission.cpp
+59 -8
View File
@@ -36,13 +36,12 @@
* Helper class to access missions
*/
// #include <stdio.h>
// #include <stdlib.h>
// #include <string.h>
// #include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
@@ -60,8 +59,11 @@ Mission::Mission() :
_offboard_mission_item_count(0),
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE)
{}
_current_mission_type(MISSION_TYPE_NONE),
_mission_result_pub(-1)
{
memset(&_mission_result, 0, sizeof(struct mission_result_s));
}
Mission::~Mission()
{
@@ -78,8 +80,16 @@ void
Mission::set_current_offboard_mission_index(int new_index)
{
if (new_index != -1) {
warnx("specifically set to %d", new_index);
_current_offboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= _offboard_mission_item_count) {
_current_offboard_mission_index = 0;
}
}
report_current_mission_item();
}
void
@@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= _onboard_mission_item_count) {
_current_onboard_mission_index = 0;
}
}
// TODO: implement this for onboard missions as well
// report_current_mission_item();
}
void
@@ -266,4 +284,37 @@ Mission::move_to_next()
default:
break;
}
}
}
void
Mission::report_mission_item_reached()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
}
void
Mission::report_current_mission_item()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.index_current_mission = _current_offboard_mission_index;
}
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish 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);
}
/* reset reached bool */
_mission_result.mission_reached = false;
}
src/modules/navigator/navigator_mission.h
+9 -2
View File
@@ -40,6 +40,7 @@
#define NAVIGATOR_MISSION_H
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
class __EXPORT Mission
@@ -71,7 +72,9 @@ public:
void move_to_next();
void add_home_pos(struct mission_item_s *new_mission_item);
void report_mission_item_reached();
void report_current_mission_item();
void publish_mission_result();
private:
bool current_onboard_mission_available();
@@ -92,6 +95,10 @@ private:
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
int _mission_result_pub;
struct mission_result_s _mission_result;
};
#endif
#endif