added auto takeoff support, updated configuration for solo and generalized landing mission items

ROMFS/px4fmu_common/init.d/4030_solo

@@ -15,10 +15,16 @@ sh /etc/init.d/rc.mc_defaults
 
 if [ $AUTOCNF == yes ]
 then
+    # tuning
     param set MC_PITCHRATE_P    0.13
     param set MC_ROLLRATE_P     0.13
     param set MPC_MANTHR_MIN    0.08
 
+    # takeoff. land and RTL settings
+    param set MIS_TAKEOFF_ALT   5.0
+    param set COM_DISARM_LAND   1
+    param set RTL_LAND_DELAY    1
+
     # enable high bandwidth mavlink by default
     param set SYS_COMPANION     921601
 fi

msg/vehicle_status.msg

@@ -10,7 +10,8 @@ uint8 MAIN_STATE_OFFBOARD = 7
 uint8 MAIN_STATE_STAB = 8
 uint8 MAIN_STATE_RATTITUDE = 9
 uint8 MAIN_STATE_AUTO_TAKEOFF = 10
-uint8 MAIN_STATE_MAX = 11
+uint8 MAIN_STATE_AUTO_LAND = 11
+uint8 MAIN_STATE_MAX = 12
 
 # If you change the order, add or remove arming_state_t states make sure to update the arrays
 # in state_machine_helper.cpp as well.
@@ -104,6 +105,7 @@ uint16 counter   # incremented by the writing thread everytime new data is store
 uint64 timestamp # in microseconds since system start, is set whenever the writing thread stores new data
 
 uint8 main_state		    	# main state machine
+uint8 main_state_prev       # previous main state
 uint8 nav_state				# set navigation state machine to specified value
 uint8 arming_state			# current arming state
 uint8 hil_state				# current hil state

src/modules/commander/commander.cpp

@@ -1083,6 +1083,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.rc_input_blocked = false;
 	status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
 	status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
+	status.main_state_prev = vehicle_status_s::MAIN_STATE_MAX;
 	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
 
@@ -2348,6 +2349,25 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* handle main state after takeoff and land */
+		if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF
+				&& mission_result.finished) {
+
+			// transition back to state we had before takeoff
+			if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX
+					&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) {
+				main_state_transition(&status, status.main_state_prev);
+			}
+
+		} else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND
+				&& status.condition_landed) {
+
+			// transition back to state we had before takeoff
+			if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX
+					&& status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) {
+				main_state_transition(&status, status.main_state_prev);
+			}
+		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
 		orb_check(cmd_sub, &updated);

src/modules/commander/state_machine_helper.cpp

@@ -352,6 +352,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
 	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
 	case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
+	case vehicle_status_s::MAIN_STATE_AUTO_LAND:
 		/* need global position and home position */
 		if (status->condition_global_position_valid && status->condition_home_position_valid) {
 			ret = TRANSITION_CHANGED;
@@ -373,6 +374,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	}
 	if (ret == TRANSITION_CHANGED) {
 		if (status->main_state != new_main_state) {
+			status->main_state_prev = status->main_state;
 			status->main_state = new_main_state;
 		} else {
 			ret = TRANSITION_NOT_CHANGED;
@@ -766,6 +768,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		}
 		break;
 
+	case vehicle_status_s::MAIN_STATE_AUTO_LAND:
+		/* require global position and home */
+
+		if (status->engine_failure) {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if ((!status->condition_global_position_valid ||
+					!status->condition_home_position_valid)) {
+			status->failsafe = true;
+
+			if (status->condition_local_altitude_valid) {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
+			}
+		} else {
+			status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
+		}
+		break;
+
 	case vehicle_status_s::MAIN_STATE_OFFBOARD:
 		/* require offboard control, otherwise stay where you are */
 		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {

src/modules/navigator/CMakeLists.txt

@@ -45,6 +45,7 @@ px4_add_module(
 		loiter.cpp
 		rtl.cpp
 		takeoff.cpp
+		land.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
 		datalinkloss.cpp

src/modules/navigator/land.cpp

@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file land.cpp
+ *
+ * Helper class to land at the current position
+ *
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "land.h"
+#include "navigator.h"
+
+Land::Land(Navigator *navigator, const char *name) :
+    MissionBlock(navigator, name)
+{
+    /* load initial params */
+    updateParams();
+}
+
+Land::~Land()
+{
+}
+
+void
+Land::on_inactive()
+{
+}
+
+void
+Land::on_activation()
+{
+    /* set current mission item to Land */
+    set_land_item(&_mission_item, true);
+
+    /* convert mission item to current setpoint */
+    struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+    pos_sp_triplet->previous.valid = false;
+    mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+    pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
+    pos_sp_triplet->next.valid = false;
+
+    _navigator->set_can_loiter_at_sp(false);
+
+    _navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+Land::on_active()
+{
+    if (is_mission_item_reached()) {
+        set_idle_item(&_mission_item);
+    }
+}

src/modules/navigator/land.h

@@ -0,0 +1,65 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2014 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file land.h
+ *
+ * Helper class to land at the current position
+ *
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#ifndef NAVIGATOR_LAND_H
+#define NAVIGATOR_LAND_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Land : public MissionBlock
+{
+public:
+    Land(Navigator *navigator, const char *name);
+
+    ~Land();
+
+    virtual void on_inactive();
+
+    virtual void on_activation();
+
+    virtual void on_active();
+
+};
+
+#endif

src/modules/navigator/mission_block.cpp

@@ -323,3 +323,51 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance,
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;
 }
+
+void
+MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location)
+{
+	item->nav_cmd = NAV_CMD_LAND;
+
+	/* use current position */
+	if (at_current_location) {
+		item->lat = _navigator->get_global_position()->lat;
+		item->lon = _navigator->get_global_position()->lon;
+	
+	/* use home position */
+	} else {
+		item->lat = _navigator->get_home_position()->lat;
+		item->lon = _navigator->get_home_position()->lon;
+	}
+
+	item->altitude = 0;
+	item->altitude_is_relative = false;
+	item->yaw = NAN;
+	item->loiter_radius = _navigator->get_loiter_radius();
+	item->loiter_direction = 1;
+	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->time_inside = 0.0f;
+	item->pitch_min = 0.0f;
+	item->autocontinue = true;
+	item->origin = ORIGIN_ONBOARD;
+}
+
+void
+MissionBlock::set_idle_item(struct mission_item_s *item)
+{
+	item->nav_cmd = NAV_CMD_IDLE;
+	item->lat = _navigator->get_home_position()->lat;
+	item->lon = _navigator->get_home_position()->lon;
+	item->altitude_is_relative = false;
+	item->altitude = _navigator->get_home_position()->alt;
+	item->yaw = NAN;
+	item->loiter_radius = _navigator->get_loiter_radius();
+	item->loiter_direction = 1;
+	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->time_inside = 0.0f;
+	item->pitch_min = 0.0f;
+	item->autocontinue = true;
+	item->origin = ORIGIN_ONBOARD;
+}
+
+

src/modules/navigator/mission_block.h

@@ -101,6 +101,16 @@ protected:
 	 */
 	void set_takeoff_item(struct mission_item_s *item, float min_clearance = -1.0f, float min_pitch = 0.0f);
 
+	/**
+	 * Set a land mission item
+	 */
+	void set_land_item(struct mission_item_s *item, bool at_current_location);
+
+	/**
+	 * Set idle mission item
+	 */
+	void set_idle_item(struct mission_item_s *item);
+
 	mission_item_s _mission_item;
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;

src/modules/navigator/navigator.h

@@ -62,6 +62,7 @@
 #include "mission.h"
 #include "loiter.h"
 #include "takeoff.h"
+#include "land.h"
 #include "rtl.h"
 #include "datalinkloss.h"
 #include "enginefailure.h"
@@ -72,7 +73,7 @@
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 8
+#define NAVIGATOR_MODE_ARRAY_SIZE 9
 
 class Navigator : public control::SuperBlock
 {
@@ -226,6 +227,7 @@ private:
 	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
 							  OBC rules (rc loss mode) */

src/modules/navigator/navigator_main.cpp

@@ -142,6 +142,7 @@ Navigator::Navigator() :
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_takeoff(this, "TKF"),
+	_land(this, "LND"),
 	_rtl(this, "RTL"),
 	_rcLoss(this, "RCL"),
 	_dataLinkLoss(this, "DLL"),
@@ -161,6 +162,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[5] = &_gpsFailure;
 	_navigation_mode_array[6] = &_rcLoss;
 	_navigation_mode_array[7] = &_takeoff;
+	_navigation_mode_array[8] = &_land;
 
 	updateParams();
 }
@@ -444,7 +446,6 @@ Navigator::task_main()
 			case vehicle_status_s::NAVIGATION_STATE_ACRO:
 			case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 			case vehicle_status_s::NAVIGATION_STATE_POSCTL:
-			case vehicle_status_s::NAVIGATION_STATE_LAND:
 			case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 			case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 				_navigation_mode = nullptr;
@@ -481,6 +482,10 @@ Navigator::task_main()
 				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_takeoff;
 				break;
+			case vehicle_status_s::NAVIGATION_STATE_LAND:
+				_pos_sp_triplet_published_invalid_once = false;
+				_navigation_mode = &_land;
+				break;
 			case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */

src/modules/navigator/rtl.cpp

@@ -243,39 +243,14 @@ RTL::set_rtl_item()
 	}
 
 	case RTL_STATE_LAND: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_home_position()->alt;
-		_mission_item.yaw = _navigator->get_home_position()->yaw;
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
+		set_land_item(&_mission_item, false);
 
 		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home");
 		break;
 	}
 
 	case RTL_STATE_LANDED: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_home_position()->alt;
-		// Do not change / control yaw in landed
-		_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
+		set_idle_item(&_mission_item);
 
 		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed");
 		break;

src/modules/navigator/takeoff.cpp

@@ -75,6 +75,8 @@ Takeoff::on_activation()
 {
 	/* set current mission item to Takeoff */
 	set_takeoff_item(&_mission_item, _param_min_alt.get());
+	_navigator->get_mission_result()->reached = false;
+	_navigator->get_mission_result()->finished = false;
 
 	/* convert mission item to current setpoint */
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -93,4 +95,8 @@ Takeoff::on_activation()
 void
 Takeoff::on_active()
 {
+	if (is_mission_item_reached()) {
+		_navigator->get_mission_result()->finished = true;
+		_navigator->set_mission_result_updated();
+	}
 }