diff options
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 24 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mission.cpp | 65 | ||||
-rw-r--r-- | src/modules/navigator/navigator_mission.h | 11 |
3 files changed, 81 insertions, 19 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 16eca8d79..c45cafc1b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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); @@ -395,7 +394,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), - _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -427,7 +425,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)); @@ -529,13 +526,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 @@ -545,12 +545,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); } } @@ -1112,6 +1112,8 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + _mission.report_current_offboard_mission_item(); + _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); @@ -1581,6 +1583,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; @@ -1627,6 +1632,7 @@ Navigator::on_mission_item_reached() mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); dispatch(EVENT_READY_REQUESTED); } + _mission.publish_mission_result(); } void diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index e72caf98e..72dddebfe 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -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_offboard_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,35 @@ Mission::move_to_next() default: break; } -}
\ No newline at end of file +} + +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_offboard_mission_item() +{ + _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; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 15d4e86bf..2bd4da82e 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -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_offboard_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
\ No newline at end of file +#endif |