diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-04 21:36:44 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-04 21:36:44 +0100 |
commit | 9e6259d8ae4dfe34660be5a145863c0dfc50e37c (patch) | |
tree | 607987c183c86ad085a56003558e91814bd5ce73 /src/lib | |
parent | b809fad3b6d1e14ad7fb32174136e1c2c21f1da2 (diff) | |
parent | 86c80678ae8a7027751fadea1ceec48d0a811122 (diff) | |
download | px4-firmware-9e6259d8ae4dfe34660be5a145863c0dfc50e37c.tar.gz px4-firmware-9e6259d8ae4dfe34660be5a145863c0dfc50e37c.tar.bz2 px4-firmware-9e6259d8ae4dfe34660be5a145863c0dfc50e37c.zip |
Merge branch 'master' of github.com:PX4/Firmware into paul_estimator
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/ecl/ecl.h | 3 | ||||
-rw-r--r-- | src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 13 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 5 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 27 |
4 files changed, 26 insertions, 22 deletions
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h index e0f207696..aa3c5000a 100644 --- a/src/lib/ecl/ecl.h +++ b/src/lib/ecl/ecl.h @@ -38,7 +38,6 @@ */ #include <drivers/drv_hrt.h> -#include <geo/geo.h> #define ecl_absolute_time hrt_absolute_time -#define ecl_elapsed_time hrt_elapsed_time
\ No newline at end of file +#define ecl_elapsed_time hrt_elapsed_time diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 3b68a0a4e..d1c864d78 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -38,6 +38,8 @@ * */ +#include <float.h> + #include "ecl_l1_pos_controller.h" float ECL_L1_Pos_Controller::nav_roll() @@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con /* calculate the vector from waypoint A to current position */ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - /* store the normalized vector from waypoint A to current position */ - math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } /* calculate eta angle towards the loiter center */ diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 0f28bccad..3730b1920 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -3,13 +3,10 @@ #include "tecs.h" #include <ecl/ecl.h> #include <systemlib/err.h> +#include <geo/geo.h> using namespace math; -#ifndef CONSTANTS_ONE_G -#define CONSTANTS_ONE_G GRAVITY -#endif - /** * @file tecs.cpp * diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index d1ebacda1..5cafb1c79 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,16 +28,7 @@ class __EXPORT TECS { public: TECS() : - - _airspeed_enabled(false), - _throttle_slewrate(0.0f), - _climbOutDem(false), - _hgt_dem_prev(0.0f), - _hgt_dem_adj_last(0.0f), - _hgt_dem_in_old(0.0f), - _TAS_dem_last(0.0f), - _TAS_dem_adj(0.0f), - _TAS_dem(0.0f), + _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), _integ3_state(0.0f), @@ -45,8 +36,16 @@ public: _integ5_state(0.0f), _integ6_state(0.0f), _integ7_state(0.0f), - _pitch_dem(0.0f), _last_pitch_dem(0.0f), + _vel_dot(0.0f), + _TAS_dem(0.0f), + _TAS_dem_last(0.0f), + _hgt_dem_in_old(0.0f), + _hgt_dem_adj_last(0.0f), + _hgt_dem_prev(0.0f), + _TAS_dem_adj(0.0f), + _STEdotErrLast(0.0f), + _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), _SPEdot_dem(0.0f), @@ -55,9 +54,9 @@ public: _SKE_est(0.0f), _SPEdot(0.0f), _SKEdot(0.0f), - _vel_dot(0.0f), - _STEdotErrLast(0.0f) { - + _airspeed_enabled(false), + _throttle_slewrate(0.0f) + { } bool airspeed_sensor_enabled() { |