pulled from PX4 master

5 files changed, 20 insertions, 16 deletions

diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
index f5f59a2dc..4593c4887 100644
--- a/src/lib/conversion/module.mk
+++ b/src/lib/conversion/module.mk
@@ -36,3 +36,5 @@
 #
 
 SRCS		 = rotation.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 5187b448f..917c7f830 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -74,6 +74,7 @@ enum Rotation {
 	ROTATION_ROLL_270_YAW_135    = 23,
 	ROTATION_PITCH_90            = 24,
 	ROTATION_PITCH_270           = 25,
+	ROTATION_ROLL_270_YAW_270    = 26,
 	ROTATION_MAX
 };
 
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
 	{270,   0,  90 },
 	{270,   0, 135 },
 	{  0,  90,   0 },
-	{  0, 270,   0 }
+	{  0, 270,   0 },
+	{270,   0, 270 }
 };
 
 /**
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 023bd71bf..0ed210cf4 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state)
 //	// 	_hgt_rate_dem);
 
 	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
 	_hgt_dem_adj_last = _hgt_dem_adj;
 
-	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
 	// Limit height rate of change
 	if (_hgt_rate_dem > _maxClimbRate) {
 		_hgt_rate_dem = _maxClimbRate;
@@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 	// Calculate throttle demand
 	// If underspeed condition is set, then demand full throttle
 	if (_underspeed) {
-		_throttle_dem_unc = 1.0f;
+		_throttle_dem = 1.0f;
 
 	} else {
 		// Calculate gain scaler from specific energy error to throttle
@@ -363,10 +363,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		} else {
 			_throttle_dem = ff_throttle;
 		}
-	}
 
-	// Constrain throttle demand
-	_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+		// Constrain throttle demand
+		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+	}
 }
 
 void TECS::_detect_bad_descent(void)
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 8ac31de6f..eb45237b7 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -345,9 +345,6 @@ private:
 	// climbout mode
 	bool _climbOutDem;
 
-	// throttle demand before limiting
-	float _throttle_dem_unc;
-
 	// pitch demand before limiting
 	float _pitch_dem_unc;
 
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 1c8d2a2a7..f595467b3 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -362,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
 	crosstrack_error->distance = 0.0f;
 	crosstrack_error->bearing = 0.0f;
 
+	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
+
 	// Return error if arguments are bad
-	if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
+	if (dist_to_end < 0.1f) {
+		return ERROR;
+	}
 
 	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -377,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
 		return return_value;
 	}
 
-	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
 	crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
 
 	if (sin(bearing_diff) >= 0) {
@@ -414,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 	crosstrack_error->bearing = 0.0f;
 
 	// Return error if arguments are bad
-	if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
+	if (radius < 0.1f) { return return_value; }
 
 
-	if (arc_sweep >= 0) {
+	if (arc_sweep >= 0.0f) {
 		bearing_sector_start = arc_start_bearing;
 		bearing_sector_end = arc_start_bearing + arc_sweep;
 
@@ -463,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 
 		double start_disp_x = (double)radius * sin(arc_start_bearing);
 		double start_disp_y = (double)radius * cos(arc_start_bearing);
-		double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
-		double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
+		double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
+		double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
 		double lon_start = lon_now + start_disp_x / 111111.0;
 		double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
 		double lon_end = lon_now + end_disp_x / 111111.0;
@@ -484,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
 
 	}
 
-	crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
+	crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
 	return_value = OK;
 	return return_value;
 }