Merged beta into mavlink rework branch

10 files changed, 61 insertions, 39 deletions

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 79a820c06..4c84c1f25 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -117,8 +117,8 @@ 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/mission_item_triplet.h"
-ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s);
+#include "topics/position_setpoint_triplet.h"
+ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s);
 
 #include "topics/vehicle_global_velocity_setpoint.h"
 ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h
index c40d0d4e5..d473dff3f 100644
--- a/src/modules/uORB/topics/battery_status.h
+++ b/src/modules/uORB/topics/battery_status.h
@@ -53,9 +53,10 @@
  */
 struct battery_status_s {
 	uint64_t	timestamp;		/**< microseconds since system boot, needed to integrate */
-	float   	voltage_v;		/**< Battery voltage in volts, filtered           	 */
-	float		current_a;		/**< Battery current in amperes, filtered, -1 if unknown */
-	float		discharged_mah;		/**< Discharged amount in mAh, filtered, -1 if unknown	 */
+	float   	voltage_v;		/**< Battery voltage in volts, 0 if unknown           	 */
+	float   	voltage_filtered_v;		/**< Battery voltage in volts, filtered, 0 if unknown  	 */
+	float		current_a;		/**< Battery current in amperes, -1 if unknown */
+	float		discharged_mah;		/**< Discharged amount in mAh, -1 if unknown	 */
 };
 
 /**
@@ -65,4 +66,4 @@ struct battery_status_s {
 /* register this as object request broker structure */
 ORB_DECLARE(battery_status);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 3e2fee84e..08d11abae 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -62,7 +62,7 @@ struct home_position_s
 	//bool altitude_is_relative;	// TODO what means home relative altitude? we need clear definition of reference altitude then
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
-	float altitude;				/**< Altitude in meters				*/
+	float alt;				/**< Altitude in meters				*/
 };
 
 /**
diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index b35eae607..4b57833b6 100644
--- a/src/modules/uORB/topics/mission_item_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -46,31 +46,42 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include "mission.h"
-
 /**
  * @addtogroup topics
  * @{
  */
 
+enum SETPOINT_TYPE
+{
+	SETPOINT_TYPE_NORMAL = 0,
+	SETPOINT_TYPE_LOITER,
+	SETPOINT_TYPE_TAKEOFF,
+	SETPOINT_TYPE_LAND,
+};
+
+struct position_setpoint_s
+{
+	bool valid;					/**< true if setpoint is valid */
+	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
+	double lat;					/**< latitude, in deg */
+	double lon;					/**< longitude, in deg */
+	float alt;					/**< altitude AMSL, in m */
+	float yaw;					/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
+	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
+	float pitch_min;			/**< minimal pitch angle for fixed wing takeoff waypoints */
+};
+
 /**
  * Global position setpoint triplet in WGS84 coordinates.
  *
  * This are the three next waypoints (or just the next two or one).
  */
-struct mission_item_triplet_s
+struct position_setpoint_triplet_s
 {
-	bool previous_valid;
-	bool current_valid;				/**< flag indicating previous mission item is valid */
-	bool next_valid;					/**< flag indicating next mission item is valid */
-
-	struct mission_item_s previous;
-	struct mission_item_s current;
-	struct mission_item_s next;
-
-	int previous_index;
-	int current_index;
-	int next_index;
+	struct position_setpoint_s previous;
+	struct position_setpoint_s current;
+	struct position_setpoint_s next;
 };
 
 /**
@@ -78,6 +89,6 @@ struct mission_item_triplet_s
  */
 
 /* register this as object request broker structure */
-ORB_DECLARE(mission_item_triplet);
+ORB_DECLARE(position_setpoint_triplet);
 
 #endif
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index 4380a5ee7..e5a35ff9b 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -76,6 +76,7 @@ struct vehicle_attitude_s {
 	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
 	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
 	float q[4];		/**< Quaternion (NED)						*/
+	float g_comp[3];	/**< Compensated gravity vector				*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/
 
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 1a245132a..7596f944f 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -61,7 +61,7 @@ struct vehicle_attitude_setpoint_s
 	float yaw_body;					/**< body angle in NED frame		*/
 	//float body_valid;				/**< Set to true if body angles are valid */
 
-	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
+	float R_body[3][3];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
 	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
 	//! For quaternion-based attitude control
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 26dcbd985..5aecac898 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -67,6 +67,7 @@ typedef enum {
 	NAV_STATE_LOITER,
 	NAV_STATE_MISSION,
 	NAV_STATE_RTL,
+	NAV_STATE_LAND,
 	NAV_STATE_MAX
 } nav_state_t;
 
@@ -92,7 +93,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_termination_enabled;   /**< true if flighttermination is enabled */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 143734e37..ae771ca00 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -54,7 +54,7 @@
  /**
  * Fused global position in WGS84.
  *
- * This struct contains the system's believ about its position. It is not the raw GPS
+ * This struct contains global position estimation. It is not the raw GPS
  * measurement (@see vehicle_gps_position). This topic is usually published by the position
  * estimator, which will take more sources of information into account than just GPS,
  * e.g. control inputs of the vehicle in a Kalman-filter implementation.
@@ -65,14 +65,13 @@ struct vehicle_global_position_s
 	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
 	bool valid;			/**< true if position satisfies validity criteria of estimator */
 
-	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
-	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
-	float alt;			/**< Altitude in meters									 	   */
-	float relative_alt;		/**< Altitude above home position in meters, 				   */
-	float vx; 			/**< Ground X velocity, m/s in NED				 			   */
-	float vy;			/**< Ground Y velocity, m/s in NED							   */
-	float vz;			/**< Ground Z velocity, m/s	in NED 							   */
-	float yaw; 			/**< Compass heading in radians -PI..+PI.					   */
+	double lat;			/**< Latitude in degrees							 	   */
+	double lon;			/**< Longitude in degrees							 	   */
+	float alt;			/**< Altitude in meters								 	   */
+	float vel_n; 		/**< Ground north velocity, m/s				 			   */
+	float vel_e;		/**< Ground east velocity, m/s							   */
+	float vel_d;		/**< Ground downside velocity, m/s						   */
+	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 427153782..d567f2e02 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -77,6 +77,11 @@ struct vehicle_local_position_s
 	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
 	bool landed;			/**< true if vehicle is landed */
+	/* Distance to surface */
+	float dist_bottom;		/**< Distance to bottom surface (ground) */
+	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
+	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
+	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1a9dec5f5..a5988d3ba 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -64,6 +64,7 @@ typedef enum {
 	MAIN_STATE_SEATBELT,
 	MAIN_STATE_EASY,
 	MAIN_STATE_AUTO,
+	MAIN_STATE_MAX
 } main_state_t;
 
 typedef enum {
@@ -73,7 +74,8 @@ typedef enum {
 	ARMING_STATE_ARMED_ERROR,
 	ARMING_STATE_STANDBY_ERROR,
 	ARMING_STATE_REBOOT,
-	ARMING_STATE_IN_AIR_RESTORE
+	ARMING_STATE_IN_AIR_RESTORE,
+	ARMING_STATE_MAX
 } arming_state_t;
 
 typedef enum {
@@ -82,9 +84,12 @@ typedef enum {
 } hil_state_t;
 
 typedef enum {
-	FLIGHTTERMINATION_STATE_OFF = 0,
-	FLIGHTTERMINATION_STATE_ON
-} flighttermination_state_t;
+	FAILSAFE_STATE_NORMAL = 0,		/**< Normal operation */
+	FAILSAFE_STATE_RTL,				/**< Return To Launch */
+	FAILSAFE_STATE_LAND,			/**< Land without position control */
+	FAILSAFE_STATE_TERMINATION,		/**< Disable motors and use parachute, can't be recovered */
+	FAILSAFE_STATE_MAX
+} failsafe_state_t;
 
 typedef enum {
 	MODE_SWITCH_MANUAL = 0,
@@ -173,6 +178,7 @@ struct vehicle_status_s
 	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
+	failsafe_state_t failsafe_state;		/**< current failsafe state */
 
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
@@ -223,8 +229,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	flighttermination_state_t flighttermination_state;
 };
 
 /**