From cb246a79cad96c469f29042dd33658d63cbcc743 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 5 Jun 2014 16:47:16 +0200
Subject: Added vision position estimate topic

---
 src/modules/uORB/objects_common.cpp                |  3 +
 src/modules/uORB/topics/vision_position_estimate.h | 82 ++++++++++++++++++++++
 2 files changed, 85 insertions(+)
 create mode 100644 src/modules/uORB/topics/vision_position_estimate.h

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 90675bb2e..fc9920a41 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -199,3 +199,6 @@ ORB_DEFINE(encoders, struct encoders_s);
 
 #include "topics/estimator_status.h"
 ORB_DEFINE(estimator_status, struct estimator_status_report);
+
+#include "topics/vision_position_estimate.h"
+ORB_DEFINE(vision_position_estimate, struct vision_position_estimate);
diff --git a/src/modules/uORB/topics/vision_position_estimate.h b/src/modules/uORB/topics/vision_position_estimate.h
new file mode 100644
index 000000000..74c96cf2e
--- /dev/null
+++ b/src/modules/uORB/topics/vision_position_estimate.h
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   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 vision_position_estimate.h
+ * Vision based position estimate
+ */
+
+#ifndef TOPIC_VISION_POSITION_ESTIMATE_H_
+#define TOPIC_VISION_POSITION_ESTIMATE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Vision based position estimate in NED frame
+ */
+struct vision_position_estimate {
+
+	unsigned id;				/**< ID of the estimator, commonly the component ID of the incoming message */
+
+	uint64_t timestamp_boot;		/**< time of this estimate, in microseconds since system start */
+	uint64_t timestamp_computer;		/**< timestamp provided by the companion computer, in us */
+
+	float x;				/**< X position in meters in NED earth-fixed frame */
+	float y;				/**< Y position in meters in NED earth-fixed frame */
+	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
+
+	float vx;				/**< X velocity in meters per second in NED earth-fixed frame */
+	float vy;				/**< Y velocity in meters per second in NED earth-fixed frame */
+	float vz;				/**< Z velocity in meters per second in NED earth-fixed frame */
+
+	float q[4];				/**< Estimated attitude as quaternion */
+
+	// XXX Add covariances here
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vision_position_estimate);
+
+#endif /* TOPIC_VISION_POSITION_ESTIMATE_H_ */
-- 
cgit v1.2.3


From 839710daf841a9528a58e915f8b04484bf54e7dc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 3 Jul 2014 14:42:59 +0200
Subject: Update offboard control uorb topic

Work towards supporting the new external setpoint mavlink messages
---
 src/modules/commander/commander.cpp                | 16 ++----
 src/modules/mavlink/mavlink_receiver.cpp           | 28 +++++++++
 src/modules/mavlink/mavlink_receiver.h             |  1 +
 src/modules/navigator/offboard.cpp                 | 67 +++++++++++++---------
 .../uORB/topics/offboard_control_setpoint.h        | 36 +++++++++---
 5 files changed, 102 insertions(+), 46 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 38297745d..7185eebc4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -946,7 +946,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* Perform system checks (again) once params are loaded and MAVLink is up. */
-		if (!system_checked && mavlink_fd && 
+		if (!system_checked && mavlink_fd &&
 			(telemetry.heartbeat_time > 0) &&
 			(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
 
@@ -1847,21 +1847,17 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
-			control_mode.flag_control_velocity_enabled = true;
-			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = true;
 			control_mode.flag_control_altitude_enabled = true;
 			control_mode.flag_control_climb_rate_enabled = true;
 			control_mode.flag_control_position_enabled = true;
 			control_mode.flag_control_velocity_enabled = true;
+			//XXX: the flags could depend on sp_offboard.ignore
 			break;
 		default:
 			control_mode.flag_control_rates_enabled = false;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 3747ad3ba..73222637f 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -154,6 +154,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
 		break;
 
+	case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL:
+		handle_message_local_ned_position_setpoint_external(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -393,6 +397,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 	}
 }
 
+void
+MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg)
+{
+	mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external;
+	mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
+
+	//XXX check if target system/component is correct
+
+	/* convert mavlink type (local, NED) to uORB offboard control struct */
+	//XXX do the conversion
+	//
+	offboard_control_sp.timestamp = hrt_absolute_time();
+
+	if (_offboard_control_sp_pub < 0) {
+		_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+	} else {
+		orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+	}
+}
+
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 65ef884a2..658f51562 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -115,6 +115,7 @@ private:
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
index ef7d11a03..27ce46a1d 100644
--- a/src/modules/navigator/offboard.cpp
+++ b/src/modules/navigator/offboard.cpp
@@ -79,33 +79,46 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	bool changed = false;
 
 	/* copy offboard setpoints to the corresponding topics */
-	if (_navigator->get_control_mode()->flag_control_position_enabled
-	    && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
-		/* position control */
-		pos_sp_triplet->current.x = _offboard_control_sp.p1;
-		pos_sp_triplet->current.y = _offboard_control_sp.p2;
-		pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
-		pos_sp_triplet->current.z = -_offboard_control_sp.p4;
-
-		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->current.position_valid = true;
-
-		changed = true;
-
-	} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
-	           && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
-		/* velocity control */
-		pos_sp_triplet->current.vx = _offboard_control_sp.p2;
-		pos_sp_triplet->current.vy = _offboard_control_sp.p1;
-		pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
-		pos_sp_triplet->current.vz = _offboard_control_sp.p4;
-
-		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->current.velocity_valid = true;
-
-		changed = true;
+	if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) {
+		/* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not
+		 * support deactivation of individual directions) */
+		if (_navigator->get_control_mode()->flag_control_position_enabled &&
+				(!_offboard_control_sp.ignore[0] &&
+				 !_offboard_control_sp.ignore[1] &&
+				 !_offboard_control_sp.ignore[2])) {
+			/* position control */
+			pos_sp_triplet->current.x = _offboard_control_sp.p1;
+			pos_sp_triplet->current.y = _offboard_control_sp.p2;
+			pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
+			pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+
+			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+			pos_sp_triplet->current.valid = true;
+			pos_sp_triplet->current.position_valid = true;
+
+			changed = true;
+
+		}
+		/* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not
+		 * support deactivation of individual directions) */
+		if (_navigator->get_control_mode()->flag_control_velocity_enabled &&
+				(!_offboard_control_sp.ignore[3] &&
+				 !_offboard_control_sp.ignore[4] &&
+				 !_offboard_control_sp.ignore[5])) {
+			/* velocity control */
+			pos_sp_triplet->current.vx = _offboard_control_sp.p2;
+			pos_sp_triplet->current.vy = _offboard_control_sp.p1;
+			pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
+			pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+
+			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+			pos_sp_triplet->current.valid = true;
+			pos_sp_triplet->current.velocity_valid = true;
+
+			changed = true;
+		}
+
+		//XXX: map acceleration setpoint once supported in setpoint triplet
 	}
 
 	return changed;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index d7b131e3c..427ccc6c8 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -53,11 +53,22 @@ enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT = 0,
 	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
 	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
-	OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
-	OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
+	OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
+	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
+	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
+	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9,
+	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+};
+
+enum OFFBOARD_CONTROL_FRAME {
+	OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
+	OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
+	OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
+	OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
+	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
 };
 
 /**
@@ -70,10 +81,17 @@ struct offboard_control_setpoint_s {
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
 
-	float p1;	/**< ailerons roll / roll rate input */
-	float p2;	/**< elevator / pitch / pitch rate */
-	float p3;	/**< rudder / yaw rate / yaw */
-	float p4;	/**< throttle / collective thrust / altitude */
+	double p1;	/**< ailerons / roll / x pos / lat */
+	double p2;	/**< elevator / pitch / y pos / lon */
+	float p3;	/**< rudder / yaw / z pos / alt */
+	float p4;	/**< throttle / x vel */
+	float p5;       /**< roll rate / y vel */
+	float p6;       /**< pitch rate / z vel */
+	float p7;	/**< yaw rate / x acc */
+	float p8;	/**< y acc */
+	float p9;	/**< z acc */
+
+	bool ignore[9]; /**< if field i is set to true, pi should be ignored */
 
 	float override_mode_switch;
 
-- 
cgit v1.2.3


From 822403e34b5e1b4adf15783b5bd701e1f52484fe Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 3 Jul 2014 16:20:58 +0200
Subject: uorb offboard control topic: add force sp flag

The flag has the same meaning as bit 10 of type_mask in
MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL
---
 src/modules/uORB/topics/offboard_control_setpoint.h | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 427ccc6c8..2741f0c07 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -91,7 +91,9 @@ struct offboard_control_setpoint_s {
 	float p8;	/**< y acc */
 	float p9;	/**< z acc */
 
+	//XXX: use a bitmask with wrapper functions instead
 	bool ignore[9]; /**< if field i is set to true, pi should be ignored */
+	bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */
 
 	float override_mode_switch;
 
-- 
cgit v1.2.3


From 2d26e913921ce80e509bd79462319d09d12c2171 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 7 Jul 2014 13:00:45 +0200
Subject: WIP, change uorb offboard control sp topic

---
 src/modules/mavlink/mavlink_receiver.cpp           | 30 ++++++++++---------
 src/modules/navigator/offboard.cpp                 | 30 ++++++++++---------
 .../uORB/topics/offboard_control_setpoint.h        | 34 +++++++++++++++-------
 3 files changed, 55 insertions(+), 39 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b72bfa858..e1a74f599 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -379,10 +379,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
 
 		/* Convert values * 1000 back */
-		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
+		//XXX: convert to quaternion
+		//offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
+		//offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
+		//offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
+		//offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
 
 		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
 
@@ -427,18 +428,19 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 				/* invalid setpoint, avoid publishing */
 				return;
 		}
-		offboard_control_sp.p1 = local_ned_position_setpoint_external.x;
-		offboard_control_sp.p2 = local_ned_position_setpoint_external.y;
-		offboard_control_sp.p3 = local_ned_position_setpoint_external.z;
-		offboard_control_sp.p4 = local_ned_position_setpoint_external.vx;
-		offboard_control_sp.p5 = local_ned_position_setpoint_external.vy;
-		offboard_control_sp.p6 = local_ned_position_setpoint_external.vz;
-		offboard_control_sp.p7 = local_ned_position_setpoint_external.afx;
-		offboard_control_sp.p8 = local_ned_position_setpoint_external.afy;
-		offboard_control_sp.p9 = local_ned_position_setpoint_external.afz;
+		offboard_control_sp.position[0] = local_ned_position_setpoint_external.x;
+		offboard_control_sp.position[1] = local_ned_position_setpoint_external.y;
+		offboard_control_sp.position[2] = local_ned_position_setpoint_external.z;
+		offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx;
+		offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy;
+		offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz;
+		offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx;
+		offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy;
+		offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
 		offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
 		for (int i = 0; i < 9; i++) {
-			offboard_control_sp.ignore[i] =  (bool)(local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore &=  ~(local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
 		}
 
 
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
index 27ce46a1d..71a14c029 100644
--- a/src/modules/navigator/offboard.cpp
+++ b/src/modules/navigator/offboard.cpp
@@ -83,14 +83,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		/* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not
 		 * support deactivation of individual directions) */
 		if (_navigator->get_control_mode()->flag_control_position_enabled &&
-				(!_offboard_control_sp.ignore[0] &&
-				 !_offboard_control_sp.ignore[1] &&
-				 !_offboard_control_sp.ignore[2])) {
+				(!offboard_control_sp_ignore_position(_offboard_control_sp, 0) &&
+				 !offboard_control_sp_ignore_position(_offboard_control_sp, 1) &&
+				 !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) {
 			/* position control */
-			pos_sp_triplet->current.x = _offboard_control_sp.p1;
-			pos_sp_triplet->current.y = _offboard_control_sp.p2;
-			pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
-			pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+			pos_sp_triplet->current.x = _offboard_control_sp.position[0];
+			pos_sp_triplet->current.y = _offboard_control_sp.position[1];
+			//pos_sp_triplet->current.yaw = _offboard_control_sp.position[2];
+			//XXX: copy yaw
+			pos_sp_triplet->current.z = -_offboard_control_sp.position[2];
 
 			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
 			pos_sp_triplet->current.valid = true;
@@ -102,14 +103,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 		/* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not
 		 * support deactivation of individual directions) */
 		if (_navigator->get_control_mode()->flag_control_velocity_enabled &&
-				(!_offboard_control_sp.ignore[3] &&
-				 !_offboard_control_sp.ignore[4] &&
-				 !_offboard_control_sp.ignore[5])) {
+				(!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) &&
+				 !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) &&
+				 !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) {
 			/* velocity control */
-			pos_sp_triplet->current.vx = _offboard_control_sp.p2;
-			pos_sp_triplet->current.vy = _offboard_control_sp.p1;
-			pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
-			pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+			pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0];
+			pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1];
+//			pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[;
+//			//XXX: copy yaw speed
+			pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2];
 
 			pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
 			pos_sp_triplet->current.valid = true;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 2741f0c07..688135533 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -81,19 +81,15 @@ struct offboard_control_setpoint_s {
 
 	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
 
-	double p1;	/**< ailerons / roll / x pos / lat */
-	double p2;	/**< elevator / pitch / y pos / lon */
-	float p3;	/**< rudder / yaw / z pos / alt */
-	float p4;	/**< throttle / x vel */
-	float p5;       /**< roll rate / y vel */
-	float p6;       /**< pitch rate / z vel */
-	float p7;	/**< yaw rate / x acc */
-	float p8;	/**< y acc */
-	float p9;	/**< z acc */
+	double position[3];	/**< lat, lon, alt / x, y, z */
+	float velocity[3];	/**< x vel, y vel, z vel */
+	float acceleration[3];	/**< x acc, y acc, z acc */
+	float attitude[4];	/**< attitude of vehicle (quaternion) */
+	float attitude_rate[3];	/**< body angular rates (x, y, z) */
 
 	//XXX: use a bitmask with wrapper functions instead
-	bool ignore[9]; /**< if field i is set to true, pi should be ignored */
-	bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */
+	uint16_t ignore; /**< if field i is set to true, pi should be ignored */
+	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
 
 	float override_mode_switch;
 
@@ -107,6 +103,22 @@ struct offboard_control_setpoint_s {
  * @}
  */
 
+inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << index));
+}
+
+inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (3 + index)));
+}
+
+inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
+}
+
+inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
+	return (bool)(offboard_control_sp.ignore & (1 << (9 + index)));
+}
+
 /* register this as object request broker structure */
 ORB_DECLARE(offboard_control_setpoint);
 
-- 
cgit v1.2.3


From 19017f100101dbf638bb91f7a520296f979ebb32 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 8 Jul 2014 13:29:59 +0200
Subject: remove comment

---
 src/modules/uORB/topics/offboard_control_setpoint.h | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 688135533..809412fc0 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -87,7 +87,6 @@ struct offboard_control_setpoint_s {
 	float attitude[4];	/**< attitude of vehicle (quaternion) */
 	float attitude_rate[3];	/**< body angular rates (x, y, z) */
 
-	//XXX: use a bitmask with wrapper functions instead
 	uint16_t ignore; /**< if field i is set to true, pi should be ignored */
 	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
 
-- 
cgit v1.2.3


From 373b1705c19627b97f1c65c1e947802e6b88af83 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 14 Jul 2014 10:57:53 +0200
Subject: vehicle command: add latest guided commands

---
 src/modules/uORB/topics/vehicle_command.h | 3 +++
 1 file changed, 3 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index c21a29b13..faf8cc7db 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -61,6 +61,9 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
-- 
cgit v1.2.3


From d0cca02e97a24cf0b9554cf429bbd5716f947e4e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 15 Jul 2014 10:34:56 +0200
Subject: add parsing of external attitude message

---
 src/modules/mavlink/mavlink_receiver.cpp           | 88 +++++++++++++++++++++-
 src/modules/mavlink/mavlink_receiver.h             |  1 +
 .../uORB/topics/offboard_control_setpoint.h        |  6 ++
 3 files changed, 93 insertions(+), 2 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 9df843e58..9e3842614 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_local_ned_position_setpoint_external(msg);
 		break;
 
+	case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL:
+		handle_message_attitude_setpoint_external(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -404,7 +408,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 	mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external);
 
 	struct offboard_control_setpoint_s offboard_control_sp;
-	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
 
 	if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system &&
 			mavlink_system.compid == local_ned_position_setpoint_external.target_component) {
@@ -438,7 +442,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 		offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
 		offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
 		for (int i = 0; i < 9; i++) {
-			offboard_control_sp.ignore &=  ~(local_ned_position_setpoint_external.type_mask & (1 << i));
+			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
 		}
 
@@ -469,6 +473,86 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 	}
 }
 
+void
+MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg)
+{
+	mavlink_attitude_setpoint_external_t attitude_setpoint_external;
+	mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
+
+	if (mavlink_system.sysid == attitude_setpoint_external.target_system &&
+			mavlink_system.compid == attitude_setpoint_external.target_component) {
+		for (int i = 0; i < 4; i++) {
+			offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i];
+		}
+		offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate;
+		offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate;
+		offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate;
+
+		for (int i = 0; i < 3; i++) {
+			offboard_control_sp.ignore &=  ~(1 << (i + 9));
+			offboard_control_sp.ignore |=  (attitude_setpoint_external.type_mask & (1 << i)) << 9;
+		}
+		offboard_control_sp.ignore &= ~(1 << 10);
+		offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3);
+
+
+		offboard_control_sp.timestamp = hrt_absolute_time();
+
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+		}
+
+		/* If we are in offboard control mode and offboard control loop through is enabled
+		 * also publish the setpoint topic which is read by the controller */
+		if (_mavlink->get_forward_externalsp()) {
+			bool updated;
+			orb_check(_control_mode_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+			}
+
+			if (_control_mode.flag_control_offboard_enabled) {
+
+				/* Publish attitude setpoint if ignore bit is not set */
+				if (!(attitude_setpoint_external.type_mask & (1 << 7))) {
+					struct vehicle_attitude_setpoint_s att_sp;
+					mavlink_quaternion_to_euler(attitude_setpoint_external.q,
+							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
+					att_sp.thrust = attitude_setpoint_external.thrust;
+					if (_att_sp_pub < 0) {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+					}
+				}
+
+				/* Publish attitude rate setpoint if ignore bit are not set */
+				///XXX add support for ignoring individual axes
+				if (!(attitude_setpoint_external.type_mask & (0b111))) {
+					struct vehicle_rates_setpoint_s rates_sp;
+					rates_sp.roll = attitude_setpoint_external.body_roll_rate;
+					rates_sp.pitch = attitude_setpoint_external.body_pitch_rate;
+					rates_sp.yaw = attitude_setpoint_external.body_yaw_rate;
+					rates_sp.thrust = attitude_setpoint_external.thrust;
+
+					if (_att_sp_pub < 0) {
+						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
+					}
+				}
+			}
+
+		}
+	}
+}
+
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 4fd80f4ac..0f12cf32c 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -114,6 +114,7 @@ private:
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg);
+	void handle_message_attitude_setpoint_external(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 809412fc0..809a2ace4 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -88,6 +88,7 @@ struct offboard_control_setpoint_s {
 	float attitude_rate[3];	/**< body angular rates (x, y, z) */
 
 	uint16_t ignore; /**< if field i is set to true, pi should be ignored */
+	//XXX define constants for bit offsets
 	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
 
 	float override_mode_switch;
@@ -118,6 +119,11 @@ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_s
 	return (bool)(offboard_control_sp.ignore & (1 << (9 + index)));
 }
 
+inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << 10));
+}
+
+
 /* register this as object request broker structure */
 ORB_DECLARE(offboard_control_setpoint);
 
-- 
cgit v1.2.3


From 3c8927c42386a6528f120d04ec93f5ab9b453a5b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 16 Jul 2014 09:38:57 +0200
Subject: once offboard is set by mavlink command ignore RC mode

---
 src/modules/commander/commander.cpp      | 10 ++++++++++
 src/modules/uORB/topics/vehicle_status.h |  2 ++
 2 files changed, 12 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e9db923e6..cc68c4e36 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -621,11 +621,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
 				if (res == TRANSITION_DENIED) {
 					print_reject_mode(status_local, "OFFBOARD");
+					status_local->offboard_control_set_by_command = false;
+				} else {
+					/* Set flag that offboard was set via command, main state is not overridden by rc */
+					status_local->offboard_control_set_by_command = true;
 				}
 			} else {
 				/* If the mavlink command is used to enable or disable offboard control:
 				 * switch back to previous mode when disabling */
 				res = main_state_transition(status_local, main_state_pre_offboard);
+				status_local->offboard_control_set_by_command = false;
 			}
 		}
 		break;
@@ -1749,6 +1754,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
+	/* if offboard is set allready by a mavlink command, abort */
+	if (status.offboard_control_set_by_command) {
+		return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+	}
+
 	/* offboard switch overrides main switch */
 	if (sp_man->offboard_switch == SWITCH_POS_ON) {
 		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b46c00b75..7a40ac636 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -206,6 +206,8 @@ struct vehicle_status_s {
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
+	bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
+						    and should not be overridden by RC */
 
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
-- 
cgit v1.2.3


From f4608707389dbc30eb25db524d6e008c8033d052 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 17 Jul 2014 09:11:57 +0200
Subject: support force setpoints

---
 src/modules/commander/commander.cpp                |  9 ++++++++
 src/modules/mavlink/mavlink_receiver.cpp           | 24 +++++++++++++++++++++-
 src/modules/mavlink/mavlink_receiver.h             |  2 ++
 .../uORB/topics/offboard_control_setpoint.h        |  7 ++++---
 src/modules/uORB/topics/vehicle_control_mode.h     |  1 +
 5 files changed, 39 insertions(+), 4 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index cc68c4e36..fc9560e18 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1951,6 +1951,15 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
 		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
 		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
 		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index e5d380cfb..2cc2d6162 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -105,6 +105,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
+	_force_sp_pub(-1),
 	_vicon_position_pub(-1),
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
@@ -444,6 +445,13 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 		offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy;
 		offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz;
 		offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9));
+
+		/* If we are in force control mode, for now set offboard mode to force control */
+		if (offboard_control_sp.isForceSetpoint) {
+			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
+		}
+
+		/* set ignore flags */
 		for (int i = 0; i < 9; i++) {
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (local_ned_position_setpoint_external.type_mask & (1 << i));
@@ -467,9 +475,23 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes
 			if (updated) {
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
-
 			if (_control_mode.flag_control_offboard_enabled) {
+				if (offboard_control_sp.isForceSetpoint) {
+					struct vehicle_force_setpoint_s	force_sp;
+					force_sp.x = offboard_control_sp.acceleration[0];
+					force_sp.y = offboard_control_sp.acceleration[1];
+					force_sp.z = offboard_control_sp.acceleration[2];
+					//XXX: yaw
+					if (_force_sp_pub < 0) {
+						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
+					}
+				} else {
+
 				//XXX: copy to and publish setpoint triplet here
+				}
+
 			}
 
 		}
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 0f12cf32c..0044b42cb 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -70,6 +70,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_force_setpoint.h>
 
 #include "mavlink_ftp.h"
 
@@ -146,6 +147,7 @@ private:
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
+	orb_advert_t _force_sp_pub;
 	orb_advert_t _vicon_position_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 809a2ace4..19f11ba92 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -58,9 +58,10 @@ enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
 	OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
 	OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8,
-	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
+	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
 enum OFFBOARD_CONTROL_FRAME {
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 49e2ba4b5..ca7705456 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -77,6 +77,7 @@ struct vehicle_control_mode_s {
 	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
+	bool flag_control_force_enabled;		/**< true if force control is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-- 
cgit v1.2.3


From 7baa337d9bcf7077a5e5080e951899cb595f6ff6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 19 Jul 2014 19:25:53 +0200
Subject: flight termination on geofence violation

---
 src/modules/commander/commander.cpp                 |  8 ++++++++
 src/modules/navigator/navigator_main.cpp            | 10 ++++++++++
 src/modules/uORB/topics/position_setpoint_triplet.h |  1 +
 3 files changed, 19 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 04450a44f..522c6e886 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1244,6 +1244,14 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
+
+			/* Check for geofence violation */
+			if (pos_sp_triplet.geofence_violated) {
+				//XXX: make this configurable to select different actions (e.g. navigation modes)
+				/* this will only trigger if geofence is activated via param and a geofence file is present */
+				armed.force_failsafe = true;
+				status_changed = true;
+			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 331a9a728..ba46bd568 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -335,6 +335,11 @@ Navigator::task_main()
 
 			/* Check geofence violation */
 			if (!_geofence.inside(&_global_pos)) {
+				/* inform other apps via the sp triplet */
+				_pos_sp_triplet.geofence_violated = true;
+				if (_pos_sp_triplet.geofence_violated != true) {
+					_pos_sp_triplet_updated = true;
+				}
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -342,6 +347,11 @@ Navigator::task_main()
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
+				/* inform other apps via the sp triplet */
+				_pos_sp_triplet.geofence_violated = false;
+				if (_pos_sp_triplet.geofence_violated != false) {
+					_pos_sp_triplet_updated = true;
+				}
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 4a1932180..4e8c6c53e 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -97,6 +97,7 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s next;
 
 	unsigned nav_state;				/**< report the navigation state */
+	bool geofence_violated;				/**< true if the geofence is violated */
 };
 
 /**
-- 
cgit v1.2.3


From 86b9e367a6cc5791f83df3223190a470798c00ff Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 20 Jul 2014 18:23:41 +0200
Subject: introduce data link lost counter

---
 src/modules/commander/commander.cpp      | 1 +
 src/modules/uORB/topics/vehicle_status.h | 1 +
 2 files changed, 2 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8eba64c1d..46065fef1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1477,6 +1477,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (!status.data_link_lost) {
 				mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
+				status.data_link_lost_counter++;
 				status_changed = true;
 			}
 		}
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b46c00b75..eda1dfaec 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -201,6 +201,7 @@ struct vehicle_status_s {
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool data_link_lost;						/**< datalink to GCS lost */
+	uint8_t data_link_lost_counter;					/**< counts unique data link lost events */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
-- 
cgit v1.2.3


From 24f380137ecb91fb9647e22e1d29c13da5fc0357 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 23 Jul 2014 22:58:19 +0200
Subject: add method to block fallback to mission

failsafe navigation modes can use a flag in mission_result to tell the
commander to not switch back to mission
---
 src/modules/commander/commander.cpp            |  5 ++--
 src/modules/commander/state_machine_helper.cpp |  3 ++-
 src/modules/commander/state_machine_helper.h   |  2 +-
 src/modules/navigator/datalinkloss.cpp         |  2 ++
 src/modules/navigator/mission.cpp              | 32 ++++++--------------------
 src/modules/navigator/mission.h                |  8 -------
 src/modules/navigator/navigator.h              | 13 ++++++++++-
 src/modules/navigator/navigator_main.cpp       | 22 ++++++++++++++++++
 src/modules/navigator/navigator_mode.cpp       |  3 +++
 src/modules/uORB/topics/mission_result.h       |  1 +
 10 files changed, 53 insertions(+), 38 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c89e0123..cb09a68e3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[])
 		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
 			if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) {
 				/* handle the case where data link was regained */
-				if (telemetry_lost[i]) {
+				if (telemetry_lost[i]) {//XXX also add hysteresis here
 					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
 					telemetry_lost[i] = false;
 				}
@@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
-						       mission_result.finished);
+						       mission_result.finished,
+						       mission_result.stay_in_failsafe);
 
 		// TODO handle mode changes by commands
 		if (main_state_changed) {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 7b26e3e8c..4e1cfb987 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
 /**
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
+		const bool stay_in_failsafe)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index bb1b87e71..4285d2977 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
 
 int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
 
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index a98e21139..52f263aff 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -200,6 +200,8 @@ DataLinkLoss::advance_dll()
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
 		//XXX check here if time is over are over
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
 		break;
 	default:
 		break;
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index ba766cd10..7ce0e2f89 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_current_offboard_mission_index(-1),
 	_need_takeoff(true),
 	_takeoff(false),
-	_mission_result_pub(-1),
-	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
 	_dist_1wp_ok(false)
@@ -577,18 +575,18 @@ void
 Mission::report_mission_item_reached()
 {
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
-		_mission_result.reached = true;
-		_mission_result.seq_reached = _current_offboard_mission_index;
+		_navigator->get_mission_result()->reached = true;
+		_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	}
-	publish_mission_result();
+	_navigator->publish_mission_result();
 }
 
 void
 Mission::report_current_offboard_mission_item()
 {
 	warnx("current offboard mission index: %d", _current_offboard_mission_index);
-	_mission_result.seq_current = _current_offboard_mission_index;
-	publish_mission_result();
+	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
+	_navigator->publish_mission_result();
 
 	save_offboard_mission_state();
 }
@@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item()
 void
 Mission::report_mission_finished()
 {
-	_mission_result.finished = true;
-	publish_mission_result();
+	_navigator->get_mission_result()->finished = true;
+	_navigator->publish_mission_result();
 }
 
-void
-Mission::publish_mission_result()
-{
-	/* lazily publish the mission result only once available */
-	if (_mission_result_pub > 0) {
-		/* publish mission result */
-		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
-
-	} else {
-		/* advertise and publish */
-		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
-	}
-	/* reset reached bool */
-	_mission_result.reached = false;
-	_mission_result.finished = false;
-}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4da6a1155..1b8c8c874 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -128,11 +128,6 @@ private:
 	 */
 	void report_mission_finished();
 
-	/**
-	 * Publish the mission result so commander and mavlink know what is going on
-	 */
-	void publish_mission_result();
-
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
@@ -145,9 +140,6 @@ private:
 	bool _need_takeoff;
 	bool _takeoff;
 
-	orb_advert_t _mission_result_pub;
-	struct mission_result_s _mission_result;
-
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_ONBOARD,
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d0b2ed841..363877bb8 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -52,6 +52,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/mission_result.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
@@ -102,6 +103,11 @@ public:
 	 */
 	void		load_fence_from_file(const char *filename);
 
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void publish_mission_result();
+
 	/**
 	 * Setters
 	 */
@@ -115,7 +121,9 @@ public:
 	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
-	struct position_setpoint_triplet_s*		    get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
+	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
+
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
 	int		get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
@@ -143,6 +151,7 @@ private:
 	int		_param_update_sub;		/**< param update subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
+	orb_advert_t	_mission_result_pub;
 
 	vehicle_status_s				_vstatus;		/**< vehicle status */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
@@ -152,6 +161,8 @@ private:
 	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
+	mission_result_s				_mission_result;
+
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1ce6770c9..77124e8f6 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -108,6 +108,7 @@ Navigator::Navigator() :
 	_offboard_mission_sub(-1),
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
+	_mission_result_pub(-1),
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
@@ -115,6 +116,7 @@ Navigator::Navigator() :
 	_mission_item{},
 	_nav_caps{},
 	_pos_sp_triplet{},
+	_mission_result{},
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence{},
@@ -370,6 +372,9 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
+				/* Some failsafe modes prohibit the fallback to mission
+				 * usually this is done after some time to make sure
+				 * that the full failsafe operation is performed */
 				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
@@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[])
 
 	return 0;
 }
+
+void
+Navigator::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.reached = false;
+	_mission_result.finished = false;
+}
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index f43215665..3c6754c55 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -63,6 +63,9 @@ NavigatorMode::run(bool active) {
 		if (_first_run) {
 			/* first run */
 			_first_run = false;
+			/* Reset stay in failsafe flag */
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
 			on_activation();
 
 		} else {
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index beb797e62..65ddfb4ad 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -57,6 +57,7 @@ struct mission_result_s
 	unsigned seq_current;		/**< Sequence of the current mission item				 */
 	bool reached;				/**< true if mission has been reached					 */
 	bool finished;				/**< true if mission has been completed					 */
+	bool stay_in_failsafe;			/**< true if the commander should not switch out of the failsafe mode*/
 };
 
 /**
-- 
cgit v1.2.3


From 303664d94af4949bcc7f1c3393f82eb242a60c5e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Jul 2014 21:49:22 +0200
Subject: add landengfail nav state

---
 src/modules/navigator/navigator_main.cpp | 3 +++
 src/modules/uORB/topics/vehicle_status.h | 1 +
 2 files changed, 4 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 21ab691ff..043d883b2 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -393,6 +393,9 @@ Navigator::task_main()
 					_navigation_mode = &_rtl; /* TODO: change this to something else */
 				}
 				break;
+			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_navigation_mode = &_engineFailure;
+				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			case NAVIGATION_STATE_OFFBOARD:
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index eda1dfaec..c2926dce8 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -103,6 +103,7 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
 	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
+	NAVIGATION_STATE_AUTO_LANDENGFAIL,	/**< Auto land on engine failure */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
-- 
cgit v1.2.3


From 1b2f9070ea063255aca4a882e4adda7f89e082e7 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Jul 2014 22:29:04 +0200
Subject: engine_failure flag

Added engine_failure flag to behicle status, alsoset_nav_state in the
state machine helper takes this flag into account
---
 src/modules/commander/state_machine_helper.cpp | 19 ++++++++++++++-----
 src/modules/uORB/topics/vehicle_status.h       |  6 ++++--
 2 files changed, 18 insertions(+), 7 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 4e1cfb987..bc6803596 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -490,9 +490,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 	case MAIN_STATE_AUTO_MISSION:
 		/* go into failsafe
+		 * - if we have an engine failure
 		 * - if either the datalink is enabled and lost as well as RC is lost
 		 * - if there is no datalink and the mission is finished */
-		if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
+		if (status->engine_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
 		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
 			status->failsafe = true;
 
@@ -532,8 +535,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_LOITER:
-		/* go into failsafe if datalink and RC is lost */
-		if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
+		/* go into failsafe on a engine failure or if datalink and RC is lost */
+		if (status->engine_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -586,8 +591,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_RTL:
-		/* require global position and home */
-		if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
+		/* require global position and home, also go into failsafe on an engine failure */
+
+		if (status->engine_failure) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if ((!status->condition_global_position_valid ||
+					!status->condition_home_position_valid)) {
 			status->failsafe = true;
 
 			if (status->condition_local_position_valid) {
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index c2926dce8..a47488621 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -201,8 +201,10 @@ struct vehicle_status_s {
 	bool rc_signal_lost;				/**< true if RC reception lost */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
-	bool data_link_lost;						/**< datalink to GCS lost */
-	uint8_t data_link_lost_counter;					/**< counts unique data link lost events */
+	bool data_link_lost;				/**< datalink to GCS lost */
+	uint8_t data_link_lost_counter;			/**< counts unique data link lost events */
+
+	bool engine_failure;				/** Set to true if an engine failure is detected */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
-- 
cgit v1.2.3


From 351598626054e5d853e8768d2f5d04226510c12a Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Jul 2014 23:44:38 +0200
Subject: define gps loss failsafe nav state

---
 src/modules/uORB/topics/vehicle_status.h | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index a47488621..e0072b52d 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -104,6 +104,7 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
 	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
 	NAVIGATION_STATE_AUTO_LANDENGFAIL,	/**< Auto land on engine failure */
+	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
 	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
-- 
cgit v1.2.3


From db5d668439be63e4c8fd7dab49b81c5e162ee095 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 27 Jul 2014 00:55:43 +0200
Subject: add simplistic gps failure detection

---
 src/modules/commander/commander.cpp      | 16 ++++++++++++++++
 src/modules/uORB/topics/vehicle_status.h |  1 +
 2 files changed, 17 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index fe1974c88..50beb5da8 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1135,6 +1135,22 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
 
 		/* check if GPS fix is ok */
+		if (gps_position.fix_type >= 3 && //XXX check eph and epv ?
+			hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
+			/* handle the case where gps was regained */
+			if (status.gps_failure) {
+				status.gps_failure = false;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "gps regained");
+			}
+		} else {
+			if (!status.gps_failure) {
+				status.gps_failure = true;
+				status_changed = true;
+				mavlink_log_critical(mavlink_fd, "gps fix lost");
+			}
+		}
+
 
 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 707abb545..0751b57fe 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -206,6 +206,7 @@ struct vehicle_status_s {
 	uint8_t data_link_lost_counter;			/**< counts unique data link lost events */
 
 	bool engine_failure;				/** Set to true if an engine failure is detected */
+	bool gps_failure;				/** Set to true if a gps failure is detected */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
-- 
cgit v1.2.3


From 020ac409be62e6c459e7aa5cb3181941c9bbfd23 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 3 Aug 2014 13:17:18 +0200
Subject: Remove airspeed and altitude raw data from TECS log, as we have these
 quantities already in the system log

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp |  6 ++----
 src/modules/sdlog2/sdlog2.c                   |  6 ++----
 src/modules/sdlog2/sdlog2_messages.h          |  4 +---
 src/modules/uORB/topics/tecs_status.h         | 12 ++++++------
 4 files changed, 11 insertions(+), 17 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 749f57a2b..bffeefc1f 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
-	_status.altitude = altitude;
-	_status.altitudeFiltered = altitudeFiltered;
+	_status.altitude_filtered = altitudeFiltered;
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
-	_status.airspeed = airspeed;
-	_status.airspeedFiltered = airspeedFiltered;
+	_status.airspeed_filtered = airspeedFiltered;
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 31861c3fc..6c4b49452 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
-			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
-			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
+			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
 			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
-			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
-			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
 			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 853a3811f..fb7609435 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -333,13 +333,11 @@ struct log_GS1B_s {
 #define LOG_TECS_MSG 30
 struct log_TECS_s {
 	float altitudeSp;
-	float altitude;
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
@@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffffB",	"ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index 33055018c..ddca19d61 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -51,11 +51,13 @@
  */
 
 typedef enum {
-	TECS_MODE_NORMAL,
+	TECS_MODE_NORMAL = 0,
 	TECS_MODE_UNDERSPEED,
 	TECS_MODE_TAKEOFF,
 	TECS_MODE_LAND,
-	TECS_MODE_LAND_THROTTLELIM
+	TECS_MODE_LAND_THROTTLELIM,
+	TECS_MODE_BAD_DESCENT,
+	TECS_MODE_CLIMBOUT
 } tecs_mode;
 
  /**
@@ -65,14 +67,12 @@ struct tecs_status_s {
 	uint64_t timestamp;		/**< timestamp, in microseconds since system start */
 
 	float altitudeSp;
-	float altitude;
-	float altitudeFiltered;
+	float altitude_filtered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
-	float airspeedFiltered;
+	float airspeed_filtered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 
-- 
cgit v1.2.3


From faaeaeb11333598abd17db2189a2bc47216d0a98 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 8 Aug 2014 00:22:57 +0200
Subject: mc_pos_control: manual and offboard control reorganization and
 cleanup

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 271 +++++++++++----------
 .../uORB/topics/position_setpoint_triplet.h        |   8 +-
 2 files changed, 140 insertions(+), 139 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d22f43b5a..a0837a2dd 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -105,6 +105,7 @@ public:
 	int		start();
 
 private:
+	const float alt_ctl_dz = 0.2f;
 
 	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
@@ -184,6 +185,8 @@ private:
 	math::Vector<3> _vel;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
 
 	/**
 	 * Update our local parameter cache.
@@ -216,6 +219,16 @@ private:
 	 */
 	void		reset_alt_sp();
 
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
 	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
@@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
 
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
@@ -515,6 +530,120 @@ MulticopterPositionControl::reset_alt_sp()
 	}
 }
 
+void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual.x;
+		_sp_move_rate(1) = _manual.y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
+	math::Matrix<3, 3> R_yaw_sp;
+	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* move position setpoint */
+	_pos_sp += _sp_move_rate * dt;
+
+	/* check if position setpoint is too far from actual position */
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode.flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	bool updated;
+	orb_check(_pos_sp_triplet_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+	}
+
+	if (_pos_sp_triplet.current.valid) {
+		if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet.current.x;
+			_pos_sp(1) = _pos_sp_triplet.current.y;
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+
+		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
+		}
+
+		if (_control_mode.flag_control_altitude_enabled) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet.current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
 void
 MulticopterPositionControl::task_main()
 {
@@ -553,13 +682,6 @@ MulticopterPositionControl::task_main()
 
 	hrt_abstime t_prev = 0;
 
-	const float alt_ctl_dz = 0.2f;
-
-	math::Vector<3> sp_move_rate;
-	sp_move_rate.zero();
-
-	float yaw_sp_move_rate;
-
 	math::Vector<3> thrust_int;
 	thrust_int.zero();
 	math::Matrix<3, 3> R;
@@ -618,138 +740,17 @@ MulticopterPositionControl::task_main()
 			_vel(1) = _local_pos.vy;
 			_vel(2) = _local_pos.vz;
 
-			sp_move_rate.zero();
+			_vel_ff.zero();
+			_sp_move_rate.zero();
 
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
-				if (_control_mode.flag_control_altitude_enabled) {
-					/* reset alt setpoint to current altitude if needed */
-					reset_alt_sp();
-
-					/* move altitude setpoint with throttle stick */
-					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
-				}
-
-				if (_control_mode.flag_control_position_enabled) {
-					/* reset position setpoint to current position if needed */
-					reset_pos_sp();
-
-					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = _manual.x;
-					sp_move_rate(1) = _manual.y;
-				}
-
-				/* limit setpoint move rate */
-				float sp_move_norm = sp_move_rate.length();
-
-				if (sp_move_norm > 1.0f) {
-					sp_move_rate /= sp_move_norm;
-				}
-
-				/* scale to max speed and rotate around yaw */
-				math::Matrix<3, 3> R_yaw_sp;
-				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-				/* move position setpoint */
-				_pos_sp += sp_move_rate * dt;
-
-				/* check if position setpoint is too far from actual position */
-				math::Vector<3> pos_sp_offs;
-				pos_sp_offs.zero();
-
-				if (_control_mode.flag_control_position_enabled) {
-					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-				}
-
-				if (_control_mode.flag_control_altitude_enabled) {
-					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-				}
-
-				float pos_sp_offs_norm = pos_sp_offs.length();
-
-				if (pos_sp_offs_norm > 1.0f) {
-					pos_sp_offs /= pos_sp_offs_norm;
-					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-				}
+				control_manual(dt);
 
 			} else if (_control_mode.flag_control_offboard_enabled) {
-				/* Offboard control */
-				bool updated;
-				orb_check(_pos_sp_triplet_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-				}
-
-				if (_pos_sp_triplet.current.valid) {
-
-					if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
-
-						_pos_sp(0) = _pos_sp_triplet.current.x;
-						_pos_sp(1) = _pos_sp_triplet.current.y;
-						_pos_sp(2) = _pos_sp_triplet.current.z;
-						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
-
-					} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
-						/* reset position setpoint to current position if needed */
-						reset_pos_sp();
-						/* move position setpoint with roll/pitch stick */
-						sp_move_rate(0) = _pos_sp_triplet.current.vx;
-						sp_move_rate(1) = _pos_sp_triplet.current.vy;
-						yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
-						_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						/* reset alt setpoint to current altitude if needed */
-						reset_alt_sp();
-
-						/* move altitude setpoint with throttle stick */
-						sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
-					}
-
-					/* limit setpoint move rate */
-					float sp_move_norm = sp_move_rate.length();
-
-					if (sp_move_norm > 1.0f) {
-						sp_move_rate /= sp_move_norm;
-					}
-
-					/* scale to max speed and rotate around yaw */
-					math::Matrix<3, 3> R_yaw_sp;
-					R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-					sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-					/* move position setpoint */
-					_pos_sp += sp_move_rate * dt;
-
-					/* check if position setpoint is too far from actual position */
-					math::Vector<3> pos_sp_offs;
-					pos_sp_offs.zero();
-
-					if (_control_mode.flag_control_position_enabled) {
-						pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-						pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-					}
-
-					float pos_sp_offs_norm = pos_sp_offs.length();
-
-					if (pos_sp_offs_norm > 1.0f) {
-						pos_sp_offs /= pos_sp_offs_norm;
-						_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-					}
-
-				} else {
-					reset_pos_sp();
-					reset_alt_sp();
-				}
+				/* offboard control */
+				control_offboard(dt);
 
 			} else {
 				/* AUTO */
@@ -823,7 +824,7 @@ MulticopterPositionControl::task_main()
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err = _pos_sp - _pos;
 
-				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+				_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
 
 				if (!_control_mode.flag_control_altitude_enabled) {
 					_reset_alt_sp = true;
@@ -903,7 +904,7 @@ MulticopterPositionControl::task_main()
 					math::Vector<3> vel_err = _vel_sp - _vel;
 
 					/* derivative of velocity error, not includes setpoint acceleration */
-					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
 					_vel_prev = _vel;
 
 					/* thrust vector in NED frame */
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 4a1932180..ec2131abd 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -60,7 +60,7 @@ enum SETPOINT_TYPE
 	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
 	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
 	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
-	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint set by offboard */
+	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
 };
 
 struct position_setpoint_s
@@ -71,9 +71,9 @@ struct position_setpoint_s
 	float y;			/**< local position setpoint in m in NED */
 	float z;			/**< local position setpoint in m in NED */
 	bool position_valid;	/**< true if local position setpoint valid */
-	float vx;			/**< local velocity setpoint in m in NED */
-	float vy;			/**< local velocity setpoint in m in NED */
-	float vz;			/**< local velocity setpoint in m in NED */
+	float vx;			/**< local velocity setpoint in m/s in NED */
+	float vy;			/**< local velocity setpoint in m/s in NED */
+	float vz;			/**< local velocity setpoint in m/s in NED */
 	bool velocity_valid;		/**< true if local velocity setpoint valid */
 	double lat;			/**< latitude, in deg */
 	double lon;			/**< longitude, in deg */
-- 
cgit v1.2.3


From 9ecec7fada7da3778c6214defcef68ea05352027 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Aug 2014 01:30:25 +0200
Subject: Add default initializers and timestamp in local position

---
 src/modules/mavlink/mavlink_messages.cpp                  | 7 ++++---
 src/modules/mc_pos_control/mc_pos_control_main.cpp        | 1 +
 src/modules/uORB/topics/vehicle_local_position_setpoint.h | 3 ++-
 3 files changed, 7 insertions(+), 4 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7f67475f2..c10be77b5 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1430,7 +1430,7 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			mavlink_position_target_global_int_t msg;
+			mavlink_position_target_global_int_t msg{};
 
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
 			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
@@ -1490,8 +1490,9 @@ protected:
 		struct vehicle_local_position_setpoint_s pos_sp;
 
 		if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
-			mavlink_position_target_local_ned_t msg;
+			mavlink_position_target_local_ned_t msg{};
 
+			msg.time_boot_ms = pos_sp.timestamp / 1000;
 			msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
 			msg.x = pos_sp.x;
 			msg.y = pos_sp.y;
@@ -1558,7 +1559,7 @@ protected:
 			struct vehicle_rates_setpoint_s att_rates_sp;
 			(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
 
-			mavlink_attitude_target_t msg;
+			mavlink_attitude_target_t msg{};
 
 			msg.time_boot_ms = att_sp.timestamp / 1000;
 			mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 0106af80a..ecc40428d 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -788,6 +788,7 @@ MulticopterPositionControl::task_main()
 			}
 
 			/* fill local position setpoint */
+			_local_pos_sp.timestamp = hrt_absolute_time();
 			_local_pos_sp.x = _pos_sp(0);
 			_local_pos_sp.y = _pos_sp(1);
 			_local_pos_sp.z = _pos_sp(2);
diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
index 8988a0330..6766bb58a 100644
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
@@ -50,11 +50,12 @@
  */
 
 struct vehicle_local_position_setpoint_s {
+	uint64_t timestamp;	/**< timestamp of the setpoint */
 	float x;		/**< in meters NED			  		*/
 	float y;		/**< in meters NED			  		*/
 	float z;		/**< in meters NED			  		*/
 	float yaw;		/**< in radians NED -PI..+PI  		*/
-}; /**< Local position in NED frame to go to */
+}; /**< Local position in NED frame */
 
 /**
  * @}
-- 
cgit v1.2.3


From a04d70ed5a800ff6d74afc6800f7bb94ed472ad4 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 12 Aug 2014 14:54:35 +0200
Subject: offboard setpoints: correctly check for force setpoint

---
 src/modules/mavlink/mavlink_receiver.cpp           |  6 ++++-
 .../uORB/topics/offboard_control_setpoint.h        | 30 ++++++++++++++++++++++
 2 files changed, 35 insertions(+), 1 deletion(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dfc5ddc91..418b81ee1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -441,7 +441,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 			}
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (offboard_control_sp.isForceSetpoint) {
+				if (offboard_control_sp.isForceSetpoint &&
+						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
+						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+					/* The offboard setpoint is a force setpoint only, directly writing to the force
+					 * setpoint topic and not publishing the setpoint triplet topic */
 					struct vehicle_force_setpoint_s	force_sp;
 					force_sp.x = offboard_control_sp.acceleration[0];
 					force_sp.y = offboard_control_sp.acceleration[1];
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 19f11ba92..4f45ac14f 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -104,14 +104,44 @@ struct offboard_control_setpoint_s {
  * @}
  */
 
+/**
+ * Returns true if the position setpoint at index should be ignored
+ */
 inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << index));
 }
 
+/**
+ * Returns true if all position setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the velocity setpoint at index should be ignored
+ */
 inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << (3 + index)));
 }
 
+/**
+ * Returns true if all velocity setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
 inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
 }
-- 
cgit v1.2.3


From 85eed22150e4a24098554992a6d77bce6f1ddf31 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 12 Aug 2014 16:27:02 +0200
Subject: offboard sp: correctly map acceleration/force in combined setpoint to
 setpoint triple

Conflicts:
	src/modules/uORB/topics/position_setpoint_triplet.h
---
 src/modules/mavlink/mavlink_receiver.cpp           | 39 +++++++++++++++-------
 .../uORB/topics/offboard_control_setpoint.h        | 21 ++++++++++++
 .../uORB/topics/position_setpoint_triplet.h        |  5 +++
 3 files changed, 53 insertions(+), 12 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 418b81ee1..da651e4e7 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -457,7 +457,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 						orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
 					}
 				} else {
-					/* It's not a force setpoint: publish to setpoint triplet  topic */
+					/* It's not a pure force setpoint: publish to setpoint triplet  topic */
 					struct position_setpoint_triplet_s pos_sp_triplet;
 					pos_sp_triplet.previous.valid = false;
 					pos_sp_triplet.next.valid = false;
@@ -466,29 +466,44 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					/* set the local pos values if the setpoint type is 'local pos' and none
 					 * of the local pos fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_position(offboard_control_sp, 0) &&
-							!offboard_control_sp_ignore_position(offboard_control_sp, 1) &&
-							!offboard_control_sp_ignore_position(offboard_control_sp, 2)) {
+							!offboard_control_sp_ignore_position_all(offboard_control_sp)) {
 					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 					pos_sp_triplet.current.position_valid = true;
 					pos_sp_triplet.current.x = offboard_control_sp.position[0];
 					pos_sp_triplet.current.y = offboard_control_sp.position[1];
 					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+					} else {
+						pos_sp_triplet.current.position_valid = false;
 					}
 
 					/* set the local vel values if the setpoint type is 'local pos' and none
 					 * of the local vel fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_velocity(offboard_control_sp, 0) &&
-							!offboard_control_sp_ignore_velocity(offboard_control_sp, 1) &&
-							!offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) {
-					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
-					pos_sp_triplet.current.velocity_valid = true;
-					pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
-					pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
-					pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+							!offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+						pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
+						pos_sp_triplet.current.velocity_valid = true;
+						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
+						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
+						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+					} else {
+						pos_sp_triplet.current.velocity_valid = false;
 					}
 
+					/* set the local acceleration values if the setpoint type is 'local pos' and none
+					 * of the accelerations fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) {
+						pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
+						pos_sp_triplet.current.acceleration_valid = true;
+						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
+						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
+						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						pos_sp_triplet.current.acceleration_is_force =
+						offboard_control_sp.isForceSetpoint;
+
+					} else {
+						pos_sp_triplet.current.acceleration_valid = false;
+					}
 					//XXX handle global pos setpoints (different MAV frames)
 
 
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 4f45ac14f..47c89f708 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -142,14 +142,35 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro
 	return false;
 }
 
+/**
+ * Returns true if the acceleration setpoint at index should be ignored
+ */
 inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
 }
 
+/**
+ * Returns true if all acceleration setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * Returns true if the bodyrate setpoint at index should be ignored
+ */
 inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
 	return (bool)(offboard_control_sp.ignore & (1 << (9 + index)));
 }
 
+/**
+ * Returns true if the attitude setpoint should be ignored
+ */
 inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
 	return (bool)(offboard_control_sp.ignore & (1 << 10));
 }
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index ec2131abd..3ea3f671e 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -83,6 +83,11 @@ struct position_setpoint_s
 	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 */
+	float a_x;			//**< acceleration x setpoint */
+	float a_y;			//**< acceleration y setpoint */
+	float a_z;			//**< acceleration z setpoint */
+	bool acceleration_valid;	//*< true if acceleration setpoint is valid/should be used */
+	bool acceleration_is_force;	//*< interprete acceleration as force */
 };
 
 /**
-- 
cgit v1.2.3


From 0f2b66fa8b3782c965966e9c0b0b6f16b80d7f38 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 14 Aug 2014 22:38:48 +0200
Subject: failsafe: enable support for commands

---
 src/modules/commander/commander.cpp            | 25 +++++++++++++++++++++++++
 src/modules/commander/state_machine_helper.cpp | 11 ++++++++++-
 src/modules/navigator/navigator_main.cpp       |  2 +-
 src/modules/uORB/topics/vehicle_status.h       |  5 ++++-
 4 files changed, 40 insertions(+), 3 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c6f56d5e3..b635a22cb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				armed_local->force_failsafe = false;
 				warnx("disabling failsafe");
 			}
+			/* param2 is currently used for other failsafe modes */
+			status_local->engine_failure_cmd = false;
+			status_local->data_link_lost_cmd = false;
+			status_local->gps_failure_cmd = false;
+			status_local->rc_signal_lost_cmd = false;
+			if ((int)cmd->param2 <= 0) {
+				/* reset all commanded failure modes */
+				warnx("revert to normal mode");
+			} else if ((int)cmd->param2 == 1) {
+				/* trigger engine failure mode */
+				status_local->engine_failure_cmd = true;
+				warnx("engine failure mode commanded");
+			} else if ((int)cmd->param2 == 2) {
+				/* trigger data link loss mode */
+				status_local->data_link_lost_cmd = true;
+				warnx("data link loss mode commanded");
+			} else if ((int)cmd->param2 == 3) {
+				/* trigger gps loss mode */
+				status_local->gps_failure_cmd = true;
+				warnx("gps loss mode commanded");
+			} else if ((int)cmd->param2 == 4) {
+				/* trigger rc loss mode */
+				status_local->rc_signal_lost_cmd = true;
+				warnx("rc loss mode commanded");
+			}
 			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index ecfe62e03..157e35ef8 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -491,10 +491,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 	case MAIN_STATE_AUTO_MISSION:
 		/* go into failsafe
+		 * - if commanded to do so
 		 * - if we have an engine failure
 		 * - if either the datalink is enabled and lost as well as RC is lost
 		 * - if there is no datalink and the mission is finished */
-		if (status->engine_failure) {
+		if (status->engine_failure_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
+		} else if (status->data_link_lost_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+		//} else if (status->gps_failure_cmd) {
+			//status->nav_state = NAVIGATION_STATE_AUTO_***;
+		} else if (status->rc_signal_lost_cmd) {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
+		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
 		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 043d883b2..4af37fa3e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -384,7 +384,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_RTL:
 				_navigation_mode = &_rtl;
 				break;
-			case NAVIGATION_STATE_AUTO_RTGS:
+			case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
 				if (_param_datalinkloss_obc.get() != 0) {
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 301503b82..b465f8407 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -200,13 +200,16 @@ struct vehicle_status_s {
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	bool rc_signal_lost_cmd;				/**< true if RC lost mode is commanded */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
 	bool data_link_lost;				/**< datalink to GCS lost */
+	bool data_link_lost_cmd;			/**< datalink to GCS lost mode commanded */
 	uint8_t data_link_lost_counter;			/**< counts unique data link lost events */
-
 	bool engine_failure;				/** Set to true if an engine failure is detected */
+	bool engine_failure_cmd;			/** Set to true if an engine failure mode is commanded */
 	bool gps_failure;				/** Set to true if a gps failure is detected */
+	bool gps_failure_cmd;				/** Set to true if a gps failure mode is commanded */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
-- 
cgit v1.2.3


From 1e9d77f1d2dfd3f500c8486edf8940bc4a15162e Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 15 Aug 2014 12:58:06 +0200
Subject: offboard: ignore mask cleanup

add handling of thrust ignore bit, add defines for the bitnumbers,
improve and fix interface to ignore bitmask
---
 src/modules/mavlink/mavlink_receiver.cpp           | 29 ++++---
 .../uORB/topics/offboard_control_setpoint.h        | 90 ++++++++++++++++++++--
 2 files changed, 100 insertions(+), 19 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7dd043bbe..6b3c3accb 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -472,7 +472,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					/* set the local pos values if the setpoint type is 'local pos' and none
 					 * of the local pos fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_position_all(offboard_control_sp)) {
+							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
 					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 					pos_sp_triplet.current.position_valid = true;
 					pos_sp_triplet.current.x = offboard_control_sp.position[0];
@@ -485,7 +485,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					/* set the local vel values if the setpoint type is 'local pos' and none
 					 * of the local vel fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
 						pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 						pos_sp_triplet.current.velocity_valid = true;
 						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
@@ -498,7 +498,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					/* set the local acceleration values if the setpoint type is 'local pos' and none
 					 * of the accelerations fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
-							!offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) {
+							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
 						pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 						pos_sp_triplet.current.acceleration_valid = true;
 						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
@@ -589,12 +589,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
 		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
 
+		/* set correct ignore flags for body rate fields: copy from mavlink message */
 		for (int i = 0; i < 3; i++) {
-			offboard_control_sp.ignore &=  ~(1 << (i + 9));
-			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << 9;
+			offboard_control_sp.ignore &=  ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
+			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
 		}
-		offboard_control_sp.ignore &= ~(1 << 10);
-		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3);
+		/* set correct ignore flags for thrust field: copy from mavlink message */
+		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
+		/* set correct ignore flags for attitude field: copy from mavlink message */
+		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
 
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
@@ -618,8 +623,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 
 			if (_control_mode.flag_control_offboard_enabled) {
 
-				/* Publish attitude setpoint if ignore bit is not set */
-				if (!(set_attitude_target.type_mask & (1 << 7))) {
+				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
+				if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
+							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
 					struct vehicle_attitude_setpoint_s att_sp;
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
@@ -634,9 +640,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					}
 				}
 
-				/* Publish attitude rate setpoint if ignore bit are not set */
+				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
 				///XXX add support for ignoring individual axes
-				if (!(set_attitude_target.type_mask & (0b111))) {
+				if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
+							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
 					struct vehicle_rates_setpoint_s rates_sp;
 					rates_sp.timestamp = hrt_absolute_time();
 					rates_sp.roll = set_attitude_target.body_roll_rate;
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 47c89f708..6e37896af 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -61,7 +61,7 @@ enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
 	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
 	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
-	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
 enum OFFBOARD_CONTROL_FRAME {
@@ -72,6 +72,23 @@ enum OFFBOARD_CONTROL_FRAME {
 	OFFBOARD_CONTROL_FRAME_GLOBAL = 4
 };
 
+/* mappings for the ignore bitmask */
+enum {OFB_IGN_BIT_POS_X,
+	OFB_IGN_BIT_POS_Y,
+	OFB_IGN_BIT_POS_Z,
+	OFB_IGN_BIT_VEL_X,
+	OFB_IGN_BIT_VEL_Y,
+	OFB_IGN_BIT_VEL_Z,
+	OFB_IGN_BIT_ACC_X,
+	OFB_IGN_BIT_ACC_Y,
+	OFB_IGN_BIT_ACC_Z,
+	OFB_IGN_BIT_BODYRATE_X,
+	OFB_IGN_BIT_BODYRATE_Y,
+	OFB_IGN_BIT_BODYRATE_Z,
+	OFB_IGN_BIT_ATT,
+	OFB_IGN_BIT_THRUST
+};
+
 /**
  * @addtogroup topics
  * @{
@@ -87,9 +104,11 @@ struct offboard_control_setpoint_s {
 	float acceleration[3];	/**< x acc, y acc, z acc */
 	float attitude[4];	/**< attitude of vehicle (quaternion) */
 	float attitude_rate[3];	/**< body angular rates (x, y, z) */
+	float thrust;	/**< thrust */
+
+	uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
+			   for mapping */
 
-	uint16_t ignore; /**< if field i is set to true, pi should be ignored */
-	//XXX define constants for bit offsets
 	bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
 
 	float override_mode_switch;
@@ -108,13 +127,25 @@ struct offboard_control_setpoint_s {
  * Returns true if the position setpoint at index should be ignored
  */
 inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << index));
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X +  index)));
 }
 
 /**
  * Returns true if all position setpoints should be ignored
  */
 inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (!offboard_control_sp_ignore_position(offboard_control_sp, i))	{
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * Returns true if some position setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
 	for (int i = 0; i < 3; i++) {
 		if (offboard_control_sp_ignore_position(offboard_control_sp, i))	{
 			return true;
@@ -127,13 +158,25 @@ inline bool offboard_control_sp_ignore_position_all(const struct offboard_contro
  * Returns true if the velocity setpoint at index should be ignored
  */
 inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (3 + index)));
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
 }
 
 /**
  * Returns true if all velocity setpoints should be ignored
  */
 inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * Returns true if some velocity setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
 	for (int i = 0; i < 3; i++) {
 		if (offboard_control_sp_ignore_velocity(offboard_control_sp, i))	{
 			return true;
@@ -146,13 +189,25 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro
  * Returns true if the acceleration setpoint at index should be ignored
  */
 inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (6 + index)));
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
 }
 
 /**
  * Returns true if all acceleration setpoints should be ignored
  */
 inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
+			return false;
+		}
+	}
+	return true;
+}
+
+/**
+ * Returns true if some acceleration setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
 	for (int i = 0; i < 3; i++) {
 		if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i))	{
 			return true;
@@ -165,14 +220,33 @@ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_co
  * Returns true if the bodyrate setpoint at index should be ignored
  */
 inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
-	return (bool)(offboard_control_sp.ignore & (1 << (9 + index)));
+	return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
+}
+
+/**
+ * Returns true if some of the bodyrate setpoints should be ignored
+ */
+inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	for (int i = 0; i < 3; i++) {
+		if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i))	{
+			return true;
+		}
+	}
+	return false;
 }
 
 /**
  * Returns true if the attitude setpoint should be ignored
  */
 inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
-	return (bool)(offboard_control_sp.ignore & (1 << 10));
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
+}
+
+/**
+ * Returns true if the thrust setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
 }
 
 
-- 
cgit v1.2.3


From 752a0a562564ccc6f7d49ceebe810de7e6a6d358 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 22 Aug 2014 00:40:45 +0200
Subject: add obc gps failure mode

---
 src/modules/commander/commander.cpp                | 18 +++-
 src/modules/navigator/gpsfailure.cpp               | 95 +++++++++++----------
 src/modules/navigator/gpsfailure.h                 | 13 ++-
 src/modules/navigator/gpsfailure_params.c          | 97 ++++++++++++++++++++++
 src/modules/navigator/module.mk                    |  3 +-
 src/modules/navigator/navigator.h                  | 12 +++
 src/modules/navigator/navigator_main.cpp           | 16 ++++
 .../uORB/topics/position_setpoint_triplet.h        |  1 +
 8 files changed, 207 insertions(+), 48 deletions(-)
 create mode 100644 src/modules/navigator/gpsfailure_params.c

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 84a4be948..109ab403a 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1311,11 +1311,12 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
 
 			/* Check for geofence violation */
-			if (pos_sp_triplet.geofence_violated) {
+			if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) {
 				//XXX: make this configurable to select different actions (e.g. navigation modes)
 				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
+				armed.fosrce_failsafe = true;
 				status_changed = true;
+				warnx("Flight termination");
 			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
@@ -2060,6 +2061,7 @@ set_control_mode()
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
 	case NAVIGATION_STATE_AUTO_RTGS:
+	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
 		control_mode.flag_control_rates_enabled = true;
@@ -2071,6 +2073,18 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index 4c526b76e..02e766ffb 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -57,7 +57,12 @@
 
 GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_gpsf_state(GPSF_STATE_NONE)
+	_param_loitertime(this, "LT"),
+	_param_openlooploiter_roll(this, "R"),
+	_param_openlooploiter_pitch(this, "P"),
+	_param_openlooploiter_thrust(this, "TR"),
+	_gpsf_state(GPSF_STATE_NONE),
+	_timestamp_activation(0)
 {
 	/* load initial params */
 	updateParams();
@@ -82,6 +87,7 @@ void
 GpsFailure::on_activation()
 {
 	_gpsf_state = GPSF_STATE_NONE;
+	_timestamp_activation = hrt_absolute_time();
 	updateParams();
 	advance_gpsf();
 	set_gpsf_item();
@@ -90,10 +96,34 @@ GpsFailure::on_activation()
 void
 GpsFailure::on_active()
 {
-	if (is_mission_item_reached()) {
-		updateParams();
-		advance_gpsf();
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_LOITER: {
+		/* Position controller does not run in this mode:
+		 * navigator has to publish an attitude setpoint */
+		_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
+		_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
+		_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
+		_navigator->publish_att_sp();
+
+		/* Measure time */
+		hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
+
+		//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
+				//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
+		if (elapsed > _param_loitertime.get() * 1e6f) {
+			/* no recovery, adavance the state machine */
+			warnx("gps not recovered, switch to next state");
+			advance_gpsf();
+		}
+		break;
+	}
+	case GPSF_STATE_TERMINATE:
 		set_gpsf_item();
+		advance_gpsf();
+		break;
+	default:
+		break;
 	}
 }
 
@@ -102,68 +132,45 @@ GpsFailure::set_gpsf_item()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	set_previous_pos_setpoint();
-	_navigator->set_can_loiter_at_sp(false);
+	/* Set pos sp triplet to invalid to stop pos controller */
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = false;
+	pos_sp_triplet->next.valid = false;
 
 	switch (_gpsf_state) {
-	case GPSF_STATE_LOITER: {
-		//_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
-		//_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
-		//_mission_item.altitude_is_relative = false;
-		//_mission_item.altitude = _param_commsholdalt.get();
-		//_mission_item.yaw = NAN;
-		//_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		//_mission_item.loiter_direction = 1;
-		//_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		//_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		//_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
-		//_mission_item.pitch_min = 0.0f;
-		//_mission_item.autocontinue = true;
-		//_mission_item.origin = ORIGIN_ONBOARD;
-
-		//_navigator->set_can_loiter_at_sp(true);
-		break;
-	}
 	case GPSF_STATE_TERMINATE: {
-		//_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
-		//_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
-		//_mission_item.altitude_is_relative = false;
-		//_mission_item.altitude = _param_airfieldhomealt.get();
-		//_mission_item.yaw = NAN;
-		//_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		//_mission_item.loiter_direction = 1;
-		//_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-		//_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		//_mission_item.pitch_min = 0.0f;
-		//_mission_item.autocontinue = true;
-		//_mission_item.origin = ORIGIN_ONBOARD;
-
-		//_navigator->set_can_loiter_at_sp(true);
-		break;
+		/* Request flight termination from the commander */
+		pos_sp_triplet->flight_termination = true;
+		warnx("request flight termination");
 	}
 	default:
 		break;
 	}
 
 	reset_mission_item_reached();
-
-	/* convert mission item to current position setpoint and make it valid */
-	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-	pos_sp_triplet->next.valid = false;
-
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
 void
 GpsFailure::advance_gpsf()
 {
+	updateParams();
+
 	switch (_gpsf_state) {
 	case GPSF_STATE_NONE:
 		_gpsf_state = GPSF_STATE_LOITER;
+		warnx("gpsf loiter");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
 		break;
 	case GPSF_STATE_LOITER:
 		_gpsf_state = GPSF_STATE_TERMINATE;
+		warnx("gpsf terminate");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
+		warnx("mavlink sent");
 		break;
+	case GPSF_STATE_TERMINATE:
+		warnx("gpsf end");
+		_gpsf_state = GPSF_STATE_END;
 	default:
 		break;
 	}
diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h
index 88d386ec4..e9e72babf 100644
--- a/src/modules/navigator/gpsfailure.h
+++ b/src/modules/navigator/gpsfailure.h
@@ -43,7 +43,11 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-#include <uORB/Subscription.hpp>
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+
+#include <drivers/drv_hrt.h>
 
 #include "navigator_mode.h"
 #include "mission_block.h"
@@ -65,13 +69,20 @@ public:
 
 private:
 	/* Params */
+	control::BlockParamFloat _param_loitertime;
+	control::BlockParamFloat _param_openlooploiter_roll;
+	control::BlockParamFloat _param_openlooploiter_pitch;
+	control::BlockParamFloat _param_openlooploiter_thrust;
 
 	enum GPSFState {
 		GPSF_STATE_NONE = 0,
 		GPSF_STATE_LOITER = 1,
 		GPSF_STATE_TERMINATE = 2,
+		GPSF_STATE_END = 3,
 	} _gpsf_state;
 
+	hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+
 	/**
 	 * Set the GPSF item
 	 */
diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c
new file mode 100644
index 000000000..39d179eed
--- /dev/null
+++ b/src/modules/navigator/gpsfailure_params.c
@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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 gpsfailure_params.c
+ *
+ * Parameters for GPSF navigation mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * GPS Failure Navigation Mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter time
+ *
+ * The amount of time in seconds the system should do open loop loiter and wait for gps recovery
+ * before it goes into flight termination.
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+
+/**
+ * Open loop loiter roll
+ *
+ * Roll in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
+
+/**
+ * Open loop loiter pitch
+ *
+ * Pitch in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min -30.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
+
+/**
+ * Open loop loiter thrust
+ *
+ * Thrust value which is set during the open loop loiter
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+
+
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index f6590605b..9e4ad053c 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -53,7 +53,8 @@ SRCS		= navigator_main.cpp \
 		  datalinkloss.cpp \
 		  enginefailure.cpp \
 		  datalinkloss_params.c \
-		  gpsfailure.cpp
+		  gpsfailure.cpp \
+		  gpsfailure_params.c
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index b161c2984..ec6e538e3 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -53,6 +53,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
@@ -108,6 +109,12 @@ public:
 	 * Publish the mission result so commander and mavlink know what is going on
 	 */
 	void publish_mission_result();
+	
+	/**
+	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
+	 * Example: mode that is triggered on gps failure
+	 */
+	void publish_att_sp();
 
 	/**
 	 * Setters
@@ -125,6 +132,7 @@ public:
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
+	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
 
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
@@ -155,6 +163,9 @@ private:
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
+	orb_advert_t	_att_sp_pub;			/**< publish att sp
+							  used only in very special failsafe modes
+							  when pos control is deactivated */
 
 	vehicle_status_s				_vstatus;		/**< vehicle status */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
@@ -166,6 +177,7 @@ private:
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
 	mission_result_s				_mission_result;
+	vehicle_attitude_setpoint_s					_att_sp;
 
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index bc9951bf2..e0913bb57 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -110,6 +110,7 @@ Navigator::Navigator() :
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
+	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
@@ -119,6 +120,7 @@ Navigator::Navigator() :
 	_nav_caps{},
 	_pos_sp_triplet{},
 	_mission_result{},
+	_att_sp{},
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence{},
@@ -609,3 +611,17 @@ Navigator::publish_mission_result()
 	_mission_result.reached = false;
 	_mission_result.finished = false;
 }
+
+void
+Navigator::publish_att_sp()
+{
+	/* lazily publish the attitude sp only once available */
+	if (_att_sp_pub > 0) {
+		/* publish att sp*/
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+	} else {
+		/* advertise and publish */
+		_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+	}
+}
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 1c78f5330..e2b1525a2 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -98,6 +98,7 @@ struct position_setpoint_triplet_s
 
 	unsigned nav_state;				/**< report the navigation state */
 	bool geofence_violated;				/**< true if the geofence is violated */
+	bool flight_termination;			/**< true if the navigator demands a flight termination from the commander app */
 };
 
 /**
-- 
cgit v1.2.3


From 127948f32f24e36c0c03a047b57fcd64287d9967 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 24 Aug 2014 11:49:45 +0200
Subject: Remove absolute pressure field as its not useful and confusing
 anywary

---
 src/drivers/ets_airspeed/ets_airspeed.cpp       | 20 ++++++--------------
 src/drivers/meas_airspeed/meas_airspeed.cpp     | 22 +++++-----------------
 src/modules/sensors/sensors.cpp                 | 14 +++++++-------
 src/modules/uORB/topics/differential_pressure.h |  1 -
 4 files changed, 18 insertions(+), 39 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index f98d615a2..0f77bb805 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -155,7 +155,6 @@ ETSAirspeed::collect()
 	}
 
 	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
-	uint16_t diff_pres_pa;
         if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
 		return -1;
         }
 
-	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
-		diff_pres_pa = 0;
-	} else {
-		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
-	}
-
 	// The raw value still should be compensated for the known offset
 	diff_pres_pa_raw -= _diff_pres_offset;
 
 	// Track maximum differential pressure measured (so we can work out top speed).
-	if (diff_pres_pa > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_pres_pa;
+	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_pres_pa_raw;
 	}
 
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_pa = (float)diff_pres_pa;
 
 	// XXX we may want to smooth out the readings to remove noise.
-	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
-	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
+	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
+	report.differential_pressure_raw_pa = diff_pres_pa_raw;
 	report.temperature = -1000.0f;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -369,7 +361,7 @@ test()
 		err(1, "immediate read failed");
 
 	warnx("single read");
-	warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
 			err(1, "periodic read failed");
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 	}
 
 	/* reset the sensor polling to its default rate */
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 479fa3ccf..1d9a463ad 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -228,18 +228,7 @@ MEASAirspeed::collect()
 	// the raw value still should be compensated for the known offset
 	diff_press_pa_raw -= _diff_pres_offset;
 
-	/* don't take the absolute value because the calibration takes this into account and warns the user if the
-	 * tubes are connected backwards */
-	float diff_press_pa = diff_press_pa_raw;
-
 	/*
-	  note that we return both the absolute value with offset
-	  applied and a raw value without the offset applied. This
-	  makes it possible for higher level code to detect if the
-	  user has the tubes connected backwards, and also makes it
-	  possible to correctly use offsets calculated by a higher
-	  level airspeed driver.
-
 	  With the above calculation the MS4525 sensor will produce a
 	  positive number when the top port is used as a dynamic port
 	  and bottom port is used as the static port
@@ -248,15 +237,14 @@ MEASAirspeed::collect()
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
-	if (diff_press_pa > _max_differential_pressure_pa) {
-		_max_differential_pressure_pa = diff_press_pa;
+	if (diff_press_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_press_pa_raw;
 	}
 
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
-	report.differential_pressure_pa = diff_press_pa;
-	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);
 
 	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -514,7 +502,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -542,7 +530,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f40034d79..cdcb428dd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1229,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 	if (updated) {
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
-		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
+		raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa;
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
 		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
 
 		_airspeed.timestamp = _diff_pres.timestamp;
-		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
-		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+
+		/* don't risk to feed negative airspeed into the system */
+		_airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa));
+		_airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
 		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
@@ -1457,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) {
 
 						float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa;
-						float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f;
 
 						_diff_pres.timestamp = t;
-						_diff_pres.differential_pressure_pa = diff_pres_pa;
 						_diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw;
-						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f);
+						_diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f);
 						_diff_pres.temperature = -1000.0f;
 
 						/* announce the airspeed if needed, just publish else */
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index cd48d3cb2..7342fcf04 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -54,7 +54,6 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
 	uint64_t	error_count;			/**< Number of errors detected by driver */
-	float	differential_pressure_pa;		/**< Differential pressure reading */
 	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
-- 
cgit v1.2.3


From c0975af375c168be98804f025192bbb30710355d Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 24 Aug 2014 13:44:43 +0200
Subject: commander: check if baro is healthy

---
 src/modules/commander/commander.cpp      | 19 +++++++++++++++++++
 src/modules/uORB/topics/vehicle_status.h |  2 ++
 2 files changed, 21 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0bf5cfe34..ddfbd65a1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1081,6 +1081,25 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+			/* Check if the barometer is healthy and issue a warning in the GCS if not so.
+			 * Because the barometer is used for calculating AMSL altitude which is used to ensure
+			 * vertical separation from other airtraffic the operator has to know when the
+			 * barometer is inoperational.
+			 * */
+			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
+				/* handle the case where baro was regained */
+				if (status.barometer_failure) {
+					status.barometer_failure = false;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "baro healthy");
+				}
+			} else {
+				if (!status.barometer_failure) {
+					status.barometer_failure = true;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "baro failed");
+				}
+			}
 		}
 
 		orb_check(diff_pres_sub, &updated);
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b465f8407..ad92f5b26 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -211,6 +211,8 @@ struct vehicle_status_s {
 	bool gps_failure;				/** Set to true if a gps failure is detected */
 	bool gps_failure_cmd;				/** Set to true if a gps failure mode is commanded */
 
+	bool barometer_failure;				/** Set to true if a barometer failure is detected */
+
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
-- 
cgit v1.2.3


From 06317046f2da215db328be660f900d265cdf9102 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 24 Aug 2014 17:45:15 +0200
Subject: move flight termination and geofence flags from setpoint triplet to
 mission result

---
 src/modules/commander/commander.cpp                | 18 ++++++-------
 src/modules/navigator/datalinkloss.cpp             | 31 ++++++++++++----------
 src/modules/navigator/gpsfailure.cpp               |  3 ++-
 src/modules/navigator/navigator_main.cpp           | 16 +++++------
 src/modules/navigator/rcloss.cpp                   |  3 ++-
 src/modules/uORB/topics/mission_result.h           |  8 +++---
 .../uORB/topics/position_setpoint_triplet.h        |  2 --
 7 files changed, 41 insertions(+), 40 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ddfbd65a1..f05f970e5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1328,15 +1328,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
-
-			/* Check for geofence violation */
-			if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) {
-				//XXX: make this configurable to select different actions (e.g. navigation modes)
-				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
-				status_changed = true;
-				warnx("Flight termination because of navigator request or geofence");
-			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
@@ -1429,6 +1420,15 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+
+			/* Check for geofence violation */
+			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
+				//XXX: make this configurable to select different actions (e.g. navigation modes)
+				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+				armed.force_failsafe = true;
+				status_changed = true;
+				warnx("Flight termination because of navigator request or geofence");
+			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		/* RC input check */
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 4e3d25840..66f1c8c73 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item()
 	}
 	case DLL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
-		pos_sp_triplet->flight_termination = true;
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -180,23 +181,25 @@ DataLinkLoss::advance_dll()
 	switch (_dll_state) {
 	case DLL_STATE_NONE:
 		/* Check the number of data link losses. If above home fly home directly */
-		if (!_param_skipcommshold.get()) {
-			/* if number of data link losses limit is not reached fly to comms hold wp */
-			if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
-				warnx("%d data link losses, limit is %d, fly to airfield home",
-						_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
-				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
-				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
-			} else {
+		/* if number of data link losses limit is not reached fly to comms hold wp */
+		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			warnx("%d data link losses, limit is %d, fly to airfield home",
+					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		} else {
+			if (!_param_skipcommshold.get()) {
 				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
 				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
 				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			} else {
+				/* comms hold wp not active, fly to airfield home directly */
+				warnx("Skipping comms hold wp. Flying directly to airfield home");
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 			}
-		} else {
-			/* comms hold wp not active, fly to airfield home directly */
-			warnx("Skipping comms hold wp. Flying directly to airfield home");
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
-			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index f0bbbcf1c..cd55f60b0 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item()
 	switch (_gpsf_state) {
 	case GPSF_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
-		pos_sp_triplet->flight_termination = true;
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
 		warnx("gps fail: request flight termination");
 	}
 	default:
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 81ceaaca2..c173ecd50 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -395,11 +395,9 @@ Navigator::task_main()
 		if (have_geofence_position_data) {
 			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
 			if (!inside) {
-				/* inform other apps via the sp triplet */
-				_pos_sp_triplet.geofence_violated = true;
-				if (_pos_sp_triplet.geofence_violated != true) {
-					_pos_sp_triplet_updated = true;
-				}
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = true;
+				publish_mission_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -407,11 +405,9 @@ Navigator::task_main()
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
-				/* inform other apps via the sp triplet */
-				_pos_sp_triplet.geofence_violated = false;
-				if (_pos_sp_triplet.geofence_violated != false) {
-					_pos_sp_triplet_updated = true;
-				}
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = false;
+				publish_mission_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 54f1813f5..651e31184 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -127,7 +127,8 @@ RCLoss::set_rcl_item()
 	}
 	case RCL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
-		pos_sp_triplet->flight_termination = true;
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
 		warnx("rc not recovered: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index 65ddfb4ad..c7d25d1f0 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -55,9 +55,11 @@ struct mission_result_s
 {
 	unsigned seq_reached;		/**< Sequence of the mission item which has been reached */
 	unsigned seq_current;		/**< Sequence of the current mission item				 */
-	bool reached;				/**< true if mission has been reached					 */
-	bool finished;				/**< true if mission has been completed					 */
-	bool stay_in_failsafe;			/**< true if the commander should not switch out of the failsafe mode*/
+	bool reached;			/**< true if mission has been reached					 */
+	bool finished;			/**< true if mission has been completed					 */
+	bool stay_in_failsafe;		/**< true if the commander should not switch out of the failsafe mode*/
+	bool geofence_violated;		/**< true if the geofence is violated */
+	bool flight_termination;	/**< true if the navigator demands a flight termination from the commander app */
 };
 
 /**
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index e2b1525a2..ec2131abd 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -97,8 +97,6 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s next;
 
 	unsigned nav_state;				/**< report the navigation state */
-	bool geofence_violated;				/**< true if the geofence is violated */
-	bool flight_termination;			/**< true if the navigator demands a flight termination from the commander app */
 };
 
 /**
-- 
cgit v1.2.3


From 55fde23233de3d311d6c8d0b2cd368e45af3caac Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 25 Aug 2014 09:19:36 +0200
Subject: support new yaw and yawrate fields in mavlnk position_target message

---
 src/modules/mavlink/mavlink_receiver.cpp           | 35 +++++++++++++++++++---
 .../uORB/topics/offboard_control_setpoint.h        | 22 +++++++++++++-
 .../uORB/topics/position_setpoint_triplet.h        |  2 ++
 3 files changed, 54 insertions(+), 5 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 60fc2937a..3ac4d3b03 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -474,6 +474,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
 		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
 		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
+		offboard_control_sp.yaw = set_position_target_local_ned.yaw;
+		offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
 		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
 
 		/* If we are in force control mode, for now set offboard mode to force control */
@@ -486,7 +488,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
-
+		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
+					OFB_IGN_BIT_YAW;
+		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
+					OFB_IGN_BIT_YAWRATE;
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -527,12 +534,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					pos_sp_triplet.previous.valid = false;
 					pos_sp_triplet.next.valid = false;
 					pos_sp_triplet.current.valid = true;
+					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 
 					/* set the local pos values if the setpoint type is 'local pos' and none
 					 * of the local pos fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
 							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
-					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 					pos_sp_triplet.current.position_valid = true;
 					pos_sp_triplet.current.x = offboard_control_sp.position[0];
 					pos_sp_triplet.current.y = offboard_control_sp.position[1];
@@ -545,7 +552,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					 * of the local vel fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
 							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
-						pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 						pos_sp_triplet.current.velocity_valid = true;
 						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
 						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
@@ -558,7 +564,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					 * of the accelerations fields is set to 'ignore' */
 					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
 							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
-						pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
 						pos_sp_triplet.current.acceleration_valid = true;
 						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
 						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
@@ -569,6 +574,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 					} else {
 						pos_sp_triplet.current.acceleration_valid = false;
 					}
+
+					/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
+					 * field is not set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
+						pos_sp_triplet.current.yaw_valid = true;
+						pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
+
+					} else {
+						pos_sp_triplet.current.yaw_valid = false;
+					}
+
+					/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
+					 * field is not set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
+						pos_sp_triplet.current.yawspeed_valid = true;
+						pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
+
+					} else {
+						pos_sp_triplet.current.yawspeed_valid = false;
+					}
 					//XXX handle global pos setpoints (different MAV frames)
 
 
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 6e37896af..72a28e501 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -86,7 +86,9 @@ enum {OFB_IGN_BIT_POS_X,
 	OFB_IGN_BIT_BODYRATE_Y,
 	OFB_IGN_BIT_BODYRATE_Z,
 	OFB_IGN_BIT_ATT,
-	OFB_IGN_BIT_THRUST
+	OFB_IGN_BIT_THRUST,
+	OFB_IGN_BIT_YAW,
+	OFB_IGN_BIT_YAWRATE,
 };
 
 /**
@@ -105,6 +107,10 @@ struct offboard_control_setpoint_s {
 	float attitude[4];	/**< attitude of vehicle (quaternion) */
 	float attitude_rate[3];	/**< body angular rates (x, y, z) */
 	float thrust;	/**< thrust */
+	float yaw;	/**< yaw: this is the yaw from the position_target message
+			  (not from the full attitude_target message) */
+	float yaw_rate;	/**< yaw rate: this is the yaw from the position_target message
+			  (not from the full attitude_target message) */
 
 	uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
 			   for mapping */
@@ -249,6 +255,20 @@ inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setp
 	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
 }
 
+/**
+ * Returns true if the yaw setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
+}
+
+/**
+ * Returns true if the yaw rate setpoint should be ignored
+ */
+inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
+	return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
+}
+
 
 /* register this as object request broker structure */
 ORB_DECLARE(offboard_control_setpoint);
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 3ea3f671e..cb2262534 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -79,7 +79,9 @@ struct position_setpoint_s
 	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 */
+	bool yaw_valid;			/**< true if yaw setpoint valid */
 	float yawspeed;			/**< yawspeed (only for multirotors, in rad/s) */
+	bool yawspeed_valid;		/**< true if yawspeed setpoint valid */
 	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 */
-- 
cgit v1.2.3


From 4af4e4e1e5d7209819c1fb43508ddd1ebed4f9c7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 26 Aug 2014 10:14:36 +0200
Subject: Support additional payload commands and let commander ignore them

---
 src/modules/commander/commander.cpp       |  5 +++++
 src/modules/uORB/topics/vehicle_command.h | 19 ++++++++++++-------
 2 files changed, 17 insertions(+), 7 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 74deda8cc..a5a772825 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
 	case VEHICLE_CMD_PREFLIGHT_STORAGE:
+	case VEHICLE_CMD_CUSTOM_0:
+	case VEHICLE_CMD_CUSTOM_1:
+	case VEHICLE_CMD_CUSTOM_2:
+	case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+	case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
 		/* ignore commands that handled in low prio loop */
 		break;
 
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index 7db33d98b..44aa50572 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 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>
+ *   Copyright (C) 2012-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
@@ -37,6 +34,10 @@
 /**
  * @file vehicle_command.h
  * Definition of the vehicle command uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_VEHICLE_COMMAND_H_
@@ -52,6 +53,9 @@
  * but can contain additional ones.
  */
 enum VEHICLE_CMD {
+	VEHICLE_CMD_CUSTOM_0 = 0, /* test command */
+	VEHICLE_CMD_CUSTOM_1 = 1, /* test command */
+	VEHICLE_CMD_CUSTOM_2 = 2, /* test command */
 	VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
@@ -87,7 +91,8 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
 	VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
 	VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
-	VEHICLE_CMD_ENUM_END = 501, /*  | */
+	VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */
+	VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */
 };
 
 /**
@@ -115,8 +120,8 @@ struct vehicle_command_s {
 	float param2;			/**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param3;			/**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param4;			/**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum.   */
-	float param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
-	float param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
+	double param5;			/**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum.   */
+	double param6;			/**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param7;			/**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum.   */
 	enum VEHICLE_CMD command;		/**< Command ID, as defined MAVLink by VEHICLE_CMD enum.   */
 	uint8_t target_system;		/**< System which should execute the command  */
-- 
cgit v1.2.3


From ccdfab245bac02565521f2d76e604f54ba3f5bd5 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 26 Aug 2014 20:26:47 +0200
Subject: mixer_multirotor: topic for motor limit notification

---
 src/modules/uORB/objects_common.cpp               |  3 +
 src/modules/uORB/topics/multirotor_motor_limits.h | 69 +++++++++++++++++++++++
 2 files changed, 72 insertions(+)
 create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index f7d5f8737..b921865eb 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
+#include "topics/multirotor_motor_limits.h"
+ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
+
 #include "topics/telemetry_status.h"
 ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h
new file mode 100644
index 000000000..44c51e4d9
--- /dev/null
+++ b/src/modules/uORB/topics/multirotor_motor_limits.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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
+ * 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 multirotor_motor_limits.h
+ *
+ * Definition of multirotor_motor_limits  topic
+ */
+
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+        uint8_t roll_pitch	: 1; // roll/pitch limit reached
+        uint8_t yaw		: 1; // yaw limit reached
+        uint8_t throttle_lower	: 1; // lower throttle limit reached
+        uint8_t throttle_upper	: 1; // upper throttle limit reached
+        uint8_t reserved	: 4;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
+
+#endif
-- 
cgit v1.2.3


From 6ece4cf7c44869d2fd6e027ad6239baf3af860df Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 26 Aug 2014 21:08:16 +0200
Subject: ORB topic for drive testing requests

---
 src/modules/uORB/objects_common.cpp  |  3 ++
 src/modules/uORB/topics/test_motor.h | 70 ++++++++++++++++++++++++++++++++++++
 2 files changed, 73 insertions(+)
 create mode 100644 src/modules/uORB/topics/test_motor.h

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index f7d5f8737..b8cdb45eb 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -198,6 +198,9 @@ ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
 ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
 
+#include "topics/test_motor.h"
+ORB_DEFINE(test_motor, struct test_motor_s);
+
 #include "topics/debug_key_value.h"
 ORB_DEFINE(debug_key_value, struct debug_key_value_s);
 
diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
new file mode 100644
index 000000000..491096934
--- /dev/null
+++ b/src/modules/uORB/topics/test_motor.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 test_motor.h
+ *
+ * Test a single drive motor while the vehicle is disarmed
+ *
+ * Publishing values to this topic causes a single motor to spin, e.g. for
+ * ground test purposes. This topic will be ignored while the vehicle is armed.
+ *
+ */
+
+#ifndef TOPIC_TEST_MOTOR_H
+#define TOPIC_TEST_MOTOR_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_MOTOR_OUTPUTS	8
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct test_motor_s {
+	uint64_t 	timestamp;				/**< output timestamp in us since system boot */
+	unsigned 	motor_number;				/**< number of motor to spin */
+	float		value;					/**< output data, in natural output units */
+};
+
+/**
+ * @}
+ */
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(test_motor);
+
+#endif
-- 
cgit v1.2.3


From 3d528a2c979e7d0df1171afc1f038759c7b01383 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 26 Aug 2014 22:22:59 +0200
Subject: introduce new nav state to allow normal rtl with RC switch

---
 src/modules/commander/state_machine_helper.cpp | 2 +-
 src/modules/navigator/navigator_main.cpp       | 5 ++++-
 src/modules/uORB/topics/vehicle_status.h       | 1 +
 3 files changed, 6 insertions(+), 2 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 098ff1a3d..e3b5d30e4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -462,7 +462,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
 			} else if (status->condition_local_position_valid) {
 				status->nav_state = NAVIGATION_STATE_LAND;
 			} else if (status->condition_local_altitude_valid) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c173ecd50..9a8c54e7e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -433,13 +433,16 @@ Navigator::task_main()
 			case NAVIGATION_STATE_AUTO_LOITER:
 				_navigation_mode = &_loiter;
 				break;
-			case NAVIGATION_STATE_AUTO_RTL:
+			case NAVIGATION_STATE_AUTO_RCRECOVER:
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
 				} else {
 					_navigation_mode = &_rtl;
 				}
 				break;
+			case NAVIGATION_STATE_AUTO_RTL:
+					_navigation_mode = &_rtl;
+				break;
 			case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index ad92f5b26..9dccb2309 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -102,6 +102,7 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_MISSION,		/**< Auto mission mode */
 	NAVIGATION_STATE_AUTO_LOITER,		/**< Auto loiter mode */
 	NAVIGATION_STATE_AUTO_RTL,		/**< Auto return to launch mode */
+	NAVIGATION_STATE_AUTO_RCRECOVER,	/**< RC recover mode */
 	NAVIGATION_STATE_AUTO_RTGS,		/**< Auto return to groundstation on data link loss */
 	NAVIGATION_STATE_AUTO_LANDENGFAIL,	/**< Auto land on engine failure */
 	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
-- 
cgit v1.2.3


From 9f94e4ac8409dc15484ee3bcfb89958890a305e6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Sep 2014 10:33:59 +0200
Subject: add terrain alt field to global pos topic

---
 src/modules/uORB/topics/vehicle_global_position.h | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 25137c1c6..6cbe25fbd 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -72,6 +72,7 @@ struct vehicle_global_position_s {
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 	float eph;			/**< Standard deviation of position estimate horizontally */
 	float epv;			/**< Standard deviation of position vertically */
+	float terrain_alt;		/**< Terrain altitude in m, WGS84 */
 };
 
 /**
-- 
cgit v1.2.3


From 0f548ab88cb36be362d14f9e9e76c53e3764e3c9 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Mon, 1 Sep 2014 10:38:40 +0200
Subject: add global pos valid flag

---
 src/modules/uORB/topics/vehicle_global_position.h | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 6cbe25fbd..c3bb3b893 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -73,6 +73,7 @@ struct vehicle_global_position_s {
 	float eph;			/**< Standard deviation of position estimate horizontally */
 	float epv;			/**< Standard deviation of position vertically */
 	float terrain_alt;		/**< Terrain altitude in m, WGS84 */
+	bool terrain_alt_valid;		/**< Terrain altitude estimate is valid */
 };
 
 /**
-- 
cgit v1.2.3


From 3d01da35d02505e751536e2cc09637797b37bed6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Wed, 3 Sep 2014 10:34:27 +0200
Subject: write sysid & compid to telemetry status

---
 src/modules/mavlink/mavlink_receiver.cpp   | 2 ++
 src/modules/uORB/topics/telemetry_status.h | 2 ++
 2 files changed, 4 insertions(+)

(limited to 'src/modules/uORB')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 1115304d4..43222880e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -534,6 +534,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		tstatus.remote_noise = rstatus.remnoise;
 		tstatus.rxerrors = rstatus.rxerrors;
 		tstatus.fixed = rstatus.fixed;
+		tstatus.system_id = msg->sysid;
+		tstatus.component_id = msg->compid;
 
 		if (_telemetry_status_pub < 0) {
 			_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 1297c1a9d..93193f32b 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -67,6 +67,8 @@ struct telemetry_status_s {
 	uint8_t noise;				/**< background noise level                     */
 	uint8_t remote_noise;			/**< remote background noise level              */
 	uint8_t txbuf;				/**< how full the tx buffer is as a percentage  */
+	uint8_t system_id;			/**< system id of the remote system */
+	uint8_t component_id;			/**< component id of the remote system */
 };
 
 /**
-- 
cgit v1.2.3


From d11f05b12c0b0c56703ab14303f91efd123b9dc6 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 30 Sep 2014 13:40:03 +0200
Subject: commander: update gps and engine cb only when changed

---
 src/modules/commander/commander.cpp      | 20 ++++++++++++++------
 src/modules/uORB/topics/vehicle_status.h |  2 ++
 2 files changed, 16 insertions(+), 6 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index b86f3678b..9ebe006f0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -775,6 +775,8 @@ int commander_thread_main(int argc, char *argv[])
 	// CIRCUIT BREAKERS
 	status.circuit_breaker_engaged_power_check = false;
 	status.circuit_breaker_engaged_airspd_check = false;
+	status.circuit_breaker_engaged_enginefailure_check = false;
+	status.circuit_breaker_engaged_gpsfailure_check = false;
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -980,8 +982,8 @@ int commander_thread_main(int argc, char *argv[])
 	int32_t ef_throttle_thres = 1.0f;
 	int32_t ef_current2throttle_thres = 0.0f;
 	int32_t ef_time_thres = 1000.0f;
-	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine
-											 was healty*/
+	uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
+
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
@@ -1028,8 +1030,14 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 
-				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
-				status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+				status.circuit_breaker_engaged_power_check =
+					circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+				status.circuit_breaker_engaged_airspd_check =
+					circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
+				status.circuit_breaker_engaged_enginefailure_check =
+					circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
+				status.circuit_breaker_engaged_gpsfailure_check =
+					circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
 
 				status_changed = true;
 
@@ -1417,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* check if GPS fix is ok */
-		if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) ||
+		if (status.circuit_breaker_engaged_gpsfailure_check ||
 				(gps_position.fix_type >= 3 &&
 				hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
 			/* handle the case where gps was regained */
@@ -1616,7 +1624,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* Check engine failure
 		 * only for fixed wing for now
 		 */
-		if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) &&
+		if (!status.circuit_breaker_engaged_enginefailure_check &&
 				status.is_rotary_wing == false &&
 				armed.armed &&
 				((actuator_controls.control[3] > ef_throttle_thres &&
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 9dccb2309..505039d90 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -239,6 +239,8 @@ struct vehicle_status_s {
 
 	bool circuit_breaker_engaged_power_check;
 	bool circuit_breaker_engaged_airspd_check;
+	bool circuit_breaker_engaged_enginefailure_check;
+	bool circuit_breaker_engaged_gpsfailure_check;
 };
 
 /**
-- 
cgit v1.2.3


From 213f4aadbdd089c3f793ba7ac13e0bc3b9b46fee Mon Sep 17 00:00:00 2001
From: Denis Yeldandi <dyeldandi@gramant.ru>
Date: Tue, 30 Sep 2014 15:44:47 +0400
Subject: Ashtech GPS driver

---
 src/drivers/drv_gps.h                          |   3 +-
 src/drivers/gps/ashtech.cpp                    | 616 +++++++++++++++++++++++++
 src/drivers/gps/ashtech.h                      |  96 ++++
 src/drivers/gps/gps.cpp                        |  18 +
 src/drivers/gps/module.mk                      |   1 +
 src/modules/uORB/topics/vehicle_gps_position.h |   2 +-
 6 files changed, 734 insertions(+), 2 deletions(-)
 create mode 100644 src/drivers/gps/ashtech.cpp
 create mode 100644 src/drivers/gps/ashtech.h

(limited to 'src/modules/uORB')

diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index e14f4e00d..76a211000 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -57,7 +57,8 @@
 typedef enum {
 	GPS_DRIVER_MODE_NONE = 0,
 	GPS_DRIVER_MODE_UBX,
-	GPS_DRIVER_MODE_MTK
+	GPS_DRIVER_MODE_MTK,
+	GPS_DRIVER_MODE_ASHTECH
 } gps_driver_mode_t;
 
 
diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp
new file mode 100644
index 000000000..d338ff8e5
--- /dev/null
+++ b/src/drivers/gps/ashtech.cpp
@@ -0,0 +1,616 @@
+#include "ashtech.h"
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
+#include <drivers/drv_hrt.h>
+
+#include <fcntl.h>
+#include <math.h>
+
+typedef double float64_t;
+typedef float  float32_t;
+
+char*  str_scanDec(const char *pos, int8_t sign, int8_t n_max_digit, int32_t *result)
+{
+  int8_t n=0;
+  int32_t d=0;
+  int8_t neg = 0;
+
+  if(*pos == '-') { neg = 1; pos++; }
+  else if(*pos == '+')     { pos++; }
+  else if(sign) return(NULL);
+  while(*pos >= '0' && *pos <= '9')
+  {
+     d = d*10 + (*(pos++) - '0');
+     n++;
+     if(n_max_digit>0 && n==n_max_digit) break;
+  }
+  if(n==0 || n>10) return(NULL);
+  if(neg) *result = -d;
+  else    *result =  d;
+  return((char *)pos);
+}
+
+char*  scanFloat64(const char *pos, int8_t sign, int8_t n_max_int, int8_t n_max_frac,
+                       float64_t *result)
+{
+  float64_t f=0.0, div=1.0;
+  int32_t d_int;
+  int8_t n=0, isneg= 0;
+
+  if( *pos == '-' ) isneg= 1;
+  if((pos = str_scanDec(pos, sign, n_max_int, &d_int)) == NULL) return(NULL);
+  if(*(pos) == '.')
+  {
+     pos++;
+     while(*pos >= '0' && *pos <= '9')
+     {
+        f = f*(10.0) + (float64_t)(*(pos++) - '0');
+        div *= (0.1);
+        n++;
+        if(n_max_frac>0 && n==n_max_frac) break;
+     }
+  }
+  else if(n_max_frac > 0) return(NULL);
+  if( isneg ) *result = (float64_t)d_int - f*div;
+  else        *result = (float64_t)d_int + f*div;
+  return((char *)pos);
+}
+
+
+
+ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
+_fd(fd),
+_satellite_info(satellite_info),
+_gps_position(gps_position)
+{
+    decode_init();  
+    _decode_state = NME_DECODE_UNINIT;
+    _rx_buffer_bytes = 0;
+}
+
+ASHTECH::~ASHTECH(){
+}
+//All NMEA descriptions are taken from
+//http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
+int ASHTECH::handle_message(int len){
+  if(len < 7) return 0;
+  int uiCalcComma = 0;
+  for(int i = 0 ; i < len; i++){
+    if(_rx_buffer[i] == ',')uiCalcComma++;
+  }
+  char* bufptr = (char*)( _rx_buffer+6);
+
+  if((memcmp(_rx_buffer+3, "ZDA,",3) == 0)&&(uiCalcComma == 6)){
+      /*
+      UTC day, month, and year, and local time zone offset
+      An example of the ZDA message string is:
+
+      $GPZDA,172809.456,12,07,1996,00,00*45
+
+      ZDA message fields
+      Field	Meaning
+      0	Message ID $GPZDA
+      1	UTC
+      2	Day, ranging between 01 and 31
+      3	Month, ranging between 01 and 12
+      4	Year
+      5	Local time zone offset from GMT, ranging from 00 through �13 hours
+      6	Local time zone offset from GMT, ranging from 00 through 59 minutes
+      7	The checksum data, always begins with *
+      Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
+      */
+      float64_t ashtech_time = 0.0;
+      int day = 0, month = 0,year = 0,local_time_off_hour = 0,local_time_off_min = 0;
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &day);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &month);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &year);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_hour);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &local_time_off_min);
+
+
+      int ashtech_hour = ashtech_time/10000;
+      int ashtech_minute = (ashtech_time - ashtech_hour*10000)/100;
+      float64_t ashtech_sec = ashtech_time - ashtech_hour*10000 - ashtech_minute*100;
+      //convert to unix timestamp
+      struct tm timeinfo;
+      timeinfo.tm_year = year - 1900;
+      timeinfo.tm_mon = month - 1;
+      timeinfo.tm_mday = day;
+      timeinfo.tm_hour = ashtech_hour;
+      timeinfo.tm_min = ashtech_minute;
+      timeinfo.tm_sec = int(ashtech_sec);
+      time_t epoch = mktime(&timeinfo);
+
+      _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+      _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
+      _gps_position->timestamp_time = hrt_absolute_time();
+    }
+
+  else if((memcmp(_rx_buffer+3, "GGA,",3) == 0)&&(uiCalcComma == 14)){
+  /* 
+    Time, position, and fix related data
+    An example of the GBS message string is:
+
+    $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
+
+    Note - The data string exceeds the ASHTECH standard length.
+    GGA message fields
+    Field   Meaning
+    0   Message ID $GPGGA
+    1   UTC of position fix
+    2   Latitude
+    3   Direction of latitude: 
+    N: North 
+    S: South
+    4   Longitude
+    5   Direction of longitude: 
+    E: East 
+    W: West
+    6   GPS Quality indicator: 
+    0: Fix not valid 
+    1: GPS fix 
+    2: Differential GPS fix, OmniSTAR VBS 
+    4: Real-Time Kinematic, fixed integers 
+    5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
+    7   Number of SVs in use, range from 00 through to 24+
+    8   HDOP
+    9   Orthometric height (MSL reference)
+    10  M: unit of measure for orthometric height is meters
+    11  Geoid separation
+    12  M: geoid separation measured in meters
+    13  Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used.
+    14  Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1.
+    15  
+    The checksum data, always begins with *
+    Note - If a user-defined geoid model, or an inclined
+  */
+      float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+      int num_of_sv = 0, fix_quality=0;
+      float64_t hdop = 99.9;
+      char ns = '?', ew = '?';
+
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat);
+      if(bufptr && *(++bufptr) != ',') ns = *(bufptr++);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon);
+      if(bufptr && *(++bufptr) != ',') ew = *(bufptr++);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt);
+
+      if(ns == 'S')
+        lat = -lat;
+      if(ew == 'W')
+        lon = -lon;
+
+      _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000;
+      _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000;
+      _gps_position->alt = alt*1000;
+      _rate_count_lat_lon++;
+
+      if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){
+        _gps_position->fix_type = 0;
+      }
+      else
+        _gps_position->fix_type = 3 + fix_quality;
+
+	  _gps_position->timestamp_position = hrt_absolute_time();
+
+      _gps_position->vel_m_s = 0;                                  /**< GPS ground speed (m/s) */
+      _gps_position->vel_n_m_s = 0;                                /**< GPS ground speed in m/s */
+      _gps_position->vel_e_m_s = 0;                                /**< GPS ground speed in m/s */
+      _gps_position->vel_d_m_s = 0;                                /**< GPS ground speed in m/s */
+      _gps_position->cog_rad = 0;                                  /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+      _gps_position->vel_ned_valid = true;                         /**< Flag to indicate if NED speed is valid */
+      _gps_position->c_variance_rad = 0.1;
+      _gps_position->timestamp_velocity = hrt_absolute_time();
+      return 1;
+  }
+  else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
+  /*
+	Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
+
+      $PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc 
+      Parameter Description Range 
+        d1 Position mode 0: standalone 
+                         1: differential 
+                         2: RTK float 
+                         3: RTK fixed 
+                         5: Dead reckoning 
+                         9: SBAS (see NPT setting) 
+        d2 Number of satellite used in position fix 0-99
+        m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99 
+        m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes 
+        c5 Latitude sector N, S
+        m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes 
+        c7 Longitude sector E,W
+        f8 Altitude above ellipsoid +9999.000
+        f9 Differential age (data link age), seconds 0.0-600.0
+        f10 True track/course over ground in degrees 0.0-359.9
+        f11 Speed over ground in knots 0.0-999.9
+        f12 Vertical velocity in decimeters per second +999.9 
+        f13 PDOP 0-99.9 
+        f14 HDOP 0-99.9 
+        f15 VDOP 0-99.9 
+        f16 TDOP 0-99.9 
+        s17 Reserved no data 
+        *cc Checksum 
+      */
+      bufptr = (char*)( _rx_buffer+10);
+      float64_t ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
+      int num_of_sv = 0, fix_quality=0;
+      float64_t track_true = 0.0, ground_speed = 0.0 , age_of_corr = 0.0;
+      float64_t hdop = 99.9, vdop = 99.9,  pdop = 99.9, tdop = 99.9,vertic_vel = 0.0;
+      char ns = '?', ew = '?';
+
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &fix_quality);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &num_of_sv);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat);
+      if(bufptr && *(++bufptr) != ',') ns = *(bufptr++);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon);
+      if(bufptr && *(++bufptr) != ',') ew = *(bufptr++);      
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&age_of_corr);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vertic_vel);      
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&pdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&hdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&vdop);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&tdop);
+
+      if(ns == 'S')
+        lat = -lat;
+      if(ew == 'W')
+        lon = -lon;
+
+      _gps_position->lat = (int(lat*0.01) + (lat*0.01 - int(lat*0.01))*100.0/60.0)*10000000;
+      _gps_position->lon = (int(lon*0.01) + (lon*0.01 - int(lon*0.01))*100.0/60.0)*10000000;
+      _gps_position->alt = alt*1000;
+      _rate_count_lat_lon++;
+
+      if((lat == 0.0) && (lon == 0.0) && (alt == 0.0)){
+    	  _gps_position->fix_type = 0;
+      }
+      else
+         _gps_position->fix_type = 3 + fix_quality;
+
+      _gps_position->timestamp_position = hrt_absolute_time();
+   
+      const float64_t m_pi  = 3.14159265;
+
+      double track_rad = track_true * m_pi / 180.0;
+
+	  	double velocity_ms = ground_speed  / 3.6;
+		double velocity_north = velocity_ms * cos(track_rad);
+		double velocity_east  = velocity_ms * sin(track_rad);
+
+		_gps_position->vel_m_s = velocity_ms;                            /**< GPS ground speed (m/s) */
+		_gps_position->vel_n_m_s = velocity_north;                       /**< GPS ground speed in m/s */
+		_gps_position->vel_e_m_s = velocity_east;                        /**< GPS ground speed in m/s */
+		_gps_position->vel_d_m_s = -vertic_vel;                      /**< GPS ground speed in m/s */
+		_gps_position->cog_rad = track_rad;                              /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+		_gps_position->vel_ned_valid = true;                             /**< Flag to indicate if NED speed is valid */
+		_gps_position->c_variance_rad = 0.1;
+		_gps_position->timestamp_velocity = hrt_absolute_time();
+      return 1;
+
+  }
+  else if((memcmp(_rx_buffer+3, "GST,",3) == 0)&&(uiCalcComma == 8)){
+  /* 
+    Position error statistics
+    An example of the GST message string is:
+
+    $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A
+
+    The Talker ID ($--) will vary depending on the satellite system used for the position solution:
+
+    $GP - GPS only
+    $GL - GLONASS only
+    $GN - Combined
+    GST message fields
+    Field   Meaning
+    0   Message ID $GPGST
+    1   UTC of position fix
+    2   RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing
+    3   Error ellipse semi-major axis 1 sigma error, in meters
+    4   Error ellipse semi-minor axis 1 sigma error, in meters
+    5   Error ellipse orientation, degrees from true north
+    6   Latitude 1 sigma error, in meters
+    7   Longitude 1 sigma error, in meters
+    8   Height 1 sigma error, in meters
+    9   The checksum data, always begins with *
+  */
+      float64_t ashtech_time = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
+      float64_t min_err = 0.0, maj_err = 0.0, deg_from_north = 0.0, rms_err = 0.0;
+
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ashtech_time);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&rms_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&maj_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&min_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&deg_from_north);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lat_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&lon_err);
+      if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&alt_err);
+
+      _gps_position->eph = sqrt(lat_err*lat_err +lon_err*lon_err);
+      _gps_position->epv = alt_err;
+
+      _gps_position->s_variance_m_s = 0;
+      _gps_position->timestamp_variance = hrt_absolute_time();
+  }
+  else if((memcmp(_rx_buffer+3, "VTG,",3) == 0)&&(uiCalcComma == 9)){
+  /*
+    Track made good and speed over ground
+    An example of the VTG message string is:
+
+    $GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48
+
+    VTG message fields
+    Field   Meaning
+    0   Message ID $GPVTG
+    1   Track made good (degrees true)
+    2   T: track made good is relative to true north
+    3   Track made good (degrees magnetic)
+    4   M: track made good is relative to magnetic north
+    5   Speed, in knots
+    6   N: speed is measured in knots
+    7   Speed over ground in kilometers/hour (kph)
+    8   K: speed over ground is measured in kph
+    9   The checksum data, always begins with *
+  */
+     /*float64_t track_true = 0.0, speed = 0.0, track_magnetic = 0.0, ground_speed = 0.0;
+     char true_north = '?', magnetic_north = '?', speed_in_knots = '?', speed_in_kph  = '?';
+     
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_true);
+     if(bufptr && *(++bufptr) != ',') true_north = *(bufptr++);
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&track_magnetic);
+     if(bufptr && *(++bufptr) != ',') magnetic_north = *(bufptr++);
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&speed);
+     if(bufptr && *(++bufptr) != ',') speed_in_knots = *(bufptr++);
+     if(bufptr && *(++bufptr) != ',') bufptr = scanFloat64(bufptr, 0, 9, 9,&ground_speed);
+     if(bufptr && *(++bufptr) != ',') speed_in_kph = *(bufptr++);
+
+     const float64_t dPI  = 3.14159265;
+		   float64_t tan_C = tan(track_true * dPI / 180.0);
+		   float64_t lat_ = sqrt(ground_speed*ground_speed/ (1+tan_C)); //km/hour
+		   float64_t lon_ = lat_*tan_C; //		 	                        //km/hour*/
+  }
+  else if((memcmp(_rx_buffer+3, "GSV,",3) == 0)){
+  /*
+    The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
+
+    $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
+
+    GSV message fields
+    Field   Meaning
+    0   Message ID $GPGSV
+    1   Total number of messages of this type in this cycle
+    2   Message number
+    3   Total number of SVs visible
+    4   SV PRN number
+    5   Elevation, in degrees, 90� maximum
+    6   Azimuth, degrees from True North, 000� through 359�
+    7   SNR, 00 through 99 dB (null when not tracking)
+    8-11    Information about second SV, same format as fields 4 through 7
+    12-15   Information about third SV, same format as fields 4 through 7
+    16-19   Information about fourth SV, same format as fields 4 through 7
+    20  The checksum data, always begins with *
+  */
+// currently process only gps, because do not know what 
+// Global satellite ID I should use for non GPS sats
+    bool bGPS = false;
+    if(memcmp(_rx_buffer, "$GP",3) != 0) 
+      return 0;
+    else
+      bGPS = true;
+
+    int all_msg_num, this_msg_num, tot_sv_visible;
+    struct gsv_sat{
+      int svid;
+      int elevation;
+      int azimuth;
+      int snr;
+    } sat[4];
+    memset(sat, 0, sizeof(sat));
+    
+    if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &all_msg_num);
+    if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &this_msg_num);
+    if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &tot_sv_visible);
+    if((this_msg_num<1) || (this_msg_num>all_msg_num)){
+      return 0;
+    }
+    if((this_msg_num == 0)&&(bGPS == true)){
+      memset(_satellite_info->svid,     0,sizeof(_satellite_info->svid));
+      memset(_satellite_info->used,     0,sizeof(_satellite_info->used));
+      memset(_satellite_info->snr,      0,sizeof(_satellite_info->snr));
+      memset(_satellite_info->elevation,0,sizeof(_satellite_info->elevation));
+      memset(_satellite_info->azimuth,  0,sizeof(_satellite_info->azimuth));
+    }
+
+    int end = 4;
+    if(this_msg_num == all_msg_num){
+    	end =  tot_sv_visible - (this_msg_num-1)*4;
+    	_gps_position->satellites_used = tot_sv_visible;
+    	_satellite_info->count = SAT_INFO_MAX_SATELLITES;
+    	_satellite_info->timestamp = hrt_absolute_time();
+    }
+    for(int y = 0 ; y < end ;y++){
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].svid);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].elevation);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].azimuth);
+      if(bufptr && *(++bufptr) != ',') bufptr = str_scanDec(bufptr, 0, 9, &sat[y].snr);
+    
+      _satellite_info->svid[y+(this_msg_num-1)*4]      = sat[y].svid;
+      _satellite_info->used[y+(this_msg_num-1)*4]      = ((sat[y].snr>0)? true: false);
+      _satellite_info->snr[y+(this_msg_num-1)*4]       = sat[y].snr;
+      _satellite_info->elevation[y+(this_msg_num-1)*4] = sat[y].elevation;
+      _satellite_info->azimuth[y+(this_msg_num-1)*4]   = sat[y].azimuth;
+    }
+  }
+
+  return 0;
+}
+
+
+int ASHTECH::receive(unsigned timeout){
+{
+    /* poll descriptor */
+    pollfd fds[1];
+    fds[0].fd = _fd;
+    fds[0].events = POLLIN;
+
+    uint8_t buf[32];
+
+    /* timeout additional to poll */
+    uint64_t time_started = hrt_absolute_time();
+
+    int j = 0;
+    ssize_t count = 0;
+
+    while (true) {
+
+        /* pass received bytes to the packet decoder */
+        while (j < count) {
+            int l=0;
+            if ((l = parse_char(buf[j])) > 0) {
+                /* return to configure during configuration or to the gps driver during normal work
+                 * if a packet has arrived */
+                 if (handle_message(l) > 0)
+                    return 1;
+            }
+            /* in case we keep trying but only get crap from GPS */
+            if (time_started + timeout*1000*2 < hrt_absolute_time() ) {
+                return -1;
+            }
+            j++;
+        }
+
+        /* everything is read */
+        j = count = 0;
+
+        /* then poll for new data */
+        int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout*2);
+
+        if (ret < 0) {
+            /* something went wrong when polling */
+            return -1;
+
+        } else if (ret == 0) {
+            /* Timeout */
+            return -1;
+
+        } else if (ret > 0) {
+            /* if we have new data from GPS, go handle it */
+            if (fds[0].revents & POLLIN) {
+                /*
+                 * We are here because poll says there is some data, so this
+                 * won't block even on a blocking device.  If more bytes are
+                 * available, we'll go back to poll() again...
+                 */
+                count = ::read(_fd, buf, sizeof(buf));
+            }
+        }
+    }
+}
+
+}
+#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
+
+int ASHTECH::parse_char(uint8_t b)
+{
+	int iRet = 0;
+
+	switch (_decode_state) {
+		/* First, look for sync1 */
+	case NME_DECODE_UNINIT:
+		if (b == '$') {
+			_decode_state = NME_DECODE_GOT_SYNC1;
+			_rx_buffer_bytes = 0;
+			_rx_buffer[_rx_buffer_bytes++] = b;
+		}
+
+		break;
+
+	case NME_DECODE_GOT_SYNC1:
+		if (b == '$') {
+			_decode_state = NME_DECODE_GOT_SYNC1;
+			_rx_buffer_bytes = 0;
+
+		} else if (b == '*') {
+			_decode_state = NME_DECODE_GOT_ASTERIKS;
+		}
+
+		if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
+			_decode_state = NME_DECODE_UNINIT;
+			_rx_buffer_bytes = 0;
+
+		} else
+			_rx_buffer[_rx_buffer_bytes++] = b;
+
+		break;
+
+	case NME_DECODE_GOT_ASTERIKS:
+		_rx_buffer[_rx_buffer_bytes++] = b;
+		_decode_state = NME_DECODE_GOT_FIRST_CS_BYTE;
+		break;
+
+	case NME_DECODE_GOT_FIRST_CS_BYTE:
+		_rx_buffer[_rx_buffer_bytes++] = b;
+		uint8_t checksum = 0;
+		uint8_t *buffer = _rx_buffer + 1;
+		uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
+
+		for (; buffer < bufend; buffer++) checksum ^= *buffer;
+
+		if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
+		    (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
+			iRet = _rx_buffer_bytes;
+		}
+
+		_decode_state = NME_DECODE_UNINIT;
+		_rx_buffer_bytes = 0;
+		break;
+	}
+
+	return iRet;
+}
+
+void ASHTECH::decode_init(void){
+
+}
+
+//ashtech boad configuration script 
+//char comm[] = "$PASHS,NME,GGA,A,ON,0.1\r\n"
+char comm[] = "$PASHS,NME,ZDA,B,ON,3\r\n"\
+              "$PASHS,NME,GGA,B,OFF\r\n"\
+              "$PASHS,NME,GST,B,ON,3\r\n"\
+              "$PASHS,NME,POS,B,ON,0.1\r\n"\
+              "$PASHS,NME,GSV,B,ON,3\r\n"\
+              "$PASHS,SPD,A,8\r\n"\
+              "$PASHS,SPD,B,7\r\n"; // default baud is 7
+
+int ASHTECH::configure(unsigned &baudrate){
+    /* try different baudrates */
+    const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+    for (int baud_i = 0; baud_i < sizeof(baudrates_to_try)/sizeof(baudrates_to_try[0]); baud_i++) {
+      baudrate = baudrates_to_try[baud_i];
+      set_baudrate(_fd, baudrate);
+      write(_fd, (uint8_t*)comm, sizeof(comm));
+    }
+    set_baudrate(_fd, 38400);
+    return 0;
+}
diff --git a/src/drivers/gps/ashtech.h b/src/drivers/gps/ashtech.h
new file mode 100644
index 000000000..e8c5ffb4c
--- /dev/null
+++ b/src/drivers/gps/ashtech.h
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013. All rights reserved.
+ *   Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
+ *           Kistanov Alexander <akistanov@gramant.ru>
+ *
+ * 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 ASHTECH protocol definitions */
+
+#ifndef ASHTECH_H_
+#define ASHTECH_H_
+
+#include "gps_helper.h"
+
+#ifndef RECV_BUFFER_SIZE
+#define RECV_BUFFER_SIZE 512
+
+#define SAT_INFO_MAX_SATELLITES  20
+#endif
+
+
+class ASHTECH : public GPS_Helper
+{
+	enum ashtech_decode_state_t {
+		NME_DECODE_UNINIT,
+		NME_DECODE_GOT_SYNC1,
+		NME_DECODE_GOT_ASTERIKS,
+		NME_DECODE_GOT_FIRST_CS_BYTE
+	};
+
+	int                    _fd;
+	struct satellite_info_s *_satellite_info;
+	struct vehicle_gps_position_s *_gps_position;
+	int ashtechlog_fd;//miklm
+
+	ashtech_decode_state_t   _decode_state;
+	uint8_t               _rx_buffer[RECV_BUFFER_SIZE];
+	uint16_t              _rx_buffer_bytes;
+	bool                  _parse_error; // parse error flag
+	char                 *_parse_pos; // parse position
+
+    bool	_gsv_in_progress;					// Indicates that gsv data parsing is in progress
+	//int     _satellites_count; 				// Number of satellites info parsed.
+	uint8_t count;					/**< Number of satellites in satellite info */
+	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
+	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
+	uint8_t elevation[SAT_INFO_MAX_SATELLITES];	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t azimuth[SAT_INFO_MAX_SATELLITES];	/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t snr[SAT_INFO_MAX_SATELLITES];		/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
+
+public:
+	ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
+	~ASHTECH();
+	int             receive(unsigned timeout);
+	int             configure(unsigned &baudrate);
+	void            decode_init(void);
+	int             handle_message(int len);
+	int             parse_char(uint8_t b);
+	/** Read int ASHTECH parameter */
+	int32_t         read_int();
+	/** Read float ASHTECH parameter */
+	double       read_float();
+	/** Read char ASHTECH parameter */
+	char            read_char();
+
+};
+
+#endif /* ASHTECH_H_ */
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 34dd63086..2547cda97 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -69,6 +69,7 @@
 
 #include "ubx.h"
 #include "mtk.h"
+#include "ashtech.h"
 
 
 #define TIMEOUT_5HZ 500
@@ -341,6 +342,11 @@ GPS::task_main()
 				_Helper = new MTK(_serial_fd, &_report_gps_pos);
 				break;
 
+			case GPS_DRIVER_MODE_ASHTECH:
+
+				_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
+				break;
+
 			default:
 				break;
 			}
@@ -402,6 +408,10 @@ GPS::task_main()
 							mode_str = "MTK";
 							break;
 
+					case GPS_DRIVER_MODE_ASHTECH:
+						mode_str = "ASHTECH";
+						break;
+
 						default:
 							break;
 						}
@@ -429,6 +439,10 @@ GPS::task_main()
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
+			_mode = GPS_DRIVER_MODE_ASHTECH;
+			break;
+
+		case GPS_DRIVER_MODE_ASHTECH:
 				_mode = GPS_DRIVER_MODE_UBX;
 				break;
 
@@ -475,6 +489,10 @@ GPS::print_info()
 		warnx("protocol: MTK");
 		break;
 
+	case GPS_DRIVER_MODE_ASHTECH:
+		warnx("protocol: ASHTECH");
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index b00818424..4f99b0d3b 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -40,6 +40,7 @@ MODULE_COMMAND	= gps
 SRCS		= gps.cpp \
 		  gps_helper.cpp \
 		  mtk.cpp \
+		  ashtech.cpp \
 		  ubx.cpp
 
 MODULE_STACKSIZE = 1200
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 80d65cd69..43909e2fa 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -62,7 +62,7 @@ struct vehicle_gps_position_s {
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
 	float c_variance_rad;				/**< course accuracy estimate rad */
-	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
+	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
 	float eph;					/**< GPS HDOP horizontal dilution of position in m */
 	float epv;					/**< GPS VDOP horizontal dilution of position in m */
-- 
cgit v1.2.3


From 1bf1cec5672a7f30656e3fe44535ab0390bbd2d0 Mon Sep 17 00:00:00 2001
From: Denis Yeldandi <dyeldandi@gramant.ru>
Date: Wed, 8 Oct 2014 23:37:41 +0400
Subject: Corrected gps_fix values description

---
 src/modules/uORB/topics/vehicle_gps_position.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 43909e2fa..31e616f4f 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -62,7 +62,7 @@ struct vehicle_gps_position_s {
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
 	float c_variance_rad;				/**< course accuracy estimate rad */
-	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: Real-Time Kinematic, fixed integers, 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
+	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
 	float eph;					/**< GPS HDOP horizontal dilution of position in m */
 	float epv;					/**< GPS VDOP horizontal dilution of position in m */
-- 
cgit v1.2.3


From 1bf4270e3ee6f33f8adf0027c1a59f3fc0b35263 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 13 Oct 2014 17:01:34 +0400
Subject: Update ORB topic 'esc_status'

---
 src/drivers/hott/messages.cpp        | 14 +++++++-------
 src/drivers/mkblctrl/mkblctrl.cpp    |  6 +++---
 src/modules/sdlog2/sdlog2_messages.h | 14 +++++++-------
 src/modules/uORB/topics/esc_status.h | 19 ++++++++++---------
 src/modules/uavcan/actuators/esc.cpp | 14 ++++++--------
 5 files changed, 33 insertions(+), 34 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 086132573..f1b12b067 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer)
 
 	esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
 	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
-	esc.esc[0].esc_temperature = msg.temperature1 - 20; 
-	esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
-	esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
+	esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
+	esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
+	esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
 
 	/* announce the esc if needed, just publish else */
 	if (_esc_pub > 0) {
@@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size)
 	msg.gam_sensor_id = GAM_SENSOR_ID;
 	msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
 	
-	msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
+	msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
 	msg.temperature2 = 20;  // 0 deg. C.
 
-	uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
+	const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
 	msg.main_voltage_L = (uint8_t)voltage & 0xff;
 	msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
 
-	uint16_t current = (uint16_t)(esc.esc[0].esc_current);
+	const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
 	msg.current_L = (uint8_t)current & 0xff;
 	msg.current_H = (uint8_t)(current >> 8) & 0xff;
 
-	uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
+	const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
 	msg.rpm_L = (uint8_t)rpm & 0xff;
 	msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
 
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 3996b76a6..1055487cb 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -600,8 +600,8 @@ MK::task_main()
 				esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
 				esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
 				esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
-				esc.esc[i].esc_voltage = (uint16_t) 0;
-				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
+				esc.esc[i].esc_voltage = 0.0F;
+				esc.esc[i].esc_current = static_cast<float>(Motor[i].Current) * 0.1F;
 				esc.esc[i].esc_rpm = (uint16_t) 0;
 				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
 
@@ -614,7 +614,7 @@ MK::task_main()
 					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
 				}
 
-				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
+				esc.esc[i].esc_temperature = static_cast<float>(Motor[i].Temperature);
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index d2845eb75..aeff4fd30 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -241,15 +241,15 @@ struct log_GPSP_s {
 #define LOG_ESC_MSG 18
 struct log_ESC_s {
 	uint16_t counter;
-	uint8_t esc_count;
-	uint8_t esc_connectiontype;
-	uint8_t esc_num;
+	uint8_t  esc_count;
+	uint8_t  esc_connectiontype;
+	uint8_t  esc_num;
 	uint16_t esc_address;
 	uint16_t esc_version;
-	uint16_t esc_voltage;
-	uint16_t esc_current;
+	float    esc_voltage;
+	float    esc_current;
 	uint16_t esc_rpm;
-	uint16_t esc_temperature;
+	float    esc_temperature;
 	float    esc_setpoint;
 	uint16_t esc_setpoint_raw;
 };
@@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHffHffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 628824efa..b82796908 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -78,6 +78,7 @@ enum ESC_CONNECTION_TYPE {
 
 /**
  * Electronic speed controller status.
+ * Unsupported float fields should be assigned NaN.
  */
 struct esc_status_s {
 	/* use of a counter and timestamp recommended (but not necessary) */
@@ -89,17 +90,17 @@ struct esc_status_s {
 	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
 
 	struct {
-		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
 		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
-		uint16_t esc_version;			/**< Version of current ESC - if supported */
-		uint16_t esc_voltage;			/**< Voltage measured from current ESC - if supported */
-		uint16_t esc_current;			/**< Current measured from current ESC (100mA steps) - if supported */
-		uint16_t esc_rpm;				/**< RPM measured from current ESC - if supported */
-		uint16_t esc_temperature;		/**< Temperature measured from current ESC - if supported */
-		float    esc_setpoint;			/**< setpoint of current ESC */
+		uint32_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+		float esc_voltage;			/**< Voltage measured from current ESC [V] - if supported */
+		float esc_current;			/**< Current measured from current ESC [A] - if supported */
+		float esc_temperature;			/**< Temperature measured from current ESC [degC] - if supported */
+		float esc_setpoint;			/**< setpoint of current ESC */
 		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
-		uint16_t esc_state;				/**< State of ESC - depend on Vendor */
-		uint16_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
+		uint16_t esc_version;			/**< Version of current ESC - if supported */
+		uint16_t esc_rpm;			/**< RPM measured from current ESC [RPM] - if supported */
+		uint16_t esc_state;			/**< State of ESC - depend on Vendor */
 	} esc[CONNECTED_ESC_MAX];
 
 };
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 0601d9fa2..249ffba07 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -134,14 +134,12 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 
 		ref.esc_address = msg.getSrcNodeID().get();
 
-		// >0 checks allow to weed out NaNs and negative values that aren't supported.
-		ref.esc_voltage     = (msg.voltage > 0)     ? msg.voltage * 10.0F     : 0;
-		ref.esc_current     = (msg.current > 0)     ? msg.current * 10.0F     : 0;
-		ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0;
-
-		ref.esc_setpoint   = msg.power_rating_pct;
-		ref.esc_rpm        = abs(msg.rpm);
-		ref.esc_errorcount = msg.error_count;
+		ref.esc_voltage     = msg.voltage;
+		ref.esc_current     = msg.current;
+		ref.esc_temperature = msg.temperature;
+		ref.esc_setpoint    = msg.power_rating_pct;
+		ref.esc_rpm         = abs(msg.rpm);
+		ref.esc_errorcount  = msg.error_count;
 	}
 }
 
-- 
cgit v1.2.3


From f9856c6228396ca2fe8eb4c86e12c60df14d6b4c Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 15 Oct 2014 13:46:16 +0400
Subject: ESC status - supporting negative RPM

---
 src/modules/sdlog2/sdlog2_messages.h | 4 ++--
 src/modules/uORB/topics/esc_status.h | 2 +-
 src/modules/uavcan/actuators/esc.cpp | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index aeff4fd30..d49fc0c79 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -248,7 +248,7 @@ struct log_ESC_s {
 	uint16_t esc_version;
 	float    esc_voltage;
 	float    esc_current;
-	uint16_t esc_rpm;
+	int32_t  esc_rpm;
 	float    esc_temperature;
 	float    esc_setpoint;
 	uint16_t esc_setpoint_raw;
@@ -452,7 +452,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
-	LOG_FORMAT(ESC, "HBBBHHffHffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index b82796908..98cdad3d5 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -99,7 +99,7 @@ struct esc_status_s {
 		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
 		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
 		uint16_t esc_version;			/**< Version of current ESC - if supported */
-		uint16_t esc_rpm;			/**< RPM measured from current ESC [RPM] - if supported */
+		int32_t esc_rpm;			/**< Motor RPM, negative for reverse rotation [RPM] - if supported */
 		uint16_t esc_state;			/**< State of ESC - depend on Vendor */
 	} esc[CONNECTED_ESC_MAX];
 
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 08e504947..1990651ef 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -140,7 +140,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 		ref.esc_current     = msg.current;
 		ref.esc_temperature = msg.temperature;
 		ref.esc_setpoint    = msg.power_rating_pct;
-		ref.esc_rpm         = abs(msg.rpm);
+		ref.esc_rpm         = msg.rpm;
 		ref.esc_errorcount  = msg.error_count;
 	}
 }
-- 
cgit v1.2.3


From 7fd3bd9af8ee150911a08e75a1e30fa4cb2b8e2d Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 15 Oct 2014 14:01:17 +0400
Subject: esc_status layout optimization

---
 src/modules/uORB/topics/esc_status.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 98cdad3d5..b45c49788 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -92,6 +92,7 @@ struct esc_status_s {
 	struct {
 		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
 		uint32_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+                int32_t esc_rpm;                        /**< Motor RPM, negative for reverse rotation [RPM] - if supported */
 		float esc_voltage;			/**< Voltage measured from current ESC [V] - if supported */
 		float esc_current;			/**< Current measured from current ESC [A] - if supported */
 		float esc_temperature;			/**< Temperature measured from current ESC [degC] - if supported */
@@ -99,7 +100,6 @@ struct esc_status_s {
 		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
 		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
 		uint16_t esc_version;			/**< Version of current ESC - if supported */
-		int32_t esc_rpm;			/**< Motor RPM, negative for reverse rotation [RPM] - if supported */
 		uint16_t esc_state;			/**< State of ESC - depend on Vendor */
 	} esc[CONNECTED_ESC_MAX];
 
-- 
cgit v1.2.3


From acb739655d5c2ebf50449842ae2b7b9b7c76dbd1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 2 Nov 2014 17:06:35 +0100
Subject: Remove huge memory overhead in RC channels topic, was completely
 unnecessary

---
 src/modules/uORB/topics/rc_channels.h | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 8978de471..b14ae1edd 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -76,7 +76,6 @@ struct rc_channels_s {
 	uint64_t timestamp_last_valid;						/**< Timestamp of last valid RC signal */
 	float channels[RC_CHANNELS_FUNCTION_MAX];			/**< Scaled to -1..1 (throttle: 0..1) */
 	uint8_t channel_count;								/**< Number of valid channels */
-	char function_name[RC_CHANNELS_FUNCTION_MAX][20];	/**< String array to store the names of the functions */
 	int8_t function[RC_CHANNELS_FUNCTION_MAX];			/**< Functions mapping */
 	uint8_t rssi;										/**< Receive signal strength index */
 	bool signal_lost;									/**< Control signal lost, should be checked together with topic timeout */
-- 
cgit v1.2.3


From 43418a674903d242271028c4d4eb473f95a24be6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 7 Nov 2014 13:31:33 +0100
Subject: application layer only, no drivers affected: Fix overflow in RC input
 topic - this topic is deprecated and will be removed, has been superseded by
 input_rc and manual_control

---
 src/modules/uORB/topics/rc_channels.h | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

(limited to 'src/modules/uORB')

diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index b14ae1edd..16916cc4d 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -34,6 +34,8 @@
 /**
  * @file rc_channels.h
  * Definition of the rc_channels uORB topic.
+ *
+ * @deprecated DO NOT USE FOR NEW CODE
  */
 
 #ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
 	AUX_2,
 	AUX_3,
 	AUX_4,
-	AUX_5,
-	RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
+	AUX_5
 };
 
+// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+
+#define RC_CHANNELS_FUNCTION_MAX 18
+
 /**
  * @addtogroup topics
  * @{
-- 
cgit v1.2.3