diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 68 |
1 files changed, 36 insertions, 32 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1c040f1ff..ff1932c66 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> @@ -154,17 +153,16 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ int _control_mode_sub; /**< vehicle control mode subscription */ - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ - bool _mission_item_valid; /**< current mission item valid */ + struct mission_item_s _mission_item; /**< current mission item */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -178,21 +176,22 @@ private: class Mission _mission; - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ + bool _mission_item_valid; /**< current mission item valid */ + bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ bool _pos_sp_triplet_updated; - char *nav_states_str[NAV_STATE_MAX]; + const char *nav_states_str[NAV_STATE_MAX]; struct { float min_altitude; @@ -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); @@ -387,35 +386,34 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _control_mode_sub(-1), /* publications */ _pos_sp_triplet_pub(-1), - _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), -/* states */ - _rtl_state(RTL_STATE_NONE), + _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _mission_item_valid(false), _global_pos_valid(false), _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0), - _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), + _set_nav_state_timestamp(0), _pos_sp_triplet_updated(false), - _geofence_violation_warning_sent(false) +/* states */ + _rtl_state(RTL_STATE_NONE) { _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); @@ -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); } } @@ -1167,7 +1167,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); } else { if (onboard) { @@ -1326,7 +1326,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } @@ -1359,7 +1359,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1377,7 +1377,7 @@ Navigator::set_rtl_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; _mission_item.origin = ORIGIN_ONBOARD; @@ -1386,7 +1386,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1552,7 +1552,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } } @@ -1581,6 +1581,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 +1630,7 @@ Navigator::on_mission_item_reached() mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); dispatch(EVENT_READY_REQUESTED); } + _mission.publish_mission_result(); } void |