Merge pull request #2349 from PX4/auto_takeoff_fix

FW: Auto takeoff fix

src/modules/commander/commander.cpp

@@ -450,7 +450,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 	// For HIL platforms, require that simulated sensors are connected
-	if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+	if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL &&
+		is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
 		mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
 		return TRANSITION_DENIED;
 	}

src/modules/commander/state_machine_helper.cpp

@@ -375,7 +375,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 		switch (new_state) {
 		case vehicle_status_s::HIL_STATE_OFF:
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
+			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
 			ret = TRANSITION_DENIED;
 			break;
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1085,7 +1085,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+		if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+			_att_sp.thrust = 0.0f;
+			_att_sp.roll_body = 0.0f;
+			_att_sp.pitch_body = 0.0f;
+
+		} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
 			_att_sp.roll_body = _l1_control.nav_roll();
@@ -1544,6 +1549,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+			pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
+		_att_sp.thrust = 0.0f;
 	} else {
 		/* Copy thrust and pitch values from tecs */
 		_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :

src/modules/navigator/loiter.cpp

@@ -55,7 +55,8 @@
 #include "navigator.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	MissionBlock(navigator, name)
+	MissionBlock(navigator, name),
+	_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
 {
 	/* load initial params */
 	updateParams();
@@ -74,7 +75,7 @@ void
 Loiter::on_activation()
 {
 	/* set current mission item to loiter */
-	set_loiter_item(&_mission_item);
+	set_loiter_item(&_mission_item, _param_min_alt.get());
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

src/modules/navigator/loiter.h

@@ -59,6 +59,9 @@ public:
 	virtual void on_activation();
 
 	virtual void on_active();
+
+private:
+	control::BlockParamFloat _param_min_alt;
 };
 
 #endif

src/modules/navigator/mission.cpp

@@ -377,6 +377,7 @@ Mission::set_mission_items()
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
+			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
 
@@ -385,6 +386,7 @@ Mission::set_mission_items()
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
+			user_feedback_done = true;
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 	} else {
@@ -392,21 +394,17 @@ Mission::set_mission_items()
 		if (_mission_type != MISSION_TYPE_NONE) {
 			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			user_feedback_done = true;
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
 
-		} else if (!user_feedback_done) {
-			/* only tell users that we got no mission if there has not been any
-			 * better, more specific feedback yet
-			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
-			 */
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
 		}
+
 		_mission_type = MISSION_TYPE_NONE;
 
-		/* set loiter mission item */
-		set_loiter_item(&_mission_item);
+		/* set loiter mission item and ensure that there is a minimum clearance from home */
+		set_loiter_item(&_mission_item, _param_takeoff_alt.get());
 
 		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
@@ -418,6 +416,24 @@ Mission::set_mission_items()
 
 		set_mission_finished();
 
+		if (!user_feedback_done) {
+			/* only tell users that we got no mission if there has not been any
+			 * better, more specific feedback yet
+			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
+			 */
+
+			if (_navigator->get_vstatus()->condition_landed) {
+				/* landed, refusing to take off without a mission */
+
+				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
+			} else {
+				mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
+			}
+
+			user_feedback_done = true;
+
+		}
+
 		_navigator->set_position_setpoint_triplet_updated();
 		return;
 	}

src/modules/navigator/mission_block.cpp

@@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint()
 }
 
 void
-MissionBlock::set_loiter_item(struct mission_item_s *item)
+MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {
 	if (_navigator->get_vstatus()->condition_landed) {
 		/* landed, don't takeoff, but switch to IDLE mode */
@@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item)
 			item->altitude = pos_sp_triplet->current.alt;
 
 		} else {
-			/* use current position */
+			/* use current position and use return altitude as clearance */
 			item->lat = _navigator->get_global_position()->lat;
 			item->lon = _navigator->get_global_position()->lon;
 			item->altitude = _navigator->get_global_position()->alt;
+
+			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
+				item->altitude = _navigator->get_home_position()->alt + min_clearance;
+			}
 		}
 
 		item->altitude_is_relative = false;

src/modules/navigator/mission_block.h

@@ -91,7 +91,7 @@ protected:
 	/**
 	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
 	 */
-	void set_loiter_item(struct mission_item_s *item);
+	void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
 
 	mission_item_s _mission_item;
 	bool _waypoint_position_reached;