diff options
author | px4dev <px4@purgatory.org> | 2013-03-20 23:05:19 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2013-03-20 23:05:19 -0700 |
commit | db91dffb23cae1e8aa6c3945aa32b9d2e4ecd6a0 (patch) | |
tree | 0c6e8e86891285c45f7a60d857929a2a5e963599 /apps/systemlib | |
parent | f7b14b2e23113093d1f76565041b91f22be79246 (diff) | |
parent | b7d65bf8fc65b2fd7c98d46d60fb24fb937baa94 (diff) | |
download | px4-firmware-db91dffb23cae1e8aa6c3945aa32b9d2e4ecd6a0.tar.gz px4-firmware-db91dffb23cae1e8aa6c3945aa32b9d2e4ecd6a0.tar.bz2 px4-firmware-db91dffb23cae1e8aa6c3945aa32b9d2e4ecd6a0.zip |
Merge branch 'master' into export-build
Diffstat (limited to 'apps/systemlib')
-rw-r--r-- | apps/systemlib/airspeed.c | 54 | ||||
-rw-r--r-- | apps/systemlib/airspeed.h | 69 | ||||
-rw-r--r-- | apps/systemlib/conversions.c | 2 | ||||
-rw-r--r-- | apps/systemlib/conversions.h | 8 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer.cpp | 1 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer.h | 2 | ||||
-rw-r--r-- | apps/systemlib/mixer/mixer_group.cpp | 1 | ||||
-rw-r--r-- | apps/systemlib/param/param.c | 2 |
8 files changed, 85 insertions, 54 deletions
diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 5c68f8ea5..264287b10 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -40,14 +40,31 @@ * */ -#include "math.h" +#include <stdio.h> +#include <math.h> #include "conversions.h" #include "airspeed.h" -float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + } /** @@ -55,14 +72,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed current indicated airspeed + * @param speed_indicated current indicated airspeed * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); } /** @@ -70,12 +87,25 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + printf("[airspeed] Invalid air density, using density at sea level\n"); + } + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } } diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index b1beb79ae..def53f0c1 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -48,43 +48,42 @@ __BEGIN_DECLS -/** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return indicated airspeed in m/s - */ -__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); -/** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature); + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); -/** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); __END_DECLS diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 2b8003e45..ac94252c5 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9]) float get_air_density(float static_pressure, float temperature_celsius) { - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN)); + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index c2987709b..5d485b01f 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,10 +44,10 @@ #include <float.h> #include <stdint.h> -#define CONSTANTS_ONE_G 9.80665f -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f -#define CONSTANTS_AIR_GAS_CONST 8.31432f -#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f +#define CONSTANTS_ONE_G 9.80665f // m/s^2 +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 +#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C __BEGIN_DECLS diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90..df0dfe838 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 00ddf1581..71386cba7 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -160,7 +160,7 @@ public: * @param control_cb Callback invoked when reading controls. */ Mixer(ControlCallback control_cb, uintptr_t cb_handle); - ~Mixer() {}; + virtual ~Mixer() {}; /** * Perform the mixing function. diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff225..1dbc512cd 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index ebb72ca3e..8073570d1 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -87,7 +87,7 @@ struct param_wbuf_s { UT_array *param_values; /** array info for the modified parameters array */ -UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; +const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; /** parameter update topic */ ORB_DEFINE(parameter_update, struct parameter_update_s); |