Merge branch 'navigator_new' into navigator_new_vector, WIP

10 files changed, 174 insertions, 81 deletions

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index c6a252b55..79a820c06 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -117,17 +117,21 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi
 #include "topics/vehicle_bodyframe_speed_setpoint.h"
 ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s);
 
-#include "topics/vehicle_global_position_setpoint.h"
-ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
-
-#include "topics/vehicle_global_position_set_triplet.h"
-ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
+#include "topics/mission_item_triplet.h"
+ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
 
 #include "topics/vehicle_global_velocity_setpoint.h"
 ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
 
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
+ORB_DEFINE(onboard_mission, struct mission_s);
+
+#include "topics/mission_result.h"
+ORB_DEFINE(mission_result, struct mission_result_s);
+
+#include "topics/fence.h"
+ORB_DEFINE(fence, unsigned);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h
new file mode 100644
index 000000000..6f16c51cf
--- /dev/null
+++ b/src/modules/uORB/topics/fence.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
+ *
+ * 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 fence.h
+ * Definition of geofence.
+ */
+
+#ifndef TOPIC_FENCE_H_
+#define TOPIC_FENCE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+#define GEOFENCE_MAX_VERTICES 16
+
+/**
+ * This is the position of a geofence vertex
+ *
+ */
+struct fence_vertex_s {
+	// Worst case float precision gives us 2 meter resolution at the equator
+	float lat;			/**< latitude in degrees */
+	float lon;			/**< longitude in degrees */
+};
+
+/**
+ * This is the position of a geofence
+ *
+ */
+struct fence_s { 
+	unsigned count;     /**< number of actual vertices */
+	struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES];
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(fence);
+
+#endif
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 7e1b82a0f..1cf37e29c 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,9 +35,10 @@
 
 /**
  * @file home_position.h
- * Definition of the GPS home position uORB topic.
+ * Definition of the home position uORB topic.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef TOPIC_HOME_POSITION_H_
@@ -55,16 +57,12 @@
  */
 struct home_position_s
 {
-	uint64_t timestamp;             /**< Timestamp (microseconds since system boot)   */
-	uint64_t time_gps_usec;         /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module   */
-	
-	int32_t lat;                    /**< Latitude in 1E7 degrees */
-	int32_t lon;                    /**< Longitude in 1E7 degrees */
-	int32_t alt;                    /**< Altitude in 1E3 meters (millimeters) above MSL */
-	float eph_m;                   /**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;                   /**< GPS VDOP horizontal dilution of position in m */
-	float s_variance_m_s;               /**< speed accuracy estimate m/s */
-	float p_variance_m;               /**< position accuracy estimate m */
+	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
+
+	bool altitude_is_relative;
+	double lat;				/**< Latitude in degrees 			*/
+	double lon;				/**< Longitude in degrees			*/
+	float altitude;				/**< Altitude in meters				*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 978a3383a..dcdb234fa 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -35,8 +35,8 @@
  ****************************************************************************/
 
 /**
- * @file mission_item.h
- * Definition of one mission item.
+ * @file mission.h
+ * Definition of a mission consisting of mission items.
  */
 
 #ifndef TOPIC_MISSION_H_
@@ -46,14 +46,24 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#define NUM_MISSIONS_SUPPORTED 256
+
+/* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
-	NAV_CMD_WAYPOINT = 0,
-	NAV_CMD_LOITER_TURN_COUNT,
-	NAV_CMD_LOITER_TIME_LIMIT,
-	NAV_CMD_LOITER_UNLIMITED,
-	NAV_CMD_RETURN_TO_LAUNCH,
-	NAV_CMD_LAND,
-	NAV_CMD_TAKEOFF
+	NAV_CMD_WAYPOINT=16,
+	NAV_CMD_LOITER_UNLIMITED=17,
+	NAV_CMD_LOITER_TURN_COUNT=18,
+	NAV_CMD_LOITER_TIME_LIMIT=19,
+	NAV_CMD_RETURN_TO_LAUNCH=20,
+	NAV_CMD_LAND=21,
+	NAV_CMD_TAKEOFF=22,
+	NAV_CMD_ROI=80,
+	NAV_CMD_PATHPLANNING=81
+};
+
+enum ORIGIN {
+	ORIGIN_MAVLINK = 0,
+	ORIGIN_ONBOARD
 };
 
 /**
@@ -70,23 +80,26 @@ enum NAV_CMD {
 struct mission_item_s
 {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	double lat;			/**< latitude in degrees * 1E7				*/
-	double lon;			/**< longitude in degrees * 1E7				*/
+	double lat;			/**< latitude in degrees				*/
+	double lon;			/**< longitude in degrees				*/
 	float altitude;			/**< altitude in meters					*/
 	float yaw;			/**< in radians NED -PI..+PI 				*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
+	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	enum NAV_CMD nav_cmd;		/**< navigation command					*/
+	float radius;			/**< radius in which the mission is accepted as reached in meters */
+	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
+	bool autocontinue;		/**< true if next waypoint should follow after this one */
+	int index;			/**< index matching the mavlink waypoint                */
+	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
 };
 
 struct mission_s
 {
-	struct mission_item_s *items;
-	unsigned count;
+	int dataman_id;			/**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
+	unsigned count;			/**< count of the missions stored in the datamanager */
+	int current_index;		/**< default -1, start at the one changed latest */
 };
 
 /**
@@ -95,5 +108,6 @@ struct mission_s
 
 /* register this as object request broker structure */
 ORB_DECLARE(mission);
+ORB_DECLARE(onboard_mission);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h
index 8516b263f..b35eae607 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/mission_item_triplet.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *           @author Julian Oes <joes@student.ethz.ch>
  *           @author Lorenz Meier <lm@inf.ethz.ch>
@@ -35,18 +35,18 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
+ * @file mission_item_triplet.h
  * Definition of the global WGS84 position setpoint uORB topic.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_
+#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
+#define TOPIC_MISSION_ITEM_TRIPLET_H_
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include "vehicle_global_position_setpoint.h"
+#include "mission.h"
 
 /**
  * @addtogroup topics
@@ -58,14 +58,19 @@
  *
  * This are the three next waypoints (or just the next two or one).
  */
-struct vehicle_global_position_set_triplet_s
+struct mission_item_triplet_s
 {
-	bool previous_valid;					/**< flag indicating previous position is valid */
-	bool next_valid;					/**< flag indicating next position is valid */
+	bool previous_valid;
+	bool current_valid;				/**< flag indicating previous mission item is valid */
+	bool next_valid;					/**< flag indicating next mission item is valid */
 
-	struct vehicle_global_position_setpoint_s previous;
-	struct vehicle_global_position_setpoint_s current;
-	struct vehicle_global_position_setpoint_s next;
+	struct mission_item_s previous;
+	struct mission_item_s current;
+	struct mission_item_s next;
+
+	int previous_index;
+	int current_index;
+	int next_index;
 };
 
 /**
@@ -73,6 +78,6 @@ struct vehicle_global_position_set_triplet_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_set_triplet);
+ORB_DECLARE(mission_item_triplet);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/mission_result.h
index a56434d3b..c99706b97 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -35,45 +35,26 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_global_position_setpoint.h
- * Definition of the global WGS84 position setpoint uORB topic.
+ * @file mission_result.h
+ * Mission results that navigator needs to pass on to commander and mavlink.
  */
 
-#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
-#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
+#ifndef TOPIC_MISSION_RESULT_H
+#define TOPIC_MISSION_RESULT_H
 
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-#include "mission.h"
 
 /**
  * @addtogroup topics
  * @{
  */
 
-/**
- * Global position setpoint in WGS84 coordinates.
- *
- * This is the position the MAV is heading towards. If it of type loiter,
- * the MAV is circling around it with the given loiter radius in meters.
- */
-struct vehicle_global_position_setpoint_s
+struct mission_result_s
 {
-	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
-	int32_t lat;			/**< latitude in degrees * 1E7				*/
-	int32_t lon;			/**< longitude in degrees * 1E7				*/
-	float altitude;			/**< altitude in meters					*/
-	float yaw;			/**< in radians NED -PI..+PI 				*/
-	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
-	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
-	float param1;
-	float param2;
-	float param3;
-	float param4;
-	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
-	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
+	bool mission_reached;		/**< true if mission has been reached			*/
+	unsigned mission_index;		/**< index of the mission which has been reached	*/
 };
 
 /**
@@ -81,6 +62,6 @@ struct vehicle_global_position_setpoint_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(vehicle_global_position_setpoint);
+ORB_DECLARE(mission_result);
 
 #endif
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
index 6a3e811e3..391756f99 100644
--- a/src/modules/uORB/topics/navigation_capabilities.h
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -53,6 +53,11 @@
  */
 struct navigation_capabilities_s {
     float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+
+    /* Landing parameters: see fw_pos_control_l1/landingslope.h */
+    float landing_horizontal_slope_displacement;
+    float landing_slope_angle_rad;
+    float landing_flare_length;
 };
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 5a8580143..086b2ef15 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
- *           @author Ivan Ovinnikov <oivan@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 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
@@ -47,13 +44,13 @@
 
 /**
  * The number of RC channel inputs supported.
- * Current (Q1/2013) radios support up to 18 channels,
+ * Current (Q4/2013) radios support up to 18 channels,
  * leaving at a sane value of 15.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -91,7 +88,7 @@ struct rc_channels_s {
   uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
   struct {
     float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_MAX];
+  } chan[RC_CHANNELS_MAPPED_MAX];
   uint8_t chan_count;                 /**< number of valid channels */
 
   /*String array to store the names of the functions*/
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 38fb74c9b..e26fb97c8 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -78,6 +78,7 @@ struct vehicle_control_mode_s
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
+	bool flag_control_flighttermination_enabled;   /**< true if flighttermination is enabled */
 
 	bool flag_control_auto_enabled;		// TEMP
 	uint8_t auto_state;	// TEMP navigation state for AUTO modes
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1c184d3a7..1c3763924 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -95,6 +95,12 @@ typedef enum {
 	HIL_STATE_ON
 } hil_state_t;
 
+
+typedef enum {
+	FLIGHTTERMINATION_STATE_OFF = 0,
+	FLIGHTTERMINATION_STATE_ON
+} flighttermination_state_t;
+
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
 	MODE_SWITCH_ASSISTED,
@@ -229,6 +235,8 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
+
+	flighttermination_state_t flighttermination_state;
 };
 
 /**