1
0
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:
Eric Price 2021-08-14 18:19:46 +02:00
parent 7c9f04d87c
commit 745835e298
7 changed files with 26 additions and 20 deletions

View File

@ -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++) {

View File

@ -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;

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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.

View File

@ -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];

View File

@ -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))