diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-31 23:41:54 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-08-31 23:41:54 +0200 |
commit | 544839657999756aa8b0f56d7490dff18d2c1fd1 (patch) | |
tree | a4b04ce8d39fa81c4faec7758db256d7371775ea /src/lib | |
parent | 25a8f2d8056deca5049a8acc27c77e5e17760167 (diff) | |
parent | 2780dc39ce5d47f2d9dfa921062100a1dc86c2be (diff) | |
download | px4-firmware-544839657999756aa8b0f56d7490dff18d2c1fd1.tar.gz px4-firmware-544839657999756aa8b0f56d7490dff18d2c1fd1.tar.bz2 px4-firmware-544839657999756aa8b0f56d7490dff18d2c1fd1.zip |
Merge branch 'master' into st24
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/attitude_fw/ecl_pitch_controller.cpp | 6 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 6 | ||||
-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 | 52 | ||||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 84 | ||||
-rw-r--r-- | src/lib/geo/geo.c | 201 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 141 | ||||
-rw-r--r-- | src/lib/launchdetection/CatapultLaunchMethod.cpp | 4 | ||||
-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 |
14 files changed, 622 insertions, 84 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/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/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 6386e37a0..d27bf776f 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state) void TECS::_detect_underspeed(void) { + if (!_detect_underspeed_enabled) { + _underspeed = false; + return; + } + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { _underspeed = true; @@ -298,7 +303,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; @@ -327,9 +332,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 +559,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..36ae4ecaf 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), @@ -45,6 +66,9 @@ public: _hgt_dem_prev(0.0f), _TAS_dem_adj(0.0f), _STEdotErrLast(0.0f), + _underspeed(false), + _detect_underspeed_enabled(true), + _badDescent(false), _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), @@ -100,29 +124,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; } @@ -187,7 +224,14 @@ public: _speedrate_p = speedrate_p; } + void set_detect_underspeed_enabled(bool enabled) { + _detect_underspeed_enabled = enabled; + } + private: + + struct tecs_state _tecs_state; + // Last time update_50Hz was called uint64_t _update_50hz_last_usec; @@ -204,6 +248,7 @@ private: float _minSinkRate; float _maxSinkRate; float _timeConst; + float _timeConstThrot; float _ptchDamp; float _thrDamp; float _integGain; @@ -285,6 +330,9 @@ private: // Underspeed condition bool _underspeed; + // Underspeed detection enabled + bool _detect_underspeed_enabled; + // Bad descent condition caused by unachievable airspeed demand bool _badDescent; 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 c555a0a69..9d479832f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -66,10 +66,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; } 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; } /** |