mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-28 06:24:10 +01:00
LP-621 fix
This commit is contained in:
parent
7c9f04d87c
commit
745835e298
@ -107,7 +107,7 @@ static inline void vector_normalizef(float *vector, const uint8_t dim)
|
||||
{
|
||||
float length = vector_lengthf(vector, dim);
|
||||
|
||||
if (length <= 0.0f || isnan(length)) {
|
||||
if (length <= 0.0f || !IS_REAL(length)) {
|
||||
return;
|
||||
}
|
||||
for (int t = 0; t < dim; t++) {
|
||||
|
@ -538,7 +538,7 @@ static int32_t updateSensorsCC3D(AccelStateData *accelStateData, GyroStateData *
|
||||
accels[2] *= accel_scale.Z * invcount;
|
||||
temp *= invcount;
|
||||
|
||||
if (isnan(temperature)) {
|
||||
if (!IS_REAL(temperature)) {
|
||||
temperature = temp;
|
||||
}
|
||||
temperature = temp_alpha * (temp - temperature) + temperature;
|
||||
@ -740,7 +740,7 @@ __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accel
|
||||
|
||||
// If quaternion has become inappropriately short or is nan reinit.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
|
||||
if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) {
|
||||
q[0] = 1;
|
||||
q[1] = 0;
|
||||
q[2] = 0;
|
||||
|
@ -518,7 +518,7 @@ static void handleBaro(float sample, float temperature)
|
||||
|
||||
float altitude = 44330.0f * (1.0f - powf((sample) / PIOS_CONST_MKS_STD_ATMOSPHERE_F, (1.0f / 5.255f)));
|
||||
|
||||
if (!isnan(altitude)) {
|
||||
if (IS_REAL(altitude)) {
|
||||
BaroSensorData data;
|
||||
data.Altitude = altitude;
|
||||
data.Temperature = temperature;
|
||||
@ -530,7 +530,7 @@ static void handleBaro(float sample, float temperature)
|
||||
|
||||
static void updateAccelTempBias(float temperature)
|
||||
{
|
||||
if (isnan(accel_temperature)) {
|
||||
if (!IS_REAL(accel_temperature)) {
|
||||
accel_temperature = temperature;
|
||||
}
|
||||
accel_temperature = temp_alpha_gyro_accel * (temperature - accel_temperature) + accel_temperature;
|
||||
@ -552,7 +552,7 @@ static void updateAccelTempBias(float temperature)
|
||||
|
||||
static void updateGyroTempBias(float temperature)
|
||||
{
|
||||
if (isnan(gyro_temperature)) {
|
||||
if (!IS_REAL(gyro_temperature)) {
|
||||
gyro_temperature = temperature;
|
||||
}
|
||||
|
||||
@ -573,7 +573,7 @@ static void updateGyroTempBias(float temperature)
|
||||
|
||||
static void updateBaroTempBias(float temperature)
|
||||
{
|
||||
if (isnan(baro_temperature)) {
|
||||
if (!IS_REAL(baro_temperature)) {
|
||||
baro_temperature = temperature;
|
||||
}
|
||||
|
||||
|
@ -535,7 +535,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
|
||||
|
||||
// If quaternion has become inappropriately short or is nan reinit.
|
||||
// THIS SHOULD NEVER ACTUALLY HAPPEN
|
||||
if ((fabsf(inv_qmag) > 1e3f) || isnan(inv_qmag)) {
|
||||
if ((fabsf(inv_qmag) > 1e3f) || !IS_REAL(inv_qmag)) {
|
||||
this->first_run = 1;
|
||||
return FILTERRESULT_WARNING;
|
||||
}
|
||||
|
@ -481,7 +481,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
|
||||
// check for invalid variance values
|
||||
static inline bool invalid_var(float data)
|
||||
{
|
||||
if (isnan(data) || isinf(data)) {
|
||||
if (!IS_REAL(data)) {
|
||||
return true;
|
||||
}
|
||||
if (data < 1e-15f) { // var should not be close to zero. And not negative either.
|
||||
|
@ -282,9 +282,9 @@ void magOffsetEstimation(struct data *this, float mag[3])
|
||||
delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]);
|
||||
delta[2] = -rate * (Rz - B_e[2]);
|
||||
|
||||
if (!isnan(delta[0]) && !isinf(delta[0]) &&
|
||||
!isnan(delta[1]) && !isinf(delta[1]) &&
|
||||
!isnan(delta[2]) && !isinf(delta[2])) {
|
||||
if (IS_REAL(delta[0]) &&
|
||||
IS_REAL(delta[1]) &&
|
||||
IS_REAL(delta[2])) {
|
||||
this->magBias[0] += delta[0];
|
||||
this->magBias[1] += delta[1];
|
||||
this->magBias[2] += delta[2];
|
||||
|
@ -66,20 +66,26 @@
|
||||
#define M_EULER_D 0.57721566490153286060651209008d /* Euler constant */
|
||||
|
||||
// Conversion macro
|
||||
#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F))
|
||||
#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f))
|
||||
#define RAD2DEG(rad) ((rad) * (180.0f / M_PI_F))
|
||||
#define DEG2RAD(deg) ((deg) * (M_PI_F / 180.0f))
|
||||
|
||||
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
|
||||
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
|
||||
#define RAD2DEG_D(rad) ((rad) * (180.0d / M_PI_D))
|
||||
#define DEG2RAD_D(deg) ((deg) * (M_PI_D / 180.0d))
|
||||
|
||||
// helper macros for LPFs
|
||||
#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
|
||||
#define LPF_ALPHA(dt, fc) (dt / (dt + 1.0f / (2.0f * M_PI_F * fc)))
|
||||
|
||||
// Useful math macros
|
||||
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||
#define MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||
#define MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||
|
||||
#define IS_REAL(f) (isfinite(f))
|
||||
#ifndef isfinite
|
||||
#include <math.h>
|
||||
#endif
|
||||
__attribute__((optimize("no-fast-math"))) static inline int IS_REAL(float f)
|
||||
{
|
||||
return isfinite(f);
|
||||
}
|
||||
|
||||
// Bitfield access
|
||||
#define IS_SET(field, mask) (((field) & (mask)) == (mask))
|
||||
|
Loading…
Reference in New Issue
Block a user