diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-11-10 17:16:30 +0100 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-11-10 17:16:30 +0100 |
commit | 31238516b537debd695f5cdf8e6c6e969907fe76 (patch) | |
tree | 8a1101258382a155832a2afa1141c27cbf061399 /src/lib | |
parent | 43ebaf287c86acbf7fba13b5203de68afcc79b44 (diff) | |
parent | 4e8e6e653beb21808f532b83c6c6e827103ea379 (diff) | |
download | px4-firmware-31238516b537debd695f5cdf8e6c6e969907fe76.tar.gz px4-firmware-31238516b537debd695f5cdf8e6c6e969907fe76.tar.bz2 px4-firmware-31238516b537debd695f5cdf8e6c6e969907fe76.zip |
pulled from PX4 master
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/conversion/module.mk | 2 | ||||
-rw-r--r-- | src/lib/conversion/rotation.h | 4 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 10 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 3 | ||||
-rw-r--r-- | src/lib/geo/geo.c | 17 |
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; } |