diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 24 |
1 files changed, 15 insertions, 9 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 |