diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 577c767a86..f59663ca00 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -154,17 +154,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 +177,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; @@ -382,11 +382,11 @@ 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), @@ -395,22 +395,22 @@ Navigator::Navigator() : /* 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"); @@ -1162,7 +1162,7 @@ Navigator::set_mission_item() } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); } else { if (onboard) { @@ -1318,7 +1318,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); break; } @@ -1344,7 +1344,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1362,7 +1362,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; @@ -1371,7 +1371,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); break; } @@ -1525,7 +1525,7 @@ Navigator::check_mission_item_reached() _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } }