Merged master into driver_framework

src/lib/ecl/CMakeLists.txt

@@ -39,6 +39,7 @@ px4_add_module(
 		attitude_fw/ecl_pitch_controller.cpp
 		attitude_fw/ecl_roll_controller.cpp
 		attitude_fw/ecl_yaw_controller.cpp
+		attitude_fw/ecl_wheel_controller.cpp
 		l1/ecl_l1_pos_controller.cpp
 		validation/data_validator.cpp
 		validation/data_validator_group.cpp

src/lib/ecl/attitude_fw/ecl_controller.h

@@ -75,6 +75,8 @@ struct ECL_ControlData {
 	float airspeed_max;
 	float airspeed;
 	float scaler;
+	float groundspeed;
+	float groundspeed_scaler;
 	bool lock_integrator;
 };
 

src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp

@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.cpp
+ * Implementation of a simple PID wheel controller for heading tracking.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_wheel_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <ecl/ecl.h>
+
+ECL_WheelController::ECL_WheelController() :
+    ECL_Controller("wheel")
+{
+}
+
+ECL_WheelController::~ECL_WheelController()
+{
+}
+
+float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+{
+    /* Do not calculate control signal with bad inputs */
+    if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
+          PX4_ISFINITE(ctl_data.groundspeed) &&
+          PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
+        perf_count(_nonfinite_input_perf);
+        return math::constrain(_last_output, -1.0f, 1.0f);
+    }
+
+    /* get the usual dt estimate */
+    uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+    _last_run = ecl_absolute_time();
+    float dt = (float)dt_micros * 1e-6f;
+
+    /* lock integral for long intervals */
+    bool lock_integrator = ctl_data.lock_integrator;
+
+    if (dt_micros > 500000) {
+        lock_integrator = true;
+    }
+
+    /* input conditioning */
+    float min_speed = 1.0f;
+
+    /* Calculate body angular rate error */
+    _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error
+
+    if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
+
+        float id = _rate_error * dt * ctl_data.groundspeed_scaler;
+
+        /*
+         * anti-windup: do not allow integrator to increase if actuator is at limit
+         */
+        if (_last_output < -1.0f) {
+            /* only allow motion to center: increase value */
+            id = math::max(id, 0.0f);
+
+        } else if (_last_output > 1.0f) {
+            /* only allow motion to center: decrease value */
+            id = math::min(id, 0.0f);
+        }
+
+        _integrator += id;
+    }
+
+    /* integrator limit */
+    //xxx: until start detection is available: integral part in control signal is limited here
+    float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+    /* Apply PI rate controller and store non-limited output */
+    _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
+            _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler +
+            integrator_constrained;
+    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f",
+            (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/
+
+
+    return math::constrain(_last_output, -1.0f, 1.0f);
+}
+
+
+float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
+{
+    /* Do not calculate control signal with bad inputs */
+    if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
+          PX4_ISFINITE(ctl_data.yaw))) {
+        perf_count(_nonfinite_input_perf);
+        warnx("not controlling wheel");
+        return _rate_setpoint;
+    }
+
+    /* Calculate the error */
+    float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
+    /* shortest angle (wrap around) */
+    yaw_error = (float)fmod((float)fmod((yaw_error + M_PI_F), M_TWOPI_F) + M_TWOPI_F, M_TWOPI_F) - M_PI_F;
+    /*warnx("yaw_error: %.4f", (double)yaw_error);*/
+
+    /*  Apply P controller: rate setpoint from current error and time constant */
+    _rate_setpoint =  yaw_error / _tc;
+
+    /* limit the rate */
+    if (_max_rate > 0.01f) {
+        if (_rate_setpoint > 0.0f) {
+            _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+
+        } else {
+            _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+        }
+
+    }
+
+    return _rate_setpoint;
+}

src/lib/ecl/attitude_fw/ecl_wheel_controller.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_wheel_controller.h
+ * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Andreas Antener <andreas@uaventure.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+#ifndef ECL_HEADING_CONTROLLER_H
+#define ECL_HEADING_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "ecl_controller.h"
+
+class __EXPORT ECL_WheelController :
+    public ECL_Controller
+{
+public:
+    ECL_WheelController();
+
+    ~ECL_WheelController();
+
+    float control_attitude(const struct ECL_ControlData &ctl_data);
+
+    float control_bodyrate(const struct ECL_ControlData &ctl_data);
+};
+
+#endif // ECL_HEADING_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_yaw_controller.h

@@ -76,14 +76,15 @@ public:
 		_coordinated_method = coordinated_method;
 	}
 
-protected:
-	float _coordinated_min_speed;
-
 	enum {
 		COORD_METHOD_OPEN = 0,
-		COORD_METHOD_CLOSEACC = 1,
+		COORD_METHOD_CLOSEACC = 1
 	};
 
+protected:
+	float _coordinated_min_speed;
+	float _max_rate;
+
 	int32_t _coordinated_method;
 
 	float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);

src/lib/ecl/validation/data_validator.cpp

@@ -43,6 +43,7 @@
 #include <ecl/ecl.h>
 
 DataValidator::DataValidator(DataValidator *prev_sibling) :
+	_error_mask(ERROR_FLAG_NO_ERROR),
 	_time_last(0),
 	_timeout_interval(20000),
 	_event_count(0),
@@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
 float
 DataValidator::confidence(uint64_t timestamp)
 {
+	float ret = 1.0f;
+	
 	/* check if we have any data */
 	if (_time_last == 0) {
-		return 0.0f;
+		_error_mask |= ERROR_FLAG_NO_DATA;
+		ret = 0.0f;
 	}
-
-	/* check error count limit */
-	if (_error_count > NORETURN_ERRCOUNT) {
-		return 0.0f;
+	
+	/* timed out - that's it */
+	if (timestamp - _time_last > _timeout_interval) {
+		_error_mask |= ERROR_FLAG_TIMEOUT;
+		ret = 0.0f;
 	}
 
 	/* we got the exact same sensor value N times in a row */
 	if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
-		return 0.0f;
+		_error_mask |= ERROR_FLAG_STALE_DATA;
+		ret = 0.0f;
 	}
-
-	/* timed out - that's it */
-	if (timestamp - _time_last > _timeout_interval) {
-		return 0.0f;
+	
+	/* check error count limit */
+	if (_error_count > NORETURN_ERRCOUNT) {
+		_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
+		ret = 0.0f;
 	}
 
 	/* cap error density counter at window size */
 	if (_error_density > ERROR_DENSITY_WINDOW) {
+		_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
 		_error_density = ERROR_DENSITY_WINDOW;
 	}
-
-	/* return local error density for last N measurements */
-	return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
-}
-
-int
-DataValidator::priority()
-{
-	return _priority;
+	
+	/* no critical errors */
+	if(ret > 0.0f) {
+		/* return local error density for last N measurements */
+		ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
+	}
+	
+	return ret;
 }
 
 void

src/lib/ecl/validation/data_validator.h

@@ -91,7 +91,18 @@ public:
 	 * Get the priority of this validator
 	 * @return		the stored priority
 	 */
-	int			priority();
+	int			priority() { return (_priority); }
+	
+	/**
+	 * Get the error state of this validator
+	 * @return		the bitmask with the error status
+	 */
+	uint32_t		state() { return (_error_mask); }
+	
+	/**
+	 * Reset the error state of this validator
+	 */
+	void			reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
 
 	/**
 	 * Get the RMS values of this validator
@@ -111,9 +122,20 @@ public:
 	 * @param timeout_interval_us The timeout interval in microseconds
 	 */
 	void			set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
+	
+	/**
+	 * Data validator error states 
+	 */
+	static constexpr uint32_t ERROR_FLAG_NO_ERROR      	= (0x00000000U);
+	static constexpr uint32_t ERROR_FLAG_NO_DATA       	= (0x00000001U);
+	static constexpr uint32_t ERROR_FLAG_STALE_DATA    	= (0x00000001U << 1);
+	static constexpr uint32_t ERROR_FLAG_TIMEOUT 	   	= (0x00000001U << 2);
+	static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT 	= (0x00000001U << 3);
+	static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY 	= (0x00000001U << 4);
 
 private:
 	static const unsigned _dimensions = 3;
+	uint32_t _error_mask;			/**< sensor error state */
 	uint64_t _time_last;			/**< last timestamp */
 	uint64_t _timeout_interval;		/**< interval in which the datastream times out in us */
 	uint64_t _event_count;			/**< total data counter */

src/lib/ecl/validation/data_validator_group.cpp

@@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
 
 		bool true_failsafe = true;
 
-		/* check wether the switch was a failsafe or preferring a higher priority sensor */
+		/* check whether the switch was a failsafe or preferring a higher priority sensor */
 		if (pre_check_prio != -1 && pre_check_prio < max_priority &&
 			fabsf(pre_check_confidence - max_confidence) < 0.1f) {
 			/* this is not a failover */
 			true_failsafe = false;
+			/* reset error flags, this is likely a hotplug sensor coming online late */
+			best->reset_state();
 		}
 
 		/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@@ -199,17 +201,25 @@ void
 DataValidatorGroup::print()
 {
 	/* print the group's state */
-	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
+	ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
 		_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
 		_toggle_count);
 
-
 	DataValidator *next = _first;
 	unsigned i = 0;
 
 	while (next != nullptr) {
 		if (next->used()) {
-			ECL_INFO("sensor #%u, prio: %d", i, next->priority());
+			uint32_t flags = next->state();
+			
+			ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
+			((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""),
+			((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""),
+			((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""),
+			((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""),
+			((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""),
+			((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
+			
 			next->print();
 		}
 		next = next->sibling();
@@ -222,3 +232,35 @@ DataValidatorGroup::failover_count()
 {
 	return _toggle_count;
 }
+
+int
+DataValidatorGroup::failover_index()
+{
+	DataValidator *next = _first;
+	unsigned i = 0;
+	
+	while (next != nullptr) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+			return i;
+		}
+		next = next->sibling();
+		i++;
+	}
+	return -1;
+}
+
+uint32_t
+DataValidatorGroup::failover_state()
+{	
+	DataValidator *next = _first;
+	unsigned i = 0;
+	
+	while (next != nullptr) {
+		if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
+			return next->state();
+		}
+		next = next->sibling();
+		i++;
+	}
+	return DataValidator::ERROR_FLAG_NO_ERROR;
+}

src/lib/ecl/validation/data_validator_group.h

@@ -80,6 +80,20 @@ public:
 	 * @return		the number of failovers
 	 */
 	unsigned		failover_count();
+	
+	/**
+	 * Get the index of the failed sensor in the group
+	 *
+	 * @return		index of the failed sensor
+	 */
+	int			failover_index();
+	
+	/**
+	 * Get the error state of the failed sensor in the group
+	 *
+	 * @return		bitmask with erro states of the failed sensor
+	 */
+	uint32_t		failover_state();
 
 	/**
 	 * Print the validator value

src/lib/geo/geo.c

@@ -298,6 +298,41 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	return CONSTANTS_RADIUS_OF_EARTH * c;
 }
 
+__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
+		double *lat_target, double *lon_target)
+{
+	if (fabsf(dist) < FLT_EPSILON) {
+		*lat_target = lat_A;
+		*lon_target = lon_A;
+
+	} else if (dist >= FLT_EPSILON) {
+		float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
+		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
+
+	} else {
+		float heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B);
+		heading = _wrap_2pi(heading + M_PI_F);
+		waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target);
+	}
+}
+
+__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
+		double *lat_target, double *lon_target)
+{
+	bearing = _wrap_2pi(bearing);
+	double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH);
+
+	double lat_start_rad = lat_start * M_DEG_TO_RAD;
+	double lon_start_rad = lon_start * M_DEG_TO_RAD;
+
+	*lat_target = asin(sin(lat_start_rad) * cos(radius_ratio) + cos(lat_start_rad) * sin(radius_ratio) * cos((
+				   double)bearing));
+	*lon_target = lon_start_rad + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start_rad),
+					    cos(radius_ratio) - sin(lat_start_rad) * sin(*lat_target));
+
+	*lat_target *= M_RAD_TO_DEG;
+	*lon_target *= M_RAD_TO_DEG;
+}
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;

src/lib/geo/geo.h

@@ -236,6 +236,36 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al
  */
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
+
+/**
+ * Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance
+ * from waypoint A
+ *
+ * @param lat_A waypoint A latitude in degrees (47.1234567°, not 471234567°)
+ * @param lon_A waypoint A longitude in degrees (8.1234567°, not 81234567°)
+ * @param lat_B waypoint B latitude in degrees (47.1234567°, not 471234567°)
+ * @param lon_B waypoint B longitude in degrees (8.1234567°, not 81234567°)
+ * @param dist distance of target waypoint from waypoint A in meters (can be negative)
+ * @param lat_target latitude of target waypoint C in degrees (47.1234567°, not 471234567°)
+ * @param lon_target longitude of target waypoint C in degrees (47.1234567°, not 471234567°)
+ */
+__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
+		double *lat_target, double *lon_target);
+
+/**
+ * Creates a waypoint from given waypoint, distance and bearing
+ * see http://www.movable-type.co.uk/scripts/latlong.html
+ *
+ * @param lat_start latitude of starting waypoint in degrees (47.1234567°, not 471234567°)
+ * @param lon_start longitude of starting waypoint in degrees (8.1234567°, not 81234567°)
+ * @param bearing in rad
+ * @param distance in meters
+ * @param lat_target latitude of target waypoint in degrees (47.1234567°, not 471234567°)
+ * @param lon_target longitude of target waypoint in degrees (47.1234567°, not 471234567°)
+ */
+__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
+		double *lat_target, double *lon_target);
+
 /**
  * Returns the bearing to the next waypoint in radians.
  *

src/lib/runway_takeoff/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__runway_takeoff
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		RunwayTakeoff.cpp
+		runway_takeoff_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/lib/runway_takeoff/RunwayTakeoff.cpp

@@ -0,0 +1,286 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 RunwayTakeoff.cpp
+ * Runway takeoff handling for fixed-wing UAVs with steerable wheels.
+ *
+ * @author Roman Bapst <roman@px4.io>
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "RunwayTakeoff.h"
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <mavlink/mavlink_log.h>
+#include <mathlib/mathlib.h>
+
+namespace runwaytakeoff
+{
+
+RunwayTakeoff::RunwayTakeoff() :
+	SuperBlock(NULL, "RWTO"),
+	_state(),
+	_initialized(false),
+	_initialized_time(0),
+	_init_yaw(0),
+	_climbout(false),
+	_throttle_ramp_time(2 * 1e6),
+	_start_wp(),
+	_runway_takeoff_enabled(this, "TKOFF"),
+	_heading_mode(this, "HDG"),
+	_nav_alt(this, "NAV_ALT"),
+	_takeoff_throttle(this, "MAX_THR"),
+	_runway_pitch_sp(this, "PSP"),
+	_max_takeoff_pitch(this, "MAX_PITCH"),
+	_max_takeoff_roll(this, "MAX_ROLL"),
+	_min_airspeed_scaling(this, "AIRSPD_SCL"),
+	_airspeed_min(this, "FW_AIRSPD_MIN", false),
+	_climbout_diff(this, "FW_CLMBOUT_DIFF", false)
+{
+
+	updateParams();
+}
+
+RunwayTakeoff::~RunwayTakeoff()
+{
+
+}
+
+void RunwayTakeoff::init(float yaw, double current_lat, double current_lon)
+{
+	_init_yaw = yaw;
+	_initialized = true;
+	_state = RunwayTakeoffState::THROTTLE_RAMP;
+	_initialized_time = hrt_absolute_time();
+	_climbout = true; // this is true until climbout is finished
+	_start_wp(0) = (float)current_lat;
+	_start_wp(1) = (float)current_lon;
+}
+
+void RunwayTakeoff::update(float airspeed, float alt_agl,
+			   double current_lat, double current_lon, int mavlink_fd)
+{
+
+	switch (_state) {
+	case RunwayTakeoffState::THROTTLE_RAMP:
+		if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) {
+			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
+		}
+
+		break;
+
+	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+		if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) {
+			_state = RunwayTakeoffState::TAKEOFF;
+			mavlink_log_info(mavlink_fd, "#Takeoff airspeed reached");
+		}
+
+		break;
+
+	case RunwayTakeoffState::TAKEOFF:
+		if (alt_agl > _nav_alt.get()) {
+			_state = RunwayTakeoffState::CLIMBOUT;
+
+			/*
+			 * If we started in heading hold mode, move the navigation start WP to the current location now.
+			 * The navigator will take this as starting point to navigate towards the takeoff WP.
+			 */
+			if (_heading_mode.get() == 0) {
+				_start_wp(0) = (float)current_lat;
+				_start_wp(1) = (float)current_lon;
+			}
+
+			mavlink_log_info(mavlink_fd, "#Climbout");
+		}
+
+		break;
+
+	case RunwayTakeoffState::CLIMBOUT:
+		if (alt_agl > _climbout_diff.get()) {
+			_climbout = false;
+			_state = RunwayTakeoffState::FLY;
+			mavlink_log_info(mavlink_fd, "#Navigating to waypoint");
+		}
+
+		break;
+
+	default:
+		break;
+	}
+}
+
+/*
+ * Returns true as long as we're below navigation altitude
+ */
+bool RunwayTakeoff::controlYaw()
+{
+	// keep controlling yaw directly until we start navigation
+	return _state < RunwayTakeoffState::CLIMBOUT;
+}
+
+/*
+ * Returns pitch setpoint to use.
+ *
+ * Limited (parameter) as long as the plane is on runway. Otherwise
+ * use the one from TECS
+ */
+float RunwayTakeoff::getPitch(float tecsPitch)
+{
+	if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
+		return math::radians(_runway_pitch_sp.get());
+	}
+
+	return tecsPitch;
+}
+
+/*
+ * Returns the roll setpoint to use.
+ */
+float RunwayTakeoff::getRoll(float navigatorRoll)
+{
+	// until we have enough ground clearance, set roll to 0
+	if (_state < RunwayTakeoffState::CLIMBOUT) {
+		return 0.0f;
+	}
+
+	// allow some roll during climbout
+	else if (_state < RunwayTakeoffState::FLY) {
+		return math::constrain(navigatorRoll,
+				       math::radians(-_max_takeoff_roll.get()),
+				       math::radians(_max_takeoff_roll.get()));
+	}
+
+	return navigatorRoll;
+}
+
+/*
+ * Returns the yaw setpoint to use.
+ *
+ * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
+ * runway. When it has enough ground clearance we start navigation towards WP.
+ */
+float RunwayTakeoff::getYaw(float navigatorYaw)
+{
+	if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
+		return _init_yaw;
+
+	} else {
+		return navigatorYaw;
+	}
+}
+
+/*
+ * Returns the throttle setpoint to use.
+ *
+ * Ramps up in the beginning, until it lifts off the runway it is set to
+ * parameter value, then it returns the TECS throttle.
+ */
+float RunwayTakeoff::getThrottle(float tecsThrottle)
+{
+	switch (_state) {
+	case RunwayTakeoffState::THROTTLE_RAMP: {
+			float throttle = hrt_elapsed_time(&_initialized_time) / (float)_throttle_ramp_time *
+					 _takeoff_throttle.get();
+			return throttle < _takeoff_throttle.get() ?
+			       throttle :
+			       _takeoff_throttle.get();
+		}
+
+	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
+		return _takeoff_throttle.get();
+
+	default:
+		return tecsThrottle;
+	}
+}
+
+bool RunwayTakeoff::resetIntegrators()
+{
+	// reset integrators if we're still on runway
+	return _state < RunwayTakeoffState::TAKEOFF;
+}
+
+/*
+ * Returns the minimum pitch for TECS to use.
+ *
+ * In climbout we either want what was set on the waypoint (sp_min) but at least
+ * the climbtout minimum pitch (parameter).
+ * Otherwise use the minimum that is enforced generally (parameter).
+ */
+float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min)
+{
+	if (_state < RunwayTakeoffState::FLY) {
+		return math::max(sp_min, climbout_min);
+	}
+
+	else {
+		return min;
+	}
+}
+
+/*
+ * Returns the maximum pitch for TECS to use.
+ *
+ * Limited by parameter (if set) until climbout is done.
+ */
+float RunwayTakeoff::getMaxPitch(float max)
+{
+	// use max pitch from parameter if set (> 0.1)
+	if (_state < RunwayTakeoffState::FLY && _max_takeoff_pitch.get() > 0.1f) {
+		return _max_takeoff_pitch.get();
+	}
+
+	else {
+		return max;
+	}
+}
+
+/*
+ * Returns the "previous" (start) WP for navigation.
+ */
+math::Vector<2> RunwayTakeoff::getStartWP()
+{
+	return _start_wp;
+}
+
+void RunwayTakeoff::reset()
+{
+	_initialized = false;
+	_state = RunwayTakeoffState::THROTTLE_RAMP;
+}
+
+}

src/lib/runway_takeoff/RunwayTakeoff.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 RunwayTakeoff.h
+ * Runway takeoff handling for fixed-wing UAVs with steerable wheels.
+ *
+ * @author Roman Bapst <roman@px4.io>
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#ifndef RUNWAYTAKEOFF_H
+#define RUNWAYTAKEOFF_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <mavlink/mavlink_log.h>
+#include <mathlib/mathlib.h>
+
+namespace runwaytakeoff
+{
+
+enum RunwayTakeoffState {
+	THROTTLE_RAMP = 0, /**< ramping up throttle */
+	CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
+	TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
+	CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
+	FLY = 4 /**< fly towards takeoff waypoint */
+};
+
+class __EXPORT RunwayTakeoff : public control::SuperBlock
+{
+public:
+	RunwayTakeoff();
+	~RunwayTakeoff();
+
+	void init(float yaw, double current_lat, double current_lon);
+	void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd);
+
+	RunwayTakeoffState getState() { return _state; };
+	bool isInitialized() { return _initialized; };
+
+	bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
+	float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); };
+	float getInitYaw() { return _init_yaw; };
+
+	bool controlYaw();
+	bool climbout() { return _climbout; };
+	float getPitch(float tecsPitch);
+	float getRoll(float navigatorRoll);
+	float getYaw(float navigatorYaw);
+	float getThrottle(float tecsThrottle);
+	bool resetIntegrators();
+	float getMinPitch(float sp_min, float climbout_min, float min);
+	float getMaxPitch(float max);
+	math::Vector<2> getStartWP();
+
+	void reset();
+
+protected:
+private:
+	/** state variables **/
+	RunwayTakeoffState _state;
+	bool _initialized;
+	hrt_abstime _initialized_time;
+	float _init_yaw;
+	bool _climbout;
+	unsigned _throttle_ramp_time;
+	math::Vector<2> _start_wp;
+
+	/** parameters **/
+	control::BlockParamInt _runway_takeoff_enabled;
+	control::BlockParamInt _heading_mode;
+	control::BlockParamFloat _nav_alt;
+	control::BlockParamFloat _takeoff_throttle;
+	control::BlockParamFloat _runway_pitch_sp;
+	control::BlockParamFloat _max_takeoff_pitch;
+	control::BlockParamFloat _max_takeoff_roll;
+	control::BlockParamFloat _min_airspeed_scaling;
+	control::BlockParamFloat _airspeed_min;
+	control::BlockParamFloat _climbout_diff;
+
+};
+
+}
+
+#endif // RUNWAYTAKEOFF_H

src/lib/runway_takeoff/runway_takeoff_params.c

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 runway_takeoff_params.c
+ *
+ * Parameters for runway takeoff
+ *
+ * @author Andreas Antener <andreas@uaventure.com>
+ */
+
+#include <px4_config.h>
+
+#include <systemlib/param/param.h>
+
+/**
+ * Enable or disable runway takeoff with landing gear
+ *
+ * 0: disabled, 1: enabled
+ *
+ * @min 0
+ * @max 1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
+
+/**
+ * Specifies which heading should be held during runnway takeoff.
+ *
+ * 0: airframe heading, 1: heading towards takeoff waypoint
+ *
+ * @min 0
+ * @max 1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_INT32(RWTO_HDG, 0);
+
+/**
+ * Altitude AGL at which we have enough ground clearance to allow some roll.
+ * Until RWTO_NAV_ALT is reached the plane is held level and only
+ * rudder is used to keep the heading (see RWTO_HDG). This should be below
+ * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0.
+ *
+ * @unit m
+ * @min 0.0
+ * @max 100.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
+
+/**
+ * Max throttle during runway takeoff.
+ * (Can be used to test taxi on runway)
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
+
+/**
+ * Pitch setpoint during taxi / before takeoff airspeed is reached.
+ * A taildragger with stearable wheel might need to pitch up
+ * a little to keep it's wheel on the ground before airspeed
+ * to takeoff is reached.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 20.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
+
+/**
+ * Max pitch during takeoff.
+ * Fixed-wing settings are used if set to 0. Note that there is also a minimum
+ * pitch of 10 degrees during takeoff, so this must be larger if set.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 60.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
+
+/**
+ * Max roll during climbout.
+ * Roll is limited during climbout to ensure enough lift and prevents aggressive
+ * navigation before we're on a safe height.
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 60.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
+
+/**
+ * Min. airspeed scaling factor for takeoff.
+ * Pitch up will be commanded when the following airspeed is reached:
+ * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL
+ *
+ * @min 0.0
+ * @max 2.0
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);

src/lib/terrain_estimation/CMakeLists.txt

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE lib__terrain_estimation
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		terrain_estimator.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -0,0 +1,201 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.cpp
+ * A terrain estimation kalman filter.
+ */
+
+#include "terrain_estimator.h"
+
+#define DISTANCE_TIMEOUT 100000		// time in usec after which laser is considered dead
+
+TerrainEstimator::TerrainEstimator() :
+	_distance_last(0.0f),
+	_terrain_valid(false),
+	_time_last_distance(0),
+	_time_last_gps(0)
+{
+	memset(&_x._data[0], 0, sizeof(_x._data));
+	_u_z = 0.0f;
+	_P.setIdentity();
+}
+
+bool TerrainEstimator::is_distance_valid(float distance)
+{
+	if (distance > 40.0f || distance < 0.00001f) {
+		return false;
+
+	} else {
+		return true;
+	}
+}
+
+void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude,
+			       const struct sensor_combined_s *sensor,
+			       const struct distance_sensor_s *distance)
+{
+	if (attitude->R_valid) {
+		matrix::Matrix<float, 3, 3> R_att(attitude->R);
+		matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
+		matrix::Vector<float, 3> u;
+		u = R_att * a;
+		_u_z = u(2) + 9.81f; // compensate for gravity
+
+	} else {
+		_u_z = 0.0f;
+	}
+
+	// dynamics matrix
+	matrix::Matrix<float, n_x, n_x> A;
+	A.setZero();
+	A(0, 1) = 1;
+	A(1, 2) = 1;
+
+	// input matrix
+	matrix::Matrix<float, n_x, 1>  B;
+	B.setZero();
+	B(1, 0) = 1;
+
+	// input noise variance
+	float R = 0.135f;
+
+	// process noise convariance
+	matrix::Matrix<float, n_x, n_x>  Q;
+	Q(0, 0) = 0;
+	Q(1, 1) = 0;
+
+	// do prediction
+	matrix::Vector<float, n_x>  dx = (A * _x) * dt;
+	dx(1) += B(1, 0) * _u_z * dt;
+
+	// propagate state and covariance matrix
+	_x += dx;
+	_P += (A * _P + _P * A.transpose() +
+	       B * R * B.transpose() + Q) * dt;
+}
+
+void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
+		const struct distance_sensor_s *distance,
+		const struct vehicle_attitude_s *attitude)
+{
+	// terrain estimate is invalid if we have range sensor timeout
+	if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) {
+		_terrain_valid = false;
+	}
+
+	if (distance->timestamp > _time_last_distance) {
+
+		float d = distance->current_distance;
+
+		matrix::Matrix<float, 1, n_x> C;
+		C(0, 0) = -1; // measured altitude,
+
+		float R = 0.009f;
+
+		matrix::Vector<float, 1> y;
+		y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch);
+
+		// residual
+		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
+		S_I(0, 0) += R;
+		S_I = matrix::inv<float, 1> (S_I);
+		matrix::Vector<float, 1> r = y - C * _x;
+
+		matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
+
+		// some sort of outlayer rejection
+		if (fabsf(distance->current_distance - _distance_last) < 1.0f) {
+			_x += K * r;
+			_P -= K * C * _P;
+		}
+
+		// if the current and the last range measurement are bad then we consider the terrain
+		// estimate to be invalid
+		if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) {
+			_terrain_valid = false;
+
+		} else {
+			_terrain_valid = true;
+		}
+
+		_time_last_distance = distance->timestamp;
+		_distance_last = distance->current_distance;
+	}
+
+	if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) {
+		matrix::Matrix<float, 1, n_x> C;
+		C(0, 1) = 1;
+
+		float R = 0.056f;
+
+		matrix::Vector<float, 1> y;
+		y(0) = gps->vel_d_m_s;
+
+		// residual
+		matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());
+		S_I(0, 0) += R;
+		S_I = matrix::inv<float, 1>(S_I);
+		matrix::Vector<float, 1> r = y - C * _x;
+
+		matrix::Matrix<float, n_x, 1> K = _P * C.transpose() * S_I;
+		_x += K * r;
+		_P -= K * C * _P;
+
+		_time_last_gps = gps->timestamp_position;
+	}
+
+	// reinitialise filter if we find bad data
+	bool reinit = false;
+
+	for (int i = 0; i < n_x; i++) {
+		if (!PX4_ISFINITE(_x(i))) {
+			reinit = true;
+		}
+	}
+
+	for (int i = 0; i < n_x; i++) {
+		for (int j = 0; j < n_x; j++) {
+			if (!PX4_ISFINITE(_P(i, j))) {
+				reinit = true;
+			}
+		}
+	}
+
+	if (reinit) {
+		memset(&_x._data[0], 0, sizeof(_x._data));
+		_P.setZero();
+		_P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f;
+	}
+
+}

src/lib/terrain_estimation/terrain_estimator.h

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 Roman Bapst. 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 terrain_estimator.h
+ */
+
+#include <lib/mathlib/mathlib.h>
+#include "matrix/Matrix.hpp"
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/distance_sensor.h>
+
+
+/*
+* This class can be used to estimate distance to the ground using a laser range finder.
+* It's assumed that the laser points down vertically if the vehicle is in it's neutral pose.
+* The predict(...) function will do a state prediciton based on accelerometer inputs. It also
+* considers accelerometer bias.
+* The measurement_update(...) function does a measurement update based on range finder and gps
+* velocity measurements. Both functions should always be called together when there is new
+* acceleration data available.
+* The is_valid() function provides information whether the estimate is valid.
+*/
+
+class __EXPORT TerrainEstimator
+{
+public:
+	TerrainEstimator();
+	~TerrainEstimator() {};
+
+	bool is_valid() {return _terrain_valid;}
+	float get_distance_to_ground() {return -_x(0);}
+	float get_velocity() {return _x(1);};
+
+	void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor,
+		     const struct distance_sensor_s *distance);
+	void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps,
+				const struct distance_sensor_s *distance,
+				const struct vehicle_attitude_s *attitude);
+
+private:
+	enum {n_x = 3};
+
+	float _distance_last;
+	bool _terrain_valid;
+
+	// kalman filter variables
+	matrix::Vector<float, n_x> _x;		// state: ground distance, velocity, accel bias in z direction
+	float  _u_z;			// acceleration in earth z direction
+	matrix::Matrix<float, 3, 3> _P;	// covariance matrix
+
+	// timestamps
+	uint64_t _time_last_distance;
+	uint64_t _time_last_gps;
+
+	/*
+	struct {
+		float var_acc_z;
+		float var_p_z;
+		float var_p_vz;
+		float var_lidar;
+		float var_gps_vz;
+	} _params;
+	*/
+
+	bool is_distance_valid(float distance);
+
+};