Navigator move get_time_inside and cleanup (#7062)

src/modules/navigator/mission.cpp

@@ -44,28 +44,20 @@
  * @author Sander Smeets <sander@droneslab.com>
  */
 
-#include <sys/types.h>
+#include "mission.h"
-#include <string.h>
+#include "navigator.h"
-#include <stdlib.h>
-#include <unistd.h>
-#include <float.h>
 
 #include <drivers/drv_hrt.h>
-
 #include <dataman/dataman.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <navigator/navigation.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
-#include "mission.h"
-#include "navigator.h"
-
 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
@@ -75,28 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_yawmode(this, "MIS_YAWMODE", false),
 	_param_force_vtol(this, "NAV_FORCE_VT", false),
 	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
-	_onboard_mission{},
+	_missionFeasibilityChecker()
-	_offboard_mission{},
-	_current_onboard_mission_index(-1),
-	_current_offboard_mission_index(-1),
-	_need_takeoff(true),
-	_mission_type(MISSION_TYPE_NONE),
-	_inited(false),
-	_home_inited(false),
-	_need_mission_reset(false),
-	_missionFeasibilityChecker(),
-	_min_current_sp_distance_xy(FLT_MAX),
-	_distance_current_previous(0.0f),
-	_work_item_type(WORK_ITEM_TYPE_DEFAULT)
 {
-	/* load initial params */
 	updateParams();
 }
 
-Mission::~Mission()
-{
-}
-
 void
 Mission::on_inactive()
 {
@@ -136,7 +111,7 @@ Mission::on_inactive()
 	} else {
 
 		/* load missions from storage */
-		mission_s mission_state;
+		mission_s mission_state = {};
 
 		dm_lock(DM_KEY_MISSION_STATE);
 
@@ -221,6 +196,7 @@ Mission::on_active()
 		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE && _param_altmode.get() == MISSION_ALTMODE_FOH) {
+
 		altitude_sp_foh_update();
 
 	} else {
@@ -230,7 +206,6 @@ Mission::on_active()
 		}
 	}
 
-
 	/* check if a cruise speed change has been commanded */
 	if (_mission_type != MISSION_TYPE_NONE) {
 		cruising_speed_sp_update();
@@ -251,10 +226,10 @@ Mission::on_active()
 
 		do_abort_landing();
 	}
-
 }
 
-bool Mission::set_current_offboard_mission_index(unsigned index)
+bool
+Mission::set_current_offboard_mission_index(unsigned index)
 {
 	if (index < _offboard_mission.count) {
 
@@ -275,7 +250,7 @@ bool Mission::set_current_offboard_mission_index(unsigned index)
 	return false;
 }
 
-unsigned
+int
 Mission::find_offboard_land_start()
 {
 	/* find the first MAV_CMD_DO_LAND_START and return the index
@@ -433,11 +408,11 @@ Mission::advance_mission()
 float
 Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
 {
-	if (_mission_item.altitude_is_relative) {
+	if (mission_item.altitude_is_relative) {
-		return _mission_item.altitude + _navigator->get_home_position()->alt;
+		return mission_item.altitude + _navigator->get_home_position()->alt;
 
 	} else {
-		return _mission_item.altitude;
+		return mission_item.altitude;
 	}
 }
 
@@ -544,6 +519,7 @@ Mission::set_mission_items()
 		if (_mission_item.nav_cmd == NAV_CMD_LAND && _param_force_vtol.get() == 1
 		    && !_navigator->get_vstatus()->is_rotary_wing
 		    && _navigator->get_vstatus()->is_vtol) {
+
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
 
@@ -825,7 +801,7 @@ Mission::set_mission_items()
 		set_current_offboard_mission_item();
 	}
 
-	if (_mission_item.autocontinue && Navigator::get_time_inside(_mission_item) < FLT_EPSILON) {
+	if (_mission_item.autocontinue && get_time_inside(_mission_item) < FLT_EPSILON) {
 		/* try to process next mission item */
 		if (has_next_position_item) {
 			/* got next mission item, update setpoint triplet */
@@ -994,7 +970,7 @@ Mission::heading_sp_update()
 	}
 
 	/* set yaw angle for the waypoint if a loiter time has been specified */
-	if (_waypoint_position_reached && Navigator::get_time_inside(_mission_item) > FLT_EPSILON) {
+	if (_waypoint_position_reached && get_time_inside(_mission_item) > FLT_EPSILON) {
 		// XXX: should actually be param4 from mission item
 		// at the moment it will just keep the heading it has
 		//_mission_item.yaw = _on_arrival_yaw;
@@ -1054,7 +1030,6 @@ Mission::heading_sp_update()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
-
 void
 Mission::altitude_sp_foh_update()
 {
@@ -1313,7 +1288,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
 void
 Mission::save_offboard_mission_state()
 {
-	mission_s mission_state;
+	mission_s mission_state = {};
 
 	/* lock MISSION_STATE item */
 	dm_lock(DM_KEY_MISSION_STATE);

src/modules/navigator/mission.h

@@ -45,24 +45,23 @@
 #ifndef NAVIGATOR_MISSION_H
 #define NAVIGATOR_MISSION_H
 
-#include <drivers/drv_hrt.h>
-
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-
-#include <dataman/dataman.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
-
-#include "navigator_mode.h"
 #include "mission_block.h"
 #include "mission_feasibility_checker.h"
+#include "navigator_mode.h"
+
+#include <cfloat>
+
+#include <controllib/block/BlockParam.hpp>
+#include <controllib/blocks.hpp>
+#include <dataman/dataman.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/uORB.h>
 
 class Navigator;
 
@@ -70,14 +69,11 @@ class Mission : public MissionBlock
 {
 public:
 	Mission(Navigator *navigator, const char *name);
+	~Mission() override = default;
 
-	virtual ~Mission();
+	void on_inactive() override;
-
+	void on_activation() override;
-	virtual void on_inactive();
+	void on_active() override;
-
-	virtual void on_activation();
-
-	virtual void on_active();
 
 	enum mission_altitude_mode {
 		MISSION_ALTMODE_ZOH = 0,
@@ -94,7 +90,7 @@ public:
 
 	bool set_current_offboard_mission_index(unsigned index);
 
-	unsigned find_offboard_land_start();
+	int find_offboard_land_start();
 
 private:
 	/**
@@ -245,28 +241,28 @@ private:
 	control::BlockParamInt _param_force_vtol;
 	control::BlockParamFloat _param_fw_climbout_diff;
 
-	struct mission_s _onboard_mission;
+	struct mission_s _onboard_mission {};
-	struct mission_s _offboard_mission;
+	struct mission_s _offboard_mission {};
 
-	int _current_onboard_mission_index;
+	int _current_onboard_mission_index{-1};
-	int _current_offboard_mission_index;
+	int _current_offboard_mission_index{-1};
-	bool _need_takeoff;					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
+	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_ONBOARD,
 		MISSION_TYPE_OFFBOARD
-	} _mission_type;
+	} _mission_type{MISSION_TYPE_NONE};
 
-	bool _inited;
+	bool _inited{false};
-	bool _home_inited;
+	bool _home_inited{false};
-	bool _need_mission_reset;
+	bool _need_mission_reset{false};
 
 	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */
 
-	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
+	float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint  */
 
-	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
+	float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet,
 					    only use if current and previous are valid */
 
 	enum work_item_type {
@@ -274,11 +270,10 @@ private:
 		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
 		WORK_ITEM_TYPE_MOVE_TO_LAND,	/**< move to land waypoint before descent */
 		WORK_ITEM_TYPE_ALIGN,		/**< align for next waypoint */
-		WORK_ITEM_TYPE_CMD_BEFORE_MOVE,	/**<  */
+		WORK_ITEM_TYPE_CMD_BEFORE_MOVE,
-		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,	/**<  */
+		WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF,
-		WORK_ITEM_TYPE_TRANSITON_BEFORE_LAND,	/**<  */
+		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION
-		WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION	/**<  */
+	} _work_item_type{WORK_ITEM_TYPE_DEFAULT};	/**< current type of work to do (sub mission item) */
-	} _work_item_type;	/**< current type of work to do (sub mission item) */
 };
 
 #endif

src/modules/navigator/mission_block.cpp

@@ -40,41 +40,24 @@
  * @author Andreas Antener <andreas@uaventure.com>
  */
 
-#include <sys/types.h>
+#include "mission_block.h"
-#include <string.h>
+#include "navigator.h"
-#include <stdlib.h>
+
-#include <unistd.h>
 #include <math.h>
 #include <float.h>
 
-#include <systemlib/err.h>
 #include <geo/geo.h>
 #include <systemlib/mavlink_log.h>
 #include <mathlib/mathlib.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
-#include "navigator.h"
-#include "mission_block.h"
-
-actuator_controls_s actuators;
-orb_advert_t actuator_pub_fd;
 
 
 MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
-	_mission_item{},
-	_waypoint_position_reached(false),
-	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0),
-	_action_start(0),
-	_time_wp_reached(0),
-	_actuators{},
-	_actuator_pub(nullptr),
-	_cmd_pub(nullptr),
 	_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
 	_param_yaw_timeout(this, "MIS_YAW_TMT", false),
 	_param_yaw_err(this, "MIS_YAW_ERR", false),
@@ -86,10 +69,6 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 {
 }
 
-MissionBlock::~MissionBlock()
-{
-}
-
 bool
 MissionBlock::is_mission_item_reached()
 {
@@ -125,6 +104,7 @@ MissionBlock::is_mission_item_reached()
 		 * We wait half a second to give the transition command time to propagate.
 		 * Then monitor the transition status for completion.
 		 */
+		// TODO: check desired transition state achieved and drop _action_start
 		if (hrt_absolute_time() - _action_start > 500000 &&
 		    !_navigator->get_vstatus()->in_transition_mode) {
 
@@ -145,8 +125,7 @@ MissionBlock::is_mission_item_reached()
 
 	hrt_abstime now = hrt_absolute_time();
 
-	if ((_navigator->get_land_detected()->landed == false)
+	if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
-	    && !_waypoint_position_reached) {
 
 		float dist = -1.0f;
 		float dist_xy = -1.0f;
@@ -375,8 +354,8 @@ MissionBlock::is_mission_item_reached()
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
-		if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) ||
+		if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
-		    (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) {
+		    (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
 
 			// exit xtrack location
 			if (_mission_item.loiter_exit_xtrack &&
@@ -459,17 +438,17 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 	if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
 		PX4_INFO("do_set_servo command");
 		// XXX: we should issue a vehicle command and handle this somewhere else
-		memset(&actuators, 0, sizeof(actuators));
+		_actuators = {};
 		// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
 		// params[1] new value for selected actuator in ms 900...2000
-		actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
+		_actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
-		actuators.timestamp = hrt_absolute_time();
+		_actuators.timestamp = hrt_absolute_time();
 
 		if (_actuator_pub != nullptr) {
-			orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
+			orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
 
 		} else {
-			_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
+			_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
 		}
 
 	} else {
@@ -478,15 +457,20 @@ MissionBlock::issue_command(const struct mission_item_s *item)
 		mission_item_to_vehicle_command(item, &cmd);
 		_action_start = hrt_absolute_time();
 
-		if (_cmd_pub != nullptr) {
+		_navigator->publish_vehicle_cmd(cmd);
-			orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
-
-		} else {
-			_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-		}
 	}
 }
 
+float
+MissionBlock::get_time_inside(const struct mission_item_s &item)
+{
+	if (item.nav_cmd == NAV_CMD_TAKEOFF) {
+		return item.time_inside;
+	}
+
+	return 0.0f;
+}
+
 bool
 MissionBlock::item_contains_position(const struct mission_item_s *item)
 {
@@ -663,7 +647,6 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
 		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
 
 		/* use current target position */
-
 		item->lat = target.lat;
 		item->lon = target.lon;
 		item->altitude = _navigator->get_home_position()->alt;
@@ -709,18 +692,15 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 {
 
 	/* VTOL transition to RW before landing */
-	if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing &&
+	if (_navigator->get_vstatus()->is_vtol &&
+	    !_navigator->get_vstatus()->is_rotary_wing &&
 	    _param_force_vtol.get() == 1) {
+
 		struct vehicle_command_s cmd = {};
 		cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
 		cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
-		if (_cmd_pub != nullptr) {
+		_navigator->publish_vehicle_cmd(cmd);
-			orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
-
-		} else {
-			_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
-		}
 	}
 
 	/* set the land item */
@@ -732,9 +712,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 		item->lon = _navigator->get_global_position()->lon;
 		item->yaw = _navigator->get_local_position()->yaw;
 
-		/* use home position */
-
 	} else {
+		/* use home position */
 		item->lat = _navigator->get_home_position()->lat;
 		item->lon = _navigator->get_home_position()->lon;
 		item->yaw = _navigator->get_home_position()->yaw;

src/modules/navigator/mission_block.h

@@ -41,18 +41,16 @@
 #ifndef NAVIGATOR_MISSION_BLOCK_H
 #define NAVIGATOR_MISSION_BLOCK_H
 
-#include <drivers/drv_hrt.h>
+#include "navigator_mode.h"
 
 #include <navigator/navigation.h>
-
-#include <uORB/topics/mission.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/follow_target.h>
-
+#include <uORB/topics/mission.h>
-#include "navigator_mode.h"
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 
 class Navigator;
 
@@ -63,15 +61,11 @@ public:
 	 * Constructor
 	 */
 	MissionBlock(Navigator *navigator, const char *name);
+	~MissionBlock() = default;
 
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
-	/**
-	 * Destructor
-	 */
-	virtual ~MissionBlock();
-
 	/* TODO: move this to a helper class in navigator */
 	static bool item_contains_position(const struct mission_item_s *item);
 
@@ -81,6 +75,7 @@ protected:
 	 * @return true if successfully reached
 	 */
 	bool is_mission_item_reached();
+
 	/**
 	 * Reset all reached flags
 	 */
@@ -121,28 +116,31 @@ protected:
 	 */
 	void set_idle_item(struct mission_item_s *item);
 
-	/**
-	 * Convert a mission item to a command
-	 */
-	void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
-
 	/**
 	 * Set follow_target item
 	 */
 	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
 
+	/**
+	 * Convert a mission item to a command
+	 */
+	void mission_item_to_vehicle_command(const struct mission_item_s *item, struct vehicle_command_s *cmd);
+
 	void issue_command(const struct mission_item_s *item);
 
-	mission_item_s _mission_item;
+	float get_time_inside(const struct mission_item_s &item);
-	bool _waypoint_position_reached;
-	bool _waypoint_yaw_reached;
-	hrt_abstime _time_first_inside_orbit;
-	hrt_abstime _action_start;
-	hrt_abstime _time_wp_reached;
 
-	actuator_controls_s _actuators;
+	mission_item_s _mission_item{};
-	orb_advert_t    _actuator_pub;
+
-	orb_advert_t	_cmd_pub;
+	bool _waypoint_position_reached{false};
+	bool _waypoint_yaw_reached{false};
+
+	hrt_abstime _time_first_inside_orbit{0};
+	hrt_abstime _action_start{0};
+	hrt_abstime _time_wp_reached{0};
+
+	actuator_controls_s _actuators{};
+	orb_advert_t    _actuator_pub{nullptr};
 
 	control::BlockParamFloat _param_loiter_min_alt;
 	control::BlockParamFloat _param_yaw_timeout;

src/modules/navigator/navigator.h

@@ -1,6 +1,6 @@
 /***************************************************************************
  *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,37 +41,34 @@
 #ifndef NAVIGATOR_H
 #define NAVIGATOR_H
 
-#include <systemlib/perf_counter.h>
-
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-#include <navigator/navigation.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/mission.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/geofence_result.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_land_detected.h>
-
-#include "navigator_mode.h"
-#include "mission.h"
-#include "loiter.h"
-#include "takeoff.h"
-#include "land.h"
-#include "rtl.h"
 #include "datalinkloss.h"
 #include "enginefailure.h"
 #include "follow_target.h"
-#include "gpsfailure.h"
-#include "rcloss.h"
 #include "geofence.h"
+#include "gpsfailure.h"
+#include "land.h"
+#include "loiter.h"
+#include "mission.h"
+#include "navigator_mode.h"
+#include "rcloss.h"
+#include "rtl.h"
+#include "takeoff.h"
+
+#include <controllib/block/BlockParam.hpp>
+#include <controllib/blocks.hpp>
+#include <navigator/navigation.h>
+#include <systemlib/perf_counter.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
+#include <uORB/topics/geofence_result.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/uORB.h>
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
@@ -81,15 +78,10 @@
 class Navigator : public control::SuperBlock
 {
 public:
-	/**
-	 * Constructor
-	 */
 	Navigator();
-
-	/**
-	 * Destructor, also kills the navigators task.
-	 */
 	~Navigator();
+	Navigator(const Navigator &) = delete;
+	Navigator operator=(const Navigator &) = delete;
 
 	/**
 	 * Start the navigator task.
@@ -124,6 +116,8 @@ public:
 	 */
 	void		publish_att_sp();
 
+	void		publish_vehicle_cmd(const struct vehicle_command_s &vcmd);
+
 	/**
 	 * Setters
 	 */
@@ -134,21 +128,20 @@ public:
 	/**
 	 * Getters
 	 */
-	struct vehicle_status_s *get_vstatus() { return &_vstatus; }
+	struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &_fw_pos_ctrl_status; }
-	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
-	struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; }
-	struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
-	struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
-	struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
-	struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s *get_home_position() { return &_home_pos; }
-	bool home_position_valid() { return (_home_pos.timestamp > 0); }
+	struct mission_result_s *get_mission_result() { return &_mission_result; }
 	struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; }
 	struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; }
-	struct mission_result_s *get_mission_result() { return &_mission_result; }
-	struct geofence_result_s *get_geofence_result() { return &_geofence_result; }
 	struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; }
+	struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
+	struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
+	struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
+	struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
+	struct vehicle_status_s *get_vstatus() { return &_vstatus; }
+
+	bool home_position_valid() { return (_home_pos.timestamp > 0); }
 
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
@@ -188,7 +181,7 @@ public:
 	 * Passing a negative value or leaving the parameter away will reset the cruising speed
 	 * to its default value.
 	 *
-	 * For VTOL: sets cuising speed for current mode only (multirotor or fixed-wing).
+	 * For VTOL: sets cruising speed for current mode only (multirotor or fixed-wing).
 	 *
 	 */
 	void		set_cruising_speed(float speed = -1.0f);
@@ -230,83 +223,70 @@ public:
 
 	bool		abort_landing();
 
-	static float		get_time_inside(struct mission_item_s &item) { return (item.nav_cmd == NAV_CMD_TAKEOFF) ? 0.0f : item.time_inside; }
-
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_should_exit{false};	/**< if true, sensor task should exit */
-	int		_navigator_task;		/**< task handle for sensor task */
+	int		_navigator_task{-1};		/**< task handle for sensor task */
 
-	orb_advert_t	_mavlink_log_pub;		/**< the uORB advert to send messages over mavlink */
+	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
 
-	int		_global_pos_sub;		/**< global position subscription */
+	int		_fw_pos_ctrl_status_sub{-1};	/**< notification of vehicle capabilities updates */
-	int		_local_pos_sub;		/**< local position subscription */
+	int		_global_pos_sub{-1};		/**< global position subscription */
-	int		_gps_pos_sub;		/**< gps position subscription */
+	int		_gps_pos_sub{-1};		/**< gps position subscription */
-	int		_sensor_combined_sub;		/**< sensor combined subscription */
+	int		_home_pos_sub{-1};		/**< home position subscription */
-	int		_home_pos_sub;			/**< home position subscription */
+	int		_land_detected_sub{-1};		/**< vehicle land detected subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_local_pos_sub{-1};		/**< local position subscription */
-	int		_land_detected_sub;		/**< vehicle land detected subscription */
+	int		_offboard_mission_sub{-1};	/**< offboard mission subscription */
-	int		_fw_pos_ctrl_status_sub;		/**< notification of vehicle capabilities updates */
+	int		_onboard_mission_sub{-1};	/**< onboard mission subscription */
-	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int		_param_update_sub{-1};		/**< param update subscription */
-	int		_onboard_mission_sub;		/**< onboard mission subscription */
+	int		_sensor_combined_sub{-1};	/**< sensor combined subscription */
-	int		_offboard_mission_sub;		/**< offboard mission subscription */
+	int		_vehicle_command_sub{-1};	/**< vehicle commands (onboard and offboard) */
-	int		_param_update_sub;		/**< param update subscription */
+	int		_vstatus_sub{-1};		/**< vehicle status subscription */
-	int		_vehicle_command_sub;		/**< vehicle commands (onboard and offboard) */
 
-	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
+	orb_advert_t	_att_sp_pub{nullptr};
-	orb_advert_t	_mission_result_pub;
+	orb_advert_t	_geofence_result_pub{nullptr};
-	orb_advert_t	_geofence_result_pub;
+	orb_advert_t	_mission_result_pub{nullptr};
-	orb_advert_t	_att_sp_pub;			/**< publish att sp
+	orb_advert_t	_pos_sp_triplet_pub{nullptr};
-							  used only in very special failsafe modes
+	orb_advert_t	_vehicle_cmd_pub{nullptr};
-							  when pos control is deactivated */
 
-	vehicle_status_s				_vstatus;		/**< vehicle status */
+	fw_pos_ctrl_status_s				_fw_pos_ctrl_status{};	/**< fixed wing navigation capabilities */
-	vehicle_land_detected_s				_land_detected;		/**< vehicle land_detected */
+	geofence_result_s				_geofence_result{};
-	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
+	home_position_s					_home_pos{};		/**< home position for RTL */
-	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	mission_result_s				_mission_result{};
+	position_setpoint_triplet_s			_pos_sp_triplet{};	/**< triplet of position setpoints */
+	position_setpoint_triplet_s			_reposition_triplet{};	/**< triplet for non-mission direct position command */
+	position_setpoint_triplet_s			_takeoff_triplet{};	/**< triplet for non-mission direct takeoff command */
+	sensor_combined_s				_sensor_combined{};	/**< sensor values */
+	vehicle_attitude_setpoint_s			_att_sp{};
+	vehicle_global_position_s			_global_pos{};		/**< global vehicle position */
+	vehicle_gps_position_s				_gps_pos{};		/**< gps position */
+	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
 	vehicle_local_position_s			_local_pos;		/**< local vehicle position */
-	vehicle_gps_position_s				_gps_pos;		/**< gps position */
+	vehicle_status_s				_vstatus{};		/**< vehicle status */
-	sensor_combined_s				_sensor_combined;	/**< sensor values */
-	home_position_s					_home_pos;		/**< home position for RTL */
-	mission_item_s 					_mission_item;		/**< current mission item */
-	fw_pos_ctrl_status_s			_fw_pos_ctrl_status;		/**< fixed wing navigation capabilities */
-	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
-	position_setpoint_triplet_s			_reposition_triplet;	/**< triplet for non-mission direct position command */
-	position_setpoint_triplet_s			_takeoff_triplet;	/**< triplet for non-mission direct takeoff command */
 
-	mission_result_s				_mission_result;
+	int		_mission_instance_count{-1};	/**< instance count for the current mission */
-	geofence_result_s				_geofence_result;
-	vehicle_attitude_setpoint_s			_att_sp;
-
-	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
-	int		_mission_instance_count;	/**< instance count for the current mission */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	Geofence	_geofence;			/**< class that handles the geofence */
-	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
+	bool		_geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */
 
-	bool		_inside_fence;			/**< vehicle is inside fence */
+	bool		_can_loiter_at_sp{false};			/**< flags if current position SP can be used to loiter */
+	bool		_pos_sp_triplet_updated{false};		/**< flags if position SP triplet needs to be published */
+	bool 		_pos_sp_triplet_published_invalid_once{false};	/**< flags if position SP triplet has been published once to UORB */
+	bool		_mission_result_updated{false};		/**< flags if mission result has seen an update */
 
-	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
+	NavigatorMode	*_navigation_mode{nullptr};		/**< abstract pointer to current navigation mode class */
-	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
-	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
-	bool		_mission_result_updated;		/**< flags if mission result has seen an update */
-
-	NavigatorMode	*_navigation_mode;		/**< abstract pointer to current navigation mode class */
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
 	Land		_land;			/**< class for handling land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
-	RCLoss 		_rcLoss;				/**< class that handles RTL according to
+	RCLoss 		_rcLoss;				/**< class that handles RTL according to OBC rules (rc loss mode) */
-							  OBC rules (rc loss mode) */
 	DataLinkLoss	_dataLinkLoss;			/**< class that handles the OBC datalink loss mode */
-	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode
+	EngineFailure	_engineFailure;			/**< class that handles the engine failure mode (FW only!) */
-							  (FW only!) */
 	GpsFailure	_gpsFailure;			/**< class that handles the OBC gpsfailure loss mode */
-
+	FollowTarget	_follow_target;
-	FollowTarget _follow_target;
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
@@ -319,59 +299,20 @@ private:
 	control::BlockParamFloat _param_cruising_speed_plane;
 	control::BlockParamFloat _param_cruising_throttle_plane;
 
-	float _mission_cruising_speed_mc;
+	float _mission_cruising_speed_mc{-1.0f};
-	float _mission_cruising_speed_fw;
+	float _mission_cruising_speed_fw{-1.0f};
-	float _mission_throttle;
+	float _mission_throttle{-1.0f};
 
-	/**
+	// update subscriptions
-	 * Retrieve global position
-	 */
-	void		global_position_update();
-
-	/**
-	 * Retrieve local position
-	 */
-	void		local_position_update();
-
-	/**
-	 * Retrieve gps position
-	 */
-	void		gps_position_update();
-
-	/**
-	 * Retrieve sensor values
-	 */
-	void		sensor_combined_update();
-
-	/**
-	 * Retrieve home position
-	 */
-	void		home_position_update(bool force = false);
-
-	/**
-	 * Retrieve fixed wing navigation capabilities
-	 */
 	void		fw_pos_ctrl_status_update();
-
+	void		global_position_update();
-	/**
+	void		gps_position_update();
-	 * Retrieve vehicle status
+	void		home_position_update(bool force = false);
-	 */
+	void		local_position_update();
-	void		vehicle_status_update();
-
-	/**
-	 * Retrieve vehicle land detected
-	 */
-	void		vehicle_land_detected_update();
-
-	/**
-	 * Retrieve vehicle control mode
-	 */
-	void		vehicle_control_mode_update();
-
-	/**
-	 * Update parameters
-	 */
 	void		params_update();
+	void		sensor_combined_update();
+	void		vehicle_land_detected_update();
+	void		vehicle_status_update();
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -393,16 +334,9 @@ private:
 	 */
 	void		publish_position_setpoint_triplet();
 
-
 	/**
 	 * Publish the mission result so commander and mavlink know what is going on
 	 */
 	void		publish_mission_result();
-
-	/* this class has ptr data members, so it should not be copied,
-	 * consequently the copy constructors are private.
-	 */
-	Navigator(const Navigator &);
-	Navigator operator=(const Navigator &);
 };
 #endif

src/modules/navigator/navigator_main.cpp

@@ -47,9 +47,7 @@
 
 #include <cfloat>
 
-#include <arch/board/board.h>
 #include <dataman/dataman.h>
-#include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <geo/geo.h>
 #include <mathlib/mathlib.h>
@@ -60,7 +58,6 @@
 #include <sys/ioctl.h>
 #include <sys/stat.h>
 #include <sys/types.h>
-#include <systemlib/err.h>
 #include <systemlib/mavlink_log.h>
 #include <systemlib/systemlib.h>
 #include <uORB/topics/fence.h>
@@ -82,57 +79,13 @@ extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
 namespace navigator
 {
-
 Navigator	*g_navigator;
 }
 
 Navigator::Navigator() :
 	SuperBlock(nullptr, "NAV"),
-	_task_should_exit(false),
-	_navigator_task(-1),
-	_mavlink_log_pub(nullptr),
-	_global_pos_sub(-1),
-	_local_pos_sub(-1),
-	_gps_pos_sub(-1),
-	_sensor_combined_sub(-1),
-	_home_pos_sub(-1),
-	_vstatus_sub(-1),
-	_land_detected_sub(-1),
-	_fw_pos_ctrl_status_sub(-1),
-	_control_mode_sub(-1),
-	_onboard_mission_sub(-1),
-	_offboard_mission_sub(-1),
-	_param_update_sub(-1),
-	_vehicle_command_sub(-1),
-	_pos_sp_triplet_pub(nullptr),
-	_mission_result_pub(nullptr),
-	_geofence_result_pub(nullptr),
-	_att_sp_pub(nullptr),
-	_vstatus{},
-	_land_detected{},
-	_control_mode{},
-	_global_pos{},
-	_gps_pos{},
-	_sensor_combined{},
-	_home_pos{},
-	_mission_item{},
-	_fw_pos_ctrl_status{},
-	_pos_sp_triplet{},
-	_reposition_triplet{},
-	_takeoff_triplet{},
-	_mission_result{},
-	_att_sp{},
-	_mission_item_valid(false),
-	_mission_instance_count(0),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence(this),
-	_geofence_violation_warning_sent(false),
-	_inside_fence(true),
-	_can_loiter_at_sp(false),
-	_pos_sp_triplet_updated(false),
-	_pos_sp_triplet_published_invalid_once(false),
-	_mission_result_updated(false),
-	_navigation_mode(nullptr),
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_takeoff(this, "TKF"),
@@ -149,10 +102,7 @@ Navigator::Navigator() :
 	_param_mc_alt_acceptance_radius(this, "MC_ALT_RAD"),
 	_param_cruising_speed_hover(this, "MPC_XY_CRUISE", false),
 	_param_cruising_speed_plane(this, "FW_AIRSPD_TRIM", false),
-	_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false),
+	_param_cruising_throttle_plane(this, "FW_THR_CRUISE", false)
-	_mission_cruising_speed_mc(-1.0f),
-	_mission_cruising_speed_fw(-1.0f),
-	_mission_throttle(-1.0f)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -232,7 +182,12 @@ Navigator::home_position_update(bool force)
 void
 Navigator::fw_pos_ctrl_status_update()
 {
-	orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
+	bool updated = false;
+	orb_check(_fw_pos_ctrl_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
+	}
 }
 
 void
@@ -250,16 +205,6 @@ Navigator::vehicle_land_detected_update()
 	orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
 }
 
-void
-Navigator::vehicle_control_mode_update()
-{
-	if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
-		/* in case the commander is not be running */
-		_control_mode.flag_control_auto_enabled = false;
-		_control_mode.flag_armed = false;
-	}
-}
-
 void
 Navigator::params_update()
 {
@@ -306,7 +251,6 @@ Navigator::task_main()
 	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
-	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -316,7 +260,6 @@ Navigator::task_main()
 	/* copy all topics first time */
 	vehicle_status_update();
 	vehicle_land_detected_update();
-	vehicle_control_mode_update();
 	global_position_update();
 	local_position_update();
 	gps_position_update();
@@ -407,13 +350,6 @@ Navigator::task_main()
 			params_update();
 		}
 
-		/* vehicle control mode updated */
-		orb_check(_control_mode_sub, &updated);
-
-		if (updated) {
-			vehicle_control_mode_update();
-		}
-
 		/* vehicle status updated */
 		orb_check(_vstatus_sub, &updated);
 
@@ -445,7 +381,7 @@ Navigator::task_main()
 		orb_check(_vehicle_command_sub, &updated);
 
 		if (updated) {
-			vehicle_command_s cmd;
+			vehicle_command_s cmd = {};
 			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);
 
 			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
@@ -532,7 +468,7 @@ Navigator::task_main()
 				/* find NAV_CMD_DO_LAND_START in the mission and
 				 * use MAV_CMD_MISSION_START to start the mission there
 				 */
-				unsigned land_start = _mission.find_offboard_land_start();
+				int land_start = _mission.find_offboard_land_start();
 
 				if (land_start != -1) {
 					vehicle_command_s vcmd = {};
@@ -542,8 +478,10 @@ Navigator::task_main()
 					vcmd.param1 = land_start;
 					vcmd.param2 = 0;
 
-					orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+					publish_vehicle_cmd(vcmd);
-					(void)orb_unadvertise(pub);
+
+				} else {
+					PX4_WARN("planned landing not available");
 				}
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) {
@@ -598,12 +536,12 @@ Navigator::task_main()
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 
+			_geofence_result.timestamp = hrt_absolute_time();
 			_geofence_result.geofence_action = _geofence.getGeofenceAction();
 
 			if (!inside) {
 				/* inform other apps via the mission result */
 				_geofence_result.geofence_violated = true;
-				publish_geofence_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -614,10 +552,12 @@ Navigator::task_main()
 			} else {
 				/* inform other apps via the mission result */
 				_geofence_result.geofence_violated = false;
-				publish_geofence_result();
+
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
+
+			publish_geofence_result();
 		}
 
 		/* Do stuff according to navigation state set by commander */
@@ -748,24 +688,8 @@ Navigator::start()
 void
 Navigator::status()
 {
-	/* TODO: add this again */
-	// PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in");
-
-	// if (_global_pos.global_valid) {
-	// 	PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
-	// 	PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters",
-	// 	      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
-	// 	PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
-	// 	      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
-	// 	PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
-	// }
-
 	if (_geofence.valid()) {
 		PX4_INFO("Geofence is valid");
-		/* TODO: needed? */
-//		PX4_INFO("Vertex longitude latitude");
-//		for (unsigned i = 0; i < _fence.count; i++)
-//		PX4_INFO("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
 
 	} else {
 		PX4_INFO("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
@@ -804,7 +728,7 @@ Navigator::get_acceptance_radius()
 float
 Navigator::get_altitude_acceptance_radius()
 {
-	if (!this->get_vstatus()->is_rotary_wing) {
+	if (!get_vstatus()->is_rotary_wing) {
 		return _param_fw_alt_acceptance_radius.get();
 
 	} else {
@@ -817,8 +741,7 @@ Navigator::get_cruising_speed()
 {
 	/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
 	if (_vstatus.is_rotary_wing) {
-		if (_mission_cruising_speed_mc > 0.0f
+		if (is_planned_mission() && _mission_cruising_speed_mc > 0.0f) {
-		    && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
 			return _mission_cruising_speed_mc;
 
 		} else {
@@ -826,8 +749,7 @@ Navigator::get_cruising_speed()
 		}
 
 	} else {
-		if (_mission_cruising_speed_fw > 0.0f
+		if (is_planned_mission() && _mission_cruising_speed_fw > 0.0f) {
-		    && _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
 			return _mission_cruising_speed_fw;
 
 		} else {
@@ -1008,7 +930,6 @@ Navigator::publish_mission_result()
 void
 Navigator::publish_geofence_result()
 {
-
 	/* lazily publish the geofence result only once available */
 	if (_geofence_result_pub != nullptr) {
 		/* publish mission result */
@@ -1034,6 +955,17 @@ Navigator::publish_att_sp()
 	}
 }
 
+void
+Navigator::publish_vehicle_cmd(const struct vehicle_command_s &vcmd)
+{
+	if (_vehicle_cmd_pub != nullptr) {
+		orb_publish(ORB_ID(vehicle_command), _vehicle_cmd_pub, &vcmd);
+
+	} else {
+		_vehicle_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH);
+	}
+}
+
 void
 Navigator::set_mission_failure(const char *reason)
 {

src/modules/navigator/navigator_mode.cpp

@@ -53,10 +53,6 @@ NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
 	on_inactive();
 }
 
-NavigatorMode::~NavigatorMode()
-{
-}
-
 void
 NavigatorMode::run(bool active)
 {

src/modules/navigator/navigator_mode.h

@@ -56,15 +56,10 @@ class Navigator;
 class NavigatorMode : public control::SuperBlock
 {
 public:
-	/**
-	 * Constructor
-	 */
 	NavigatorMode(Navigator *navigator, const char *name);
-
+	virtual ~NavigatorMode() = default;
-	/**
+	NavigatorMode(const NavigatorMode &) = delete;
-	 * Destructor
+	NavigatorMode operator=(const NavigatorMode &) = delete;
-	 */
-	virtual ~NavigatorMode();
 
 	void run(bool active);
 
@@ -84,16 +79,10 @@ public:
 	virtual void on_active();
 
 protected:
-	Navigator *_navigator;
+	Navigator *_navigator{nullptr};
 
 private:
-	bool _first_run;
+	bool _first_run{true};
-
-	/* this class has ptr data members, so it should not be copied,
-	 * consequently the copy constructors are private.
-	 */
-	NavigatorMode(const NavigatorMode &);
-	NavigatorMode operator=(const NavigatorMode &);
 };
 
 #endif

src/modules/navigator/rtl.cpp

@@ -268,9 +268,9 @@ RTL::set_rtl_item()
 
 			_navigator->set_can_loiter_at_sp(true);
 
-			if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
+			if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
-						 (double)Navigator::get_time_inside(_mission_item));
+						 (double)get_time_inside(_mission_item));
 
 			} else {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");