diff options
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/conversion/rotation.cpp | 142 | ||||
-rw-r--r-- | src/lib/conversion/rotation.h | 8 | ||||
-rw-r--r-- | src/lib/ecl/module.mk | 2 | ||||
-rw-r--r-- | src/lib/external_lgpl/module.mk | 2 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 49 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 74 | ||||
-rw-r--r-- | src/lib/geo/geo.c | 24 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp | 25 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp | 21 | ||||
-rw-r--r-- | src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 12 |
10 files changed, 293 insertions, 66 deletions
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 614877b18..e17715733 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -49,3 +49,145 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) rot_matrix->from_euler(roll, pitch, yaw); } + +#define HALF_SQRT_2 0.70710678118654757f + +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z) +{ + float tmp; + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_YAW_180: + x = -x; y = -y; + return; + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2*(y - x); + y = -HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; + return; + } + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2*(y - x); + y = HALF_SQRT_2*(y + x); + x = tmp; z = -z; + return; + } + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(y - x); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2*(x - y); + y = -HALF_SQRT_2*(x + y); + x = tmp; z = -z; + return; + } + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2*(x - y); + y = HALF_SQRT_2*(x + y); + x = tmp; + return; + } + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2*(x + y); + y = HALF_SQRT_2*(x - y); + x = tmp; + return; + } + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + } +} diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 0c56494c5..5187b448f 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -118,4 +118,12 @@ const rot_lookup_t rot_lookup[] = { __EXPORT void get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); + +/** + * rotate a 3 element float vector in-place + */ +__EXPORT void +rotate_3f(enum Rotation rot, float &x, float &y, float &z); + + #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index f2aa3db6a..93a5b511f 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ l1/ecl_l1_pos_controller.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/external_lgpl/module.mk b/src/lib/external_lgpl/module.mk index 53f1629e3..29d3514f6 100644 --- a/src/lib/external_lgpl/module.mk +++ b/src/lib/external_lgpl/module.mk @@ -46,3 +46,5 @@ # SRCS = tecs/tecs.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 3730b1920..a57a0481a 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM } else { // Calculate gain scaler from specific energy error to throttle - float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min)); + float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; @@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f); if (STEdot_dem >= 0) { - ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr); + ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr); } else { ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr; @@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM _throttle_dem = constrain(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); - _last_throttle_dem = _throttle_dem; } + // Ensure _last_throttle_dem is always initialized properly + // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!! + _last_throttle_dem = _throttle_dem; + // Calculate integrator state upper and lower limits // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand @@ -551,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f // Calculate pitch demand _update_pitch(); -// // Write internal variables to the log_tuning structure. This -// // structure will be logged in dataflash at 10Hz - // log_tuning.hgt_dem = _hgt_dem_adj; - // log_tuning.hgt = _integ3_state; - // log_tuning.dhgt_dem = _hgt_rate_dem; - // log_tuning.dhgt = _integ2_state; - // log_tuning.spd_dem = _TAS_dem_adj; - // log_tuning.spd = _integ5_state; - // log_tuning.dspd = _vel_dot; - // log_tuning.ithr = _integ6_state; - // log_tuning.iptch = _integ7_state; - // log_tuning.thr = _throttle_dem; - // log_tuning.ptch = _pitch_dem; - // log_tuning.dspd_dem = _TAS_rate_dem; + _tecs_state.timestamp = now; + + if (_underspeed) { + _tecs_state.mode = ECL_TECS_MODE_UNDERSPEED; + } else if (_badDescent) { + _tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_climbOutDem) { + _tecs_state.mode = ECL_TECS_MODE_CLIMBOUT; + } else { + // If no error flag applies, conclude normal + _tecs_state.mode = ECL_TECS_MODE_NORMAL; + } + + _tecs_state.hgt_dem = _hgt_dem_adj; + _tecs_state.hgt = _integ3_state; + _tecs_state.dhgt_dem = _hgt_rate_dem; + _tecs_state.dhgt = _integ2_state; + _tecs_state.spd_dem = _TAS_dem_adj; + _tecs_state.spd = _integ5_state; + _tecs_state.dspd = _vel_dot; + _tecs_state.ithr = _integ6_state; + _tecs_state.iptch = _integ7_state; + _tecs_state.thr = _throttle_dem; + _tecs_state.ptch = _pitch_dem; + _tecs_state.dspd_dem = _TAS_rate_dem; + } diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 5cafb1c79..bcc2d90e5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -28,6 +28,27 @@ class __EXPORT TECS { public: TECS() : + _tecs_state {}, + _update_50hz_last_usec(0), + _update_speed_last_usec(0), + _update_pitch_throttle_last_usec(0), + // TECS tuning parameters + _hgtCompFiltOmega(0.0f), + _spdCompFiltOmega(0.0f), + _maxClimbRate(2.0f), + _minSinkRate(1.0f), + _maxSinkRate(2.0f), + _timeConst(5.0f), + _timeConstThrot(8.0f), + _ptchDamp(0.0f), + _thrDamp(0.0f), + _integGain(0.0f), + _vertAccLim(0.0f), + _rollComp(0.0f), + _spdWeight(0.5f), + _heightrate_p(0.0f), + _speedrate_p(0.0f), + _throttle_dem(0.0f), _pitch_dem(0.0f), _integ1_state(0.0f), _integ2_state(0.0f), @@ -100,29 +121,42 @@ public: return _spdWeight; } - // log data on internal state of the controller. Called at 10Hz - // void log_data(DataFlash_Class &dataflash, uint8_t msgid); - - // struct PACKED log_TECS_Tuning { - // LOG_PACKET_HEADER; - // float hgt; - // float dhgt; - // float hgt_dem; - // float dhgt_dem; - // float spd_dem; - // float spd; - // float dspd; - // float ithr; - // float iptch; - // float thr; - // float ptch; - // float dspd_dem; - // } log_tuning; + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + struct tecs_state { + uint64_t timestamp; + float hgt; + float dhgt; + float hgt_dem; + float dhgt_dem; + float spd_dem; + float spd; + float dspd; + float ithr; + float iptch; + float thr; + float ptch; + float dspd_dem; + enum ECL_TECS_MODE mode; + }; + + void get_tecs_state(struct tecs_state& state) { + state = _tecs_state; + } void set_time_const(float time_const) { _timeConst = time_const; } + void set_time_const_throt(float time_const_throt) { + _timeConstThrot = time_const_throt; + } + void set_min_sink_rate(float rate) { _minSinkRate = rate; } @@ -188,6 +222,9 @@ public: } private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -204,6 +241,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 36be32158..ad1ff37e3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -460,23 +460,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // calculate the position of the start and end points. We should not be doing this often // as this function generally will not be called repeatedly when we are out of the sector. - // TO DO - this is messed up and won't compile - float start_disp_x = radius * sinf(arc_start_bearing); - float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0f; - float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = lon_now + end_disp_x / 111111.0f; - float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; - float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + 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 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; + double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; + double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; @@ -485,7 +483,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ea0cf4ca1..ca931e2da 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -69,27 +69,34 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * Initializes the elements to zero. */ - MatrixBase() { - arm_mat = {M, N, &data[0][0]}; + MatrixBase() : + data{}, + arm_mat{M, N, &data[0][0]} + { } + virtual ~MatrixBase() {}; + /** * copyt ctor */ - MatrixBase(const MatrixBase<M, N> &m) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const MatrixBase<M, N> &m) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, m.data, sizeof(data)); } - MatrixBase(const float *d) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float *d) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } - MatrixBase(const float d[M][N]) { - arm_mat = {M, N, &data[0][0]}; + MatrixBase(const float d[M][N]) : + arm_mat{M, N, &data[0][0]} + { memcpy(data, d, sizeof(data)); } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index c7323c215..0ddf77615 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -69,25 +69,32 @@ public: /** * trivial ctor - * note that this ctor will not initialize elements + * initializes elements to zero */ - VectorBase() { - arm_col = {N, 1, &data[0]}; + VectorBase() : + data{}, + arm_col{N, 1, &data[0]} + { + } + virtual ~VectorBase() {}; + /** * copy ctor */ - VectorBase(const VectorBase<N> &v) { - arm_col = {N, 1, &data[0]}; + VectorBase(const VectorBase<N> &v) : + arm_col{N, 1, &data[0]} + { memcpy(data, v.data, sizeof(data)); } /** * setting ctor */ - VectorBase(const float d[N]) { - arm_col = {N, 1, &data[0]}; + VectorBase(const float d[N]) : + arm_col{N, 1, &data[0]} + { memcpy(data, d, sizeof(data)); } diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 74cd5d78c..436065175 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -46,10 +46,18 @@ class __EXPORT LowPassFilter2p { public: // constructor - LowPassFilter2p(float sample_freq, float cutoff_freq) { + LowPassFilter2p(float sample_freq, float cutoff_freq) : + _cutoff_freq(cutoff_freq), + _a1(0.0f), + _a2(0.0f), + _b0(0.0f), + _b1(0.0f), + _b2(0.0f), + _delay_element_1(0.0f), + _delay_element_2(0.0f) + { // set initial parameters set_cutoff_frequency(sample_freq, cutoff_freq); - _delay_element_1 = _delay_element_2 = 0; } /** |