5 files changed, 326 insertions, 32 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 46db788a6..926a8db2a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		 * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 //	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 //	warnx("roll: _last_output %.4f", (double)_last_output);
 	return math::constrain(_last_output, -1.0f, 1.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 9894a34d7..94bd26f03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * dt;
+		float id = _rate_error * dt * scaler;
 
 		/*
 		* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
 	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	_last_output = _bodyrate_setpoint * _k_ff * scaler +
+		_rate_error * _k_p * scaler * scaler
+		+ integrator_constrained;  //scaler is proportional to 1/airspeed
 
 	return math::constrain(_last_output, -1.0f, 1.0f);
 }
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index e600976ce..1c8d2a2a7 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
+ *   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
@@ -49,39 +49,124 @@
 #include <stdio.h>
 #include <math.h>
 #include <stdbool.h>
+#include <string.h>
+#include <float.h>
+
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
 
 /*
  * Azimuthal Equidistant Projection
  * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
  */
 
-__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
+static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
+
+__EXPORT bool map_projection_global_initialized()
+{
+	return map_projection_initialized(&mp_ref);
+}
+
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
+{
+	return ref->init_done;
+}
+
+__EXPORT uint64_t map_projection_global_timestamp()
+{
+	return map_projection_timestamp(&mp_ref);
+}
+
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
 {
-	ref->lat = lat_0 / 180.0 * M_PI;
-	ref->lon = lon_0 / 180.0 * M_PI;
+	return ref->timestamp;
+}
 
-	ref->sin_lat = sin(ref->lat);
-	ref->cos_lat = cos(ref->lat);
+__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	if (strcmp("commander", getprogname()) == 0) {
+		return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
+	} else {
+		return -1;
+	}
 }
 
-__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
-	double lat_rad = lat / 180.0 * M_PI;
-	double lon_rad = lon / 180.0 * M_PI;
+
+	ref->lat_rad = lat_0 * M_DEG_TO_RAD;
+	ref->lon_rad = lon_0 * M_DEG_TO_RAD;
+	ref->sin_lat = sin(ref->lat_rad);
+	ref->cos_lat = cos(ref->lat_rad);
+
+	ref->timestamp = timestamp;
+	ref->init_done = true;
+
+	return 0;
+}
+
+__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+{
+	return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
+}
+
+__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
+{
+	return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
+}
+
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	*ref_lat_rad = ref->lat_rad;
+	*ref_lon_rad = ref->lon_rad;
+
+	return 0;
+}
+
+__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
+{
+	return map_projection_project(&mp_ref, lat, lon, x, y);
+
+}
+
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
+{
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
+	double lat_rad = lat * M_DEG_TO_RAD;
+	double lon_rad = lon * M_DEG_TO_RAD;
 
 	double sin_lat = sin(lat_rad);
 	double cos_lat = cos(lat_rad);
-	double cos_d_lon = cos(lon_rad - ref->lon);
+	double cos_d_lon = cos(lon_rad - ref->lon_rad);
 
 	double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
-	double k = (c == 0.0) ? 1.0 : (c / sin(c));
+	double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
 
 	*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
-	*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
+	*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
+
+	return 0;
+}
+
+__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon)
+{
+	return map_projection_reproject(&mp_ref, x, y, lat, lon);
 }
 
-__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
 {
+	if (!map_projection_initialized(ref)) {
+		return -1;
+	}
+
 	double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
 	double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
 	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
@@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
 	double lat_rad;
 	double lon_rad;
 
-	if (c != 0.0) {
+	if (fabs(c) > DBL_EPSILON) {
 		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
-		lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+		lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
 
 	} else {
-		lat_rad = ref->lat;
-		lon_rad = ref->lon;
+		lat_rad = ref->lat_rad;
+		lon_rad = ref->lon_rad;
 	}
 
 	*lat = lat_rad * 180.0 / M_PI;
 	*lon = lon_rad * 180.0 / M_PI;
+
+	return 0;
+}
+
+__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0)
+{
+	if (!map_projection_global_initialized()) {
+		return -1;
+	}
+
+	if (lat_0 != NULL) {
+		*lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
+	}
+
+	if (lon_0 != NULL) {
+		*lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
+	}
+
+	return 0;
+
+}
+__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
+{
+	if (strcmp("commander", getprogname()) == 0) {
+		gl_ref.alt = alt_0;
+		if (!map_projection_global_init(lat_0, lon_0, timestamp))
+		{
+			gl_ref.init_done = true;
+			return 0;
+		} else {
+			gl_ref.init_done = false;
+			return -1;
+		}
+	} else {
+		return -1;
+	}
+}
+
+__EXPORT bool globallocalconverter_initialized()
+{
+	return gl_ref.init_done && map_projection_global_initialized();
 }
 
+__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
+{
+	if (!map_projection_global_initialized()) {
+		return -1;
+	}
+
+	map_projection_global_project(lat, lon, x, y);
+	*z = gl_ref.alt - alt;
+
+	return 0;
+}
+
+__EXPORT int globallocalconverter_toglobal(float x, float y, float z,  double *lat, double *lon, float *alt)
+{
+	if (!map_projection_global_initialized()) {
+		return -1;
+	}
+
+	map_projection_global_reproject(x, y, lat, lon);
+	*alt = gl_ref.alt - z;
+
+	return 0;
+}
+
+__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
+{
+	if (!map_projection_global_initialized()) {
+		return -1;
+	}
+
+	if (map_projection_global_getref(lat_0, lon_0))
+	{
+		return -1;
+	}
+
+	if (alt_0 != NULL) {
+		*alt_0 = gl_ref.alt;
+	}
+
+	return 0;
+}
 
 __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
 {
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 8b286af36..2311e0a7c 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -69,39 +69,162 @@ struct crosstrack_error_s {
 
 /* lat/lon are in radians */
 struct map_projection_reference_s {
-	double lat;
-	double lon;
+	double lat_rad;
+	double lon_rad;
 	double sin_lat;
 	double cos_lat;
+	bool init_done;
+	uint64_t timestamp;
 };
 
+struct globallocal_converter_reference_s {
+	float alt;
+	bool init_done;
+};
+
+/**
+ * Checks if global projection was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool map_projection_global_initialized(void);
+
+/**
+ * Checks if projection given as argument was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref);
+
+/**
+ * Get the timestamp of the global map projection
+ * @return the timestamp of the map_projection
+ */
+__EXPORT uint64_t map_projection_global_timestamp(void);
+
+/**
+ * Get the timestamp of the map projection given by the argument
+ * @return the timestamp of the map_projection
+ */
+__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
+
+/**
+ * Writes the reference values of the global projection to ref_lat and ref_lon
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
+
 /**
- * Initializes the map transformation.
+ * Writes the reference values of the projection given by the argument to ref_lat and ref_lon
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
+
+/**
+ * Initializes the global map transformation.
+ *
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
+
+/**
+ * Initializes the map transformation given by the argument.
+ *
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ */
+__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref,
+		double lat_0, double lon_0, uint64_t timestamp);
+
+/**
+ * Initializes the map transformation given by the argument and sets the timestamp to now.
  *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
+ * Initializes the transformation between the geographic coordinate system and
+ * the azimuthal equidistant plane
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
  */
-__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
+__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
 
 /**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
+ * Transforms a point in the geographic coordinate system to the local
+ * azimuthal equidistant plane using the global projection
  * @param x north
  * @param y east
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
  */
-__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
+__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y);
+
+
+ /* Transforms a point in the geographic coordinate system to the local
+  * azimuthal equidistant plane using the projection given by the argument
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
+
+/**
+ * Transforms a point in the local azimuthal equidistant plane to the
+ * geographic coordinate system using the global projection
+ *
+ * @param x north
+ * @param y east
+ * @param lat in degrees (47.1234567°, not 471234567°)
+ * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon);
 
 /**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
+ * Transforms a point in the local azimuthal equidistant plane to the
+ * geographic coordinate system using the projection given by the argument
  *
  * @param x north
  * @param y east
  * @param lat in degrees (47.1234567°, not 471234567°)
  * @param lon in degrees (8.1234567°, not 81234567°)
+ * @return 0 if map_projection_init was called before, -1 else
+ */
+__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
+
+/**
+ * Get reference position of the global map projection
+ */
+__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0);
+
+/**
+ * Initialize the global mapping between global position (spherical) and local position (NED).
+ */
+__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
+
+/**
+ * Checks if globallocalconverter was initialized
+ * @return true if map was initialized before, false else
+ */
+__EXPORT bool globallocalconverter_initialized(void);
+
+/**
+ * Convert from global position coordinates to local position coordinates using the global reference
+ */
+__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
+
+/**
+ * Convert from local position coordinates to global position coordinates using the global reference
+ */
+__EXPORT int globallocalconverter_toglobal(float x, float y, float z,  double *lat, double *lon, float *alt);
+
+/**
+ * Get reference position of the global to local converter
  */
-__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
+__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
 
 /**
  * Returns the distance to the next waypoint in meters.
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index c4425bbe0..2b876b629 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -67,10 +67,10 @@ void CatapultLaunchMethod::update(float accel_x)
 	last_timestamp = hrt_absolute_time();
 
 	if (accel_x > threshold_accel.get()) {
-		integrator += accel_x * dt;
+		integrator += dt;
 //		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
 //				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
-		if (integrator > threshold_accel.get() * threshold_time.get()) {
+		if (integrator > threshold_time.get()) {
 			launchDetected = true;
 		}