mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
update sensor fusion
This commit is contained in:
ahmedsalah52
parent
f959083383
commit
6dd63aa5cf
6 changed files with 422 additions and 245 deletions
|
|
@ -169,12 +169,23 @@ float filterGetNotchQ(float centerFreq, float cutoffFreq)
|
|||
return centerFreq * cutoffFreq / (centerFreq * centerFreq - cutoffFreq * cutoffFreq);
|
||||
}
|
||||
|
||||
float filterGetBandQ(float lowerFreq, float higherFreq)
|
||||
{
|
||||
float centerFreq = sqrtf(lowerFreq * higherFreq);
|
||||
return centerFreq / (higherFreq - lowerFreq);
|
||||
}
|
||||
|
||||
/* sets up a biquad filter as a 2nd order butterworth LPF */
|
||||
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
|
||||
{
|
||||
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF, 1.0f);
|
||||
}
|
||||
|
||||
void biquadFilterInitHPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
|
||||
{
|
||||
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_HPF, 1.0f);
|
||||
}
|
||||
|
||||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight)
|
||||
{
|
||||
biquadFilterUpdate(filter, filterFreq, refreshRate, Q, filterType, weight);
|
||||
|
|
@ -202,6 +213,13 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
|
|||
filter->a1 = -2 * cs;
|
||||
filter->a2 = 1 - alpha;
|
||||
break;
|
||||
case FILTER_HPF:
|
||||
filter->b0 = 0.5f * (1 + cs);
|
||||
filter->b1 = -1 * (1 + cs); // the negative sign here is not in the TI document
|
||||
filter->b2 = filter->b0;
|
||||
filter->a1 = -2 * cs;
|
||||
filter->a2 = 1 - alpha;
|
||||
break;
|
||||
case FILTER_NOTCH:
|
||||
filter->b0 = 1;
|
||||
filter->b1 = -2 * cs;
|
||||
|
|
|
|||
|
|
@ -38,6 +38,7 @@ typedef enum {
|
|||
FILTER_LPF, // 2nd order Butterworth section
|
||||
FILTER_NOTCH,
|
||||
FILTER_BPF,
|
||||
FILTER_HPF,
|
||||
} biquadFilterType_e;
|
||||
|
||||
typedef struct pt1Filter_s {
|
||||
|
|
@ -116,8 +117,10 @@ void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
|
|||
float pt3FilterApply(pt3Filter_t *filter, float input);
|
||||
|
||||
float filterGetNotchQ(float centerFreq, float cutoffFreq);
|
||||
float filterGetBandQ(float lowerFreq, float higherFreq);
|
||||
|
||||
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
void biquadFilterInitHPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
|
||||
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType, float weight);
|
||||
void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
|
||||
|
|
|
|||
|
|
@ -59,12 +59,12 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/alt_hold.h"
|
||||
#include "flight/altitude.h"
|
||||
|
||||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
#include "io/beeper.h"
|
||||
#include "io/dashboard.h"
|
||||
#include "io/flashfs.h"
|
||||
#include "io/gimbal_control.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/ledstrip.h"
|
||||
#include "io/piniobox.h"
|
||||
|
|
@ -157,6 +157,7 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
|
|||
static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
|
||||
{
|
||||
accUpdate(currentTimeUs);
|
||||
updateAccItegralCallback(currentTimeUs);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -453,9 +454,6 @@ task_attribute_t task_attributes[TASK_COUNT] = {
|
|||
[TASK_RC_STATS] = DEFINE_TASK("RC_STATS", NULL, NULL, rcStatsUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
|
||||
#endif
|
||||
|
||||
#ifdef USE_GIMBAL
|
||||
[TASK_GIMBAL] = DEFINE_TASK("GIMBAL", NULL, NULL, gimbalUpdate, TASK_PERIOD_HZ(100), TASK_PRIORITY_MEDIUM),
|
||||
#endif
|
||||
};
|
||||
|
||||
task_t *getTask(unsigned taskId)
|
||||
|
|
@ -635,8 +633,4 @@ void tasksInit(void)
|
|||
#ifdef USE_RC_STATS
|
||||
setTaskEnabled(TASK_RC_STATS, true);
|
||||
#endif
|
||||
|
||||
#ifdef USE_GIMBAL
|
||||
setTaskEnabled(TASK_GIMBAL, true);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,6 +37,7 @@
|
|||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
|
||||
|
|
@ -50,67 +51,26 @@
|
|||
#include "flight/altitude.h"
|
||||
|
||||
|
||||
#define ACC_VAR_COEFF (1.0f)
|
||||
#define ACC_CONST_VAR (10.0f)
|
||||
#define SENSOR_VEL_ERROR_THRESH (1000.0f) // the error above this value excludes the sensor from the fusion and changes the offset
|
||||
#define SENSOR_VEL_MAX_ERROR (10000.0f)
|
||||
#define SENSOR_MAX_PENALITY_ITERS (100)
|
||||
#define SENSOR_MAX_OFFSET_ERROR (1000.0f)
|
||||
|
||||
#define BARO_VAR_ALT_COEFF (0.05f)
|
||||
#define BARO_VAR_VEL_COEFF (0.01f)
|
||||
#define BARO_VAR_TEMP_COEFF (0.5f)
|
||||
#define BARO_VAR_ALT_COEFF (0.01f)
|
||||
#define BARO_VAR_VEL_COEFF (0.01f)
|
||||
#define BARO_VAR_TEMP_COEFF (0.01f)
|
||||
#define BARO_VAR_VEL_ERROR_COEFF (1.00f)
|
||||
|
||||
#define GPS_VAR_VDOP_COEFF (1.0f)
|
||||
|
||||
#define RANGEFINDER_ACC_ERROR_THRESH (50.0f)
|
||||
#define RANGEFINDER_RAPID_ERR_THRESH (100.0f)
|
||||
#define RANGEFINDER_ACC_ERROR_THRESH (50.0f)
|
||||
#define RANGEFINDER_RAPID_ERR_THRESH (100.0f)
|
||||
#define RANGEFINDER_VAR_VEL_ERROR_COEFF (2.0f)
|
||||
#define RANGEFINDER_CONST_VAR (10.0f)
|
||||
|
||||
typedef struct sensorState_s {
|
||||
float currentAltReadingCm;
|
||||
float previousAltReadingCm;
|
||||
float zeroAltOffsetCm;
|
||||
float filteredAltReadingCm;
|
||||
float variance;
|
||||
timeMs_t deltaTimeMs;
|
||||
float VelocityCmSec;
|
||||
bool isValid;
|
||||
void (*updateReading)(struct sensorState_s *sensor);
|
||||
void (*updateVariance)(struct sensorState_s *sensor);
|
||||
void (*updateOffset)(struct sensorState_s *sensor);
|
||||
} sensorState_t;
|
||||
|
||||
typedef struct velocity3D_s {
|
||||
uint16_t value;
|
||||
bool isValid;
|
||||
} velocity3D_t;
|
||||
|
||||
// #ifdef USE_ACC
|
||||
// void updateAccReading(sensorState_t *sensor, bool ARMING_FLAG(ARMED));
|
||||
// void updateAccVariance(sensorState_t *sensor, bool ARMING_FLAG(ARMED));
|
||||
// float updateAccFilter(float value);
|
||||
// void updateAccItegralCallback(timeUs_t currentTimeUs);
|
||||
|
||||
// typedef struct accIntegral_s {
|
||||
// float vel[3];
|
||||
// timeDelta_t deltaTimeUs;
|
||||
// } accIntegral_t;
|
||||
// accIntegral_t accIntegral;
|
||||
// #endif
|
||||
|
||||
#ifdef USE_BARO
|
||||
void updateBaroReading(sensorState_t *sensor);
|
||||
void updateBaroVariance(sensorState_t *sensor);
|
||||
#endif
|
||||
|
||||
#ifdef USE_GPS
|
||||
void updateGpsReading(sensorState_t *sensor);
|
||||
void updateGpsVariance(sensorState_t *sensor);
|
||||
#endif
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
void updateRangefinderReading(sensorState_t *sensor);
|
||||
void updateRangefinderVariance(sensorState_t *sensor);
|
||||
void updateRangefinderOffset(sensorState_t *sensor);
|
||||
#endif
|
||||
|
||||
void updateSensorOffset(sensorState_t *sensor);
|
||||
#define GPS_VAR_DOP_COEFF (1.0f)
|
||||
#define GPS_VAR_VEL_ERROR_COEFF (2.0f)
|
||||
#define GPS_RAPID_ERR_THRESH (100.0f)
|
||||
#define GPS_ACC_ERROR_THRESH (200.0f)
|
||||
|
||||
typedef enum {
|
||||
#ifdef USE_BARO
|
||||
|
|
@ -120,165 +80,289 @@ typedef enum {
|
|||
SF_GPS,
|
||||
#endif
|
||||
// the only must for the order of the sensors is that the local sensors like the rangefinder should be last after the global sensors like the GPS and Baro
|
||||
#ifdef USE_ACC
|
||||
SF_ACC,
|
||||
#endif
|
||||
#ifdef USE_RANGEFINDER
|
||||
SF_RANGEFINDER,
|
||||
#endif
|
||||
SENSOR_COUNT
|
||||
} altSensor_e;
|
||||
|
||||
typedef struct sensorState_s {
|
||||
float currentAltReadingCm;
|
||||
float zeroAltOffsetCm;
|
||||
float velocityCmS;
|
||||
float variance;
|
||||
float offsetError;
|
||||
uint32_t velError;
|
||||
altSensor_e type;
|
||||
uint8_t penalityIters;
|
||||
bool isValid;
|
||||
bool toFuse;
|
||||
void (*updateReading)(struct sensorState_s *sensor);
|
||||
void (*updateVariance)(struct sensorState_s *sensor);
|
||||
void (*updateOffset)(struct sensorState_s *sensor);
|
||||
} sensorState_t;
|
||||
|
||||
typedef struct velocity3D_s {
|
||||
float value;
|
||||
bool isValid;
|
||||
} velocity3D_t;
|
||||
|
||||
#ifdef USE_ACC
|
||||
void updateAccReading(sensorState_t *sensor);
|
||||
void applyAccVelFilter(float *velocity);
|
||||
void updateAccItegralCallback(timeUs_t currentTimeUs);
|
||||
|
||||
typedef struct accIntegral_s {
|
||||
float vel[XYZ_AXIS_COUNT + 1]; // 3 axis + 1 for the 3D magnitude
|
||||
timeDelta_t deltaTimeUs;
|
||||
} accIntegral_t;
|
||||
accIntegral_t accIntegral;
|
||||
#endif
|
||||
|
||||
#ifdef USE_BARO
|
||||
void updateBaroReading(sensorState_t *sensor);
|
||||
void updateBaroVariance(sensorState_t *sensor);
|
||||
void applyBaroFilter(float *value);
|
||||
#endif
|
||||
|
||||
#ifdef USE_GPS
|
||||
void updateGpsReading(sensorState_t *sensor);
|
||||
void updateGpsVariance(sensorState_t *sensor);
|
||||
void updateGpsOffset(sensorState_t *sensor);
|
||||
void applyGpsFilter(float *value);
|
||||
#endif
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
void updateRangefinderReading(sensorState_t *sensor);
|
||||
void updateRangefinderVariance(sensorState_t *sensor);
|
||||
void updateRangefinderOffset(sensorState_t *sensor);
|
||||
void applyRangefinderFilter(float *value);
|
||||
#endif
|
||||
|
||||
void updateSensorOffset(sensorState_t *sensor);
|
||||
void updateVelError(sensorState_t *sensor);
|
||||
void updateSensorFusability(sensorState_t *sensor);
|
||||
void doNothing(sensorState_t *sensor);
|
||||
|
||||
sensorState_t altSenFusSources[SENSOR_COUNT];
|
||||
altitudeState_t altitudeState;
|
||||
velocity3D_t velocityDm3D; // for Decimeter/sec 0.1m/s, coming from the GPS if available
|
||||
velocity3D_t velocityCm3D;
|
||||
static KalmanFilter kf;
|
||||
|
||||
void altSensorFusionInit(void) {
|
||||
#ifdef USE_ACC
|
||||
altSenFusSources[SF_ACC].updateReading = updateAccReading;
|
||||
altSenFusSources[SF_ACC].updateVariance = doNothing;
|
||||
altSenFusSources[SF_ACC].updateOffset = doNothing;
|
||||
altSenFusSources[SF_ACC].type = SF_ACC;
|
||||
altSenFusSources[SF_ACC].isValid = false;
|
||||
altSenFusSources[SF_ACC].toFuse = false;
|
||||
velocityCm3D.isValid = true;
|
||||
#endif
|
||||
|
||||
#ifdef USE_BARO
|
||||
altSenFusSources[SF_BARO].updateReading = updateBaroReading;
|
||||
altSenFusSources[SF_BARO].updateVariance = updateBaroVariance;
|
||||
altSenFusSources[SF_BARO].updateOffset = updateSensorOffset;
|
||||
altSenFusSources[SF_BARO].currentAltReadingCm = 0;
|
||||
altSenFusSources[SF_BARO].variance = 0.0f;
|
||||
altSenFusSources[SF_BARO].zeroAltOffsetCm = 0.0f;
|
||||
altSenFusSources[SF_BARO].type = SF_BARO;
|
||||
altSenFusSources[SF_BARO].isValid = isBaroReady();
|
||||
altSenFusSources[SF_BARO].toFuse = true;
|
||||
#endif
|
||||
|
||||
#ifdef USE_GPS
|
||||
altSenFusSources[SF_GPS].updateReading = updateGpsReading;
|
||||
altSenFusSources[SF_GPS].updateVariance = updateGpsVariance;
|
||||
altSenFusSources[SF_GPS].updateOffset = updateSensorOffset;
|
||||
altSenFusSources[SF_GPS].currentAltReadingCm = 0;
|
||||
altSenFusSources[SF_GPS].variance = 0.0f;
|
||||
altSenFusSources[SF_GPS].zeroAltOffsetCm = 0.0f;
|
||||
altSenFusSources[SF_GPS].type = SF_GPS;
|
||||
altSenFusSources[SF_GPS].isValid = false;
|
||||
altSenFusSources[SF_GPS].toFuse = true;
|
||||
#endif
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
altSenFusSources[SF_RANGEFINDER].updateReading = updateRangefinderReading;
|
||||
altSenFusSources[SF_RANGEFINDER].updateVariance = updateRangefinderVariance;
|
||||
altSenFusSources[SF_RANGEFINDER].updateOffset = updateRangefinderOffset;
|
||||
altSenFusSources[SF_RANGEFINDER].currentAltReadingCm = 0;
|
||||
altSenFusSources[SF_RANGEFINDER].variance = 0.0f;
|
||||
altSenFusSources[SF_RANGEFINDER].zeroAltOffsetCm = 0.0f;
|
||||
altSenFusSources[SF_RANGEFINDER].updateOffset = updateSensorOffset;
|
||||
altSenFusSources[SF_RANGEFINDER].type = SF_RANGEFINDER;
|
||||
altSenFusSources[SF_RANGEFINDER].isValid = false;
|
||||
altSenFusSources[SF_RANGEFINDER].toFuse = true;
|
||||
#endif
|
||||
|
||||
kf_init(&kf, 0.0f, 1.0f, 1.0f);
|
||||
|
||||
velocityDm3D.value = 0;
|
||||
velocityDm3D.isValid = false;
|
||||
|
||||
velocityCm3D.value = 0;
|
||||
velocityCm3D.isValid = false;
|
||||
}
|
||||
|
||||
void altSensorFusionUpdate(void) {
|
||||
kf_update_variance(&kf);
|
||||
SensorMeasurement tempSensorMeas;
|
||||
float previousAltitude = altitudeState.value;
|
||||
for (altSensor_e sensor = 0; sensor < SENSOR_COUNT; sensor++) {
|
||||
altSenFusSources[sensor].updateReading(&altSenFusSources[sensor]);
|
||||
altSenFusSources[sensor].updateVariance(&altSenFusSources[sensor]);
|
||||
altSenFusSources[sensor].updateOffset(&altSenFusSources[sensor]);
|
||||
float previousAltitude = altitudeState.distCm;
|
||||
for (sensorState_t * sensor = altSenFusSources; sensor < altSenFusSources + SENSOR_COUNT; sensor++) {
|
||||
sensor->updateReading(sensor);
|
||||
updateVelError(sensor);
|
||||
updateSensorFusability(sensor); // this should handle the case of sudden jumps in the sensor readings compared to the accelerometer velocity estimation
|
||||
sensor->updateVariance(sensor);
|
||||
sensor->updateOffset(sensor);
|
||||
|
||||
tempSensorMeas.value = altSenFusSources[sensor].currentAltReadingCm - altSenFusSources[sensor].zeroAltOffsetCm;
|
||||
tempSensorMeas.variance = altSenFusSources[sensor].variance;
|
||||
kf_update(&kf, tempSensorMeas);
|
||||
altitudeState.value = kf.estimatedValue; // update the altitude state every loop since it maybe used in the variance estimate
|
||||
if (sensor->isValid && sensor->toFuse) {
|
||||
tempSensorMeas.value = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
|
||||
tempSensorMeas.variance = sensor->variance;
|
||||
kf_update(&kf, tempSensorMeas);
|
||||
}
|
||||
}
|
||||
altitudeState.variance = kf.estimatedVariance;
|
||||
altitudeState.velocity = (altitudeState.value - previousAltitude) * HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
|
||||
altitudeState.distCm = kf.estimatedValue;
|
||||
altitudeState.variance = kf.estimatedVariance;
|
||||
altitudeState.velocityCm = (altitudeState.distCm - previousAltitude) * TASK_ALTITUDE_RATE_HZ;
|
||||
previousAltitude = altitudeState.distCm;
|
||||
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 4, (int32_t)(altitudeState.value));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 5, (int32_t)(altSenFusSources[SF_BARO].currentAltReadingCm));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 6, (int32_t)(altSenFusSources[SF_GPS].currentAltReadingCm));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 7, (int32_t)(altSenFusSources[SF_RANGEFINDER].currentAltReadingCm));
|
||||
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 0, altSenFusSources[SF_BARO].isValid ? lrintf(altSenFusSources[SF_BARO].currentAltReadingCm) : -1);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(altSenFusSources[SF_GPS].currentAltReadingCm));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, altSenFusSources[SF_RANGEFINDER].isValid ? lrintf(altSenFusSources[SF_RANGEFINDER].currentAltReadingCm) : -1);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, lrintf(altitudeState.distCm));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 4, lrintf(altSenFusSources[SF_BARO].variance));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 5, lrintf(altSenFusSources[SF_GPS].variance));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 6, lrintf(altSenFusSources[SF_RANGEFINDER].variance));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 7, lrintf(altitudeState.variance));
|
||||
}
|
||||
|
||||
// void updateAccItegralCallback(timeUs_t currentTimeUs) {
|
||||
// static bool firstRun = true;
|
||||
// static timeUs_t prevTimeUs = 0;
|
||||
// if (firstRun) {
|
||||
// prevTimeUs = currentTimeUs;
|
||||
// firstRun = false;
|
||||
// return;
|
||||
// }
|
||||
|
||||
// timeDelta_t deltaTimeUs = cmpTimeUs(currentTimeUs, prevTimeUs);
|
||||
// for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
// accIntegral.vel[i] += acc.accADC.v[i] * (float)deltaTimeUs / 1e6f;
|
||||
// }
|
||||
// accIntegral.deltaTimeUs += deltaTimeUs;
|
||||
// prevTimeUs = currentTimeUs;
|
||||
// }
|
||||
void updateSensorOffset(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
// void updateAccReading(sensorState_t *sensor, bool ARMING_FLAG(ARMED)) {
|
||||
// static float velOffset = 0;
|
||||
// static float velAccumlator = 0;
|
||||
// static float velocityDecay = 0.99f;
|
||||
// sensor->previousAltReadingCm = sensor->currentAltReadingCm;
|
||||
// // given the attiude roll and pitch angles of the drone, and the integrated acceleration in x,y and z
|
||||
// // calculate the integrated acceleration in the z direction in the world frame
|
||||
// float roll = DEGREES_TO_RADIANS((float)attitude.values.roll / 10.0f); // integer devision to reduce noise
|
||||
// float pitch = DEGREES_TO_RADIANS((float)attitude.values.pitch / 10.0f);
|
||||
if (!ARMING_FLAG(ARMED)) {// default offset update when not armed
|
||||
sensor->zeroAltOffsetCm = 0.2f * sensor->currentAltReadingCm + 0.8f * sensor->zeroAltOffsetCm;
|
||||
} else { // when armed the offset should be updated according to the velocity error value
|
||||
// float estAlt = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
|
||||
float newOffset = sensor->currentAltReadingCm - kf.estimatedValue;
|
||||
if (sensor->velError > SENSOR_VEL_ERROR_THRESH) {
|
||||
sensor->zeroAltOffsetCm = newOffset;
|
||||
sensor->velError = 0;
|
||||
} else { // update the offset smoothly using the error value, if the error is 0 then no update is done
|
||||
// float correctnessRatio = (float)(SENSOR_VEL_MAX_ERROR - sensor->velError) / SENSOR_VEL_MAX_ERROR;
|
||||
// sensor->zeroAltOffsetCm = (correctnessRatio * sensor->zeroAltOffsetCm)
|
||||
// + ((1.0f - correctnessRatio) * newOffset);
|
||||
sensor->offsetError += newOffset - sensor->zeroAltOffsetCm;
|
||||
if (fabsf(sensor->offsetError) > SENSOR_MAX_OFFSET_ERROR) {
|
||||
sensor->zeroAltOffsetCm = 0.01f * newOffset + 0.99f * sensor->zeroAltOffsetCm;
|
||||
// decaying the error
|
||||
sensor->offsetError = 0.99f * sensor->offsetError;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void updateVelError(sensorState_t *sensor) {
|
||||
if (!sensor->isValid || sensor->type == SF_ACC) {
|
||||
return;
|
||||
}
|
||||
sensor->velError = 0.1 * (sq(altSenFusSources[SF_ACC].velocityCmS - sensor->velocityCmS) / (100.f))
|
||||
+ 0.9 * sensor->velError;
|
||||
|
||||
sensor->velError = constrain(sensor->velError, 0, SENSOR_VEL_MAX_ERROR);
|
||||
}
|
||||
|
||||
void updateSensorFusability(sensorState_t *sensor) {
|
||||
if (!sensor->isValid || sensor->type == SF_ACC) {
|
||||
return;
|
||||
}
|
||||
|
||||
// float cosPitch = cosf(pitch);
|
||||
|
||||
// float velWorldZ = - accIntegral.vel[0] * sinf(pitch)
|
||||
// + accIntegral.vel[1] * cosPitch * sinf(roll)
|
||||
// + accIntegral.vel[2] * cosPitch * cosf(roll);
|
||||
|
||||
// sensor->deltaTimeMs = (float)accIntegral.deltaTimeUs / 1e3f;
|
||||
|
||||
// float gravityVel = (float)accIntegral.deltaTimeUs / 1e6f; // g/Us to g/S
|
||||
if (sensor->velError > SENSOR_VEL_ERROR_THRESH) {
|
||||
sensor->penalityIters = SENSOR_MAX_PENALITY_ITERS;
|
||||
} else if (sensor->penalityIters > 0) {
|
||||
sensor->penalityIters--;
|
||||
}
|
||||
|
||||
// float velCmSec = ((velWorldZ * acc.dev.acc_1G_rec) - gravityVel) * 981.0f; // g to cm/s
|
||||
sensor->toFuse = (sensor->penalityIters == 0);
|
||||
}
|
||||
|
||||
// velCmSec -= velOffset;
|
||||
void doNothing(sensorState_t *sensor) {
|
||||
UNUSED(sensor);
|
||||
}
|
||||
|
||||
// velCmSec = updateAccFilter(velCmSec);
|
||||
// ======================================================================================================
|
||||
// ==================================== Sensor specific functions =======================================
|
||||
// ======================================================================================================
|
||||
void updateAccItegralCallback(timeUs_t currentTimeUs) {
|
||||
static bool firstRun = true;
|
||||
static timeUs_t prevTimeUs = 0;
|
||||
if (firstRun) {
|
||||
prevTimeUs = currentTimeUs;
|
||||
firstRun = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// if (!ARMING_FLAG(ARMED)) {
|
||||
// velOffset = 0.99f * velOffset + 0.01f * velCmSec;
|
||||
// sensor->currentAltReadingCm = 0;
|
||||
// velAccumlator = 0;
|
||||
// }
|
||||
timeDelta_t deltaTimeUs = cmpTimeUs(currentTimeUs, prevTimeUs);
|
||||
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
|
||||
accIntegral.vel[i] += acc.accADC.v[i] * (float)deltaTimeUs / 1e6f;
|
||||
}
|
||||
accIntegral.vel[XYZ_AXIS_COUNT] += (acc.accMagnitude - 1.0f) * (float)deltaTimeUs / 1e6f;
|
||||
accIntegral.deltaTimeUs += deltaTimeUs;
|
||||
prevTimeUs = currentTimeUs;
|
||||
}
|
||||
|
||||
// // use the average velocity and cut the decimals after the first decimal
|
||||
// sensor->VelocityCmSec = (int)((velCmSec) * 100 / 2.0f) / 100.0f;
|
||||
// velAccumlator += sensor->VelocityCmSec;
|
||||
void updateAccReading(sensorState_t *sensor) {
|
||||
static float velDriftZ = 0;
|
||||
|
||||
// sensor->currentAltReadingCm += velAccumlator * ((float)accIntegral.deltaTimeUs/1e6f);
|
||||
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 4, (int32_t)(accIntegral.vel[2] * 1000));
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 5, (int32_t)(sensor->VelocityCmSec * 1000));
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 6, (int32_t)(velAccumlator * 1000));
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 7, (int32_t)(sensor->currentAltReadingCm * 1000));
|
||||
|
||||
// accIntegral.vel[0] = 0;
|
||||
// accIntegral.vel[1] = 0;
|
||||
// accIntegral.vel[2] = 0;
|
||||
// accIntegral.deltaTimeUs = 0;
|
||||
|
||||
// }
|
||||
|
||||
// void updateAccVariance(sensorState_t *sensor, bool ARMING_FLAG(ARMED)) { // maybe we don't need this since the altitude quality is not good anyway
|
||||
// static float unVar = 0;
|
||||
// if (!armed) {
|
||||
// float newVar = ACC_VAR_COEFF * sq(sensor->currentAltReadingCm - sensor->previousAltReadingCm);
|
||||
// unarmedVar = (unarmedVar + newVar)/2;
|
||||
// }
|
||||
// sensor->variance = unarmedVar + ACC_CONST_VAR;
|
||||
// }
|
||||
|
||||
// float updateAccFilter(float value) {
|
||||
// static pt3Filter_t velFilter;
|
||||
// static bool firstRun = true;
|
||||
|
||||
// if (firstRun) {
|
||||
// pt3FilterInit(&velFilter, 0.5);
|
||||
// firstRun = false;
|
||||
// }
|
||||
static float accVelZ = 0;
|
||||
// given the attiude roll and pitch angles of the drone, and the integrated acceleration in x,y and z
|
||||
// calculate the integrated acceleration in the z direction in the world frame
|
||||
float roll = DEGREES_TO_RADIANS((float)attitude.values.roll / 10.0f); // integer devision to reduce noise
|
||||
float pitch = DEGREES_TO_RADIANS((float)attitude.values.pitch / 10.0f);
|
||||
|
||||
// return pt3FilterApply(&velFilter, value);
|
||||
// }
|
||||
float cosPitch = cosf(pitch);
|
||||
|
||||
float velWorldZ = - accIntegral.vel[0] * sinf(pitch)
|
||||
+ accIntegral.vel[1] * cosPitch * sinf(roll)
|
||||
+ accIntegral.vel[2] * cosPitch * cosf(roll);
|
||||
|
||||
// sensor->deltaTimeMs = (float)accIntegral.deltaTimeUs / 1e3f;
|
||||
|
||||
float gravityVel = (float)accIntegral.deltaTimeUs / 1e6f; // g/Us to g/S
|
||||
|
||||
float velCmSecZ = ((velWorldZ * acc.dev.acc_1G_rec) - gravityVel) * 981.0f; // g to cm/s
|
||||
|
||||
velCmSecZ = (int)(velCmSecZ * 10) / 10.0f;
|
||||
|
||||
accVelZ += velCmSecZ;
|
||||
|
||||
velDriftZ = 0.005 * accVelZ + (1.0f - 0.005) * velDriftZ;
|
||||
|
||||
sensor->velocityCmS = accVelZ - velDriftZ;
|
||||
|
||||
velocityCm3D.value = fabsf(accIntegral.vel[XYZ_AXIS_COUNT] * 981.0f);
|
||||
// applyAccVelFilter(&sensor->velocityCmS);
|
||||
// sensor->currentAltReadingCm += sensor->velocityCmS * ((float)accIntegral.deltaTimeUs/1e6f);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 0, lrintf(accIntegral.vel[2] * 981.0f));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(accVelZ));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(velDriftZ));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, lrintf(sensor->velocityCmS));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 4, lrintf(accIntegral.vel[XYZ_AXIS_COUNT] * 981.0f));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 7, lrintf(velocityCm3D.value));
|
||||
|
||||
for (int i = 0; i <= XYZ_AXIS_COUNT; i++) {
|
||||
accIntegral.vel[i] = 0;
|
||||
}
|
||||
accIntegral.deltaTimeUs = 0;
|
||||
|
||||
}
|
||||
|
||||
void applyAccVelFilter(float *velocity) {
|
||||
static pt2Filter_t velFilter;
|
||||
static bool firstRun = true;
|
||||
|
||||
if (firstRun) {
|
||||
pt2FilterInit(&velFilter, 0.5);
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
*velocity = pt2FilterApply(&velFilter, *velocity);
|
||||
}
|
||||
|
||||
|
||||
#ifdef USE_BARO
|
||||
|
|
@ -288,80 +372,153 @@ void updateBaroReading(sensorState_t *sensor) {
|
|||
return;
|
||||
}
|
||||
|
||||
static pt2Filter_t baroLpfFilter;
|
||||
static bool firstRun = true;
|
||||
static timeMs_t prevTime = 0;
|
||||
static float previousAltitude = 0.0f;
|
||||
|
||||
if (firstRun) { // init
|
||||
const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
|
||||
|
||||
const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
|
||||
const float altitudeGain = pt2FilterGain(altitudeCutoffHz, sampleTimeS);
|
||||
pt2FilterInit(&baroLpfFilter, altitudeGain);
|
||||
|
||||
prevTime = millis();
|
||||
previousAltitude = getBaroAltitude();
|
||||
sensor->zeroAltOffsetCm = previousAltitude; // init the offset with the first reading
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
sensor->previousAltReadingCm = sensor->currentAltReadingCm;
|
||||
sensor->currentAltReadingCm = pt2FilterApply(&baroLpfFilter, getBaroAltitude());
|
||||
|
||||
sensor->deltaTimeMs = millis() - prevTime;
|
||||
prevTime = millis();
|
||||
sensor->currentAltReadingCm = getBaroAltitude();
|
||||
applyBaroFilter(&sensor->currentAltReadingCm);
|
||||
sensor->velocityCmS = (sensor->currentAltReadingCm - previousAltitude) * TASK_ALTITUDE_RATE_HZ;
|
||||
previousAltitude = sensor->currentAltReadingCm;
|
||||
}
|
||||
|
||||
void updateBaroVariance(sensorState_t *sensor) {
|
||||
// the baro variance can be calculated as initial variance while the drone is stationary (not armed)
|
||||
// and while armed as a funtion of the altitude, velocity and the baro temprature
|
||||
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
static float contantVariance = 0;
|
||||
static float stationaryVariance = 0;
|
||||
static float stationaryMean = 0;
|
||||
static uint16_t n = 0;
|
||||
|
||||
float velocity = 0;
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
contantVariance = (sensor->variance * n + sq(sensor->currentAltReadingCm)) / (n + 1);
|
||||
n++;
|
||||
if (!ARMING_FLAG(ARMED)) { // approximating the mean and variance of the baro readings during the stationary phase
|
||||
stationaryMean += (sensor->currentAltReadingCm - stationaryMean) / (n + 1);
|
||||
stationaryVariance += (sq(sensor->currentAltReadingCm - stationaryMean) - stationaryVariance) / (n + 1);
|
||||
n++;
|
||||
velocity = 0;
|
||||
} else {
|
||||
sensor->variance = contantVariance
|
||||
+ BARO_VAR_VEL_COEFF * fabsf(altitudeState.velocity) //todo: use the global velocity, but from where?
|
||||
+ BARO_VAR_TEMP_COEFF * fabsf((float)getBaroTemperature())
|
||||
+ BARO_VAR_ALT_COEFF * fabsf(sensor->currentAltReadingCm);
|
||||
velocity = velocityCm3D.isValid ? (float)velocityCm3D.value : (altitudeState.velocityCm);
|
||||
}
|
||||
|
||||
float newVariance = stationaryVariance
|
||||
+ BARO_VAR_VEL_ERROR_COEFF * (float)sensor->velError
|
||||
+ BARO_VAR_VEL_COEFF * fabsf(velocity)
|
||||
+ BARO_VAR_TEMP_COEFF * fabsf((float)getBaroTemperature())
|
||||
+ BARO_VAR_ALT_COEFF * fabsf(sensor->currentAltReadingCm);
|
||||
|
||||
sensor->variance = 0.9f * sensor->variance + 0.1f * newVariance;
|
||||
}
|
||||
|
||||
void applyBaroFilter(float *value) {
|
||||
static pt2Filter_t baroLpfFilter;
|
||||
static bool firstRun = true;
|
||||
|
||||
if (firstRun) {
|
||||
const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
|
||||
const float altitudeGain = pt2FilterGain(altitudeCutoffHz, HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ));
|
||||
pt2FilterInit(&baroLpfFilter, altitudeGain);
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
*value = pt2FilterApply(&baroLpfFilter, *value);
|
||||
}
|
||||
#endif // USE_BARO
|
||||
|
||||
#ifdef USE_GPS
|
||||
void updateGpsReading(sensorState_t *sensor) {
|
||||
static timeMs_t prevTime = 0;
|
||||
sensor->isValid = gpsIsHealthy() && sensors(SENSOR_GPS) && STATE(GPS_FIX);
|
||||
static uint32_t prevTimeStamp = 0;
|
||||
static bool firstRun = true;
|
||||
static float previousAltitude = 0.0f;
|
||||
bool hasNewData = gpsSol.time != prevTimeStamp;
|
||||
|
||||
sensor->isValid = gpsIsHealthy() && sensors(SENSOR_GPS) && STATE(GPS_FIX) && hasNewData;
|
||||
if (!sensor->isValid) {
|
||||
velocityDm3D.isValid = false;
|
||||
#ifndef USE_ACC
|
||||
velocityCm3D.isValid = false;
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
sensor->previousAltReadingCm = sensor->currentAltReadingCm;
|
||||
sensor->currentAltReadingCm = gpsSol.llh.altCm;
|
||||
velocityDm3D.value = gpsSol.speed3d;
|
||||
velocityDm3D.isValid = true;
|
||||
if (firstRun) {
|
||||
previousAltitude = gpsSol.llh.altCm;
|
||||
sensor->zeroAltOffsetCm = previousAltitude;
|
||||
prevTimeStamp = gpsSol.time;
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
sensor->deltaTimeMs = gpsSol.time - prevTime;
|
||||
prevTime = gpsSol.time;
|
||||
sensor->currentAltReadingCm = gpsSol.llh.altCm;
|
||||
applyGpsFilter(&sensor->currentAltReadingCm);
|
||||
|
||||
#ifndef USE_ACC
|
||||
velocityCm3D.value = gpsSol.speed3d * 10;
|
||||
velocityCm3D.isValid = true;
|
||||
#endif
|
||||
timeDelta_t deltaTime = cmpTimeUs(gpsSol.time, prevTimeStamp);
|
||||
sensor->velocityCmS = ((sensor->currentAltReadingCm - previousAltitude) * deltaTime) / 1000.0f;
|
||||
previousAltitude = sensor->currentAltReadingCm;
|
||||
prevTimeStamp = gpsSol.time;
|
||||
}
|
||||
|
||||
void updateGpsVariance(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
float newVariance = GPS_VAR_VEL_ERROR_COEFF * sensor->velError;
|
||||
|
||||
if (gpsSol.dop.vdop != 0) {
|
||||
sensor->variance = GPS_VAR_VDOP_COEFF * gpsSol.dop.vdop;
|
||||
newVariance += GPS_VAR_DOP_COEFF * gpsSol.dop.vdop;
|
||||
} else if (gpsSol.dop.pdop != 0) {
|
||||
newVariance += GPS_VAR_DOP_COEFF * gpsSol.dop.pdop;
|
||||
} else {
|
||||
sensor->variance = 1000.0f;
|
||||
}
|
||||
newVariance += 10000.0f;
|
||||
}
|
||||
|
||||
sensor->variance = 0.9f * sensor->variance + 0.1f * newVariance;
|
||||
}
|
||||
|
||||
void updateGpsOffset(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) { // default offset update when not armed
|
||||
updateSensorOffset(sensor);
|
||||
} else {
|
||||
// the offset calculated when not armed is not accurate anymore, in case of getting more satalites, the offset should be updated.
|
||||
static float accError = 0;
|
||||
float estAlt = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
|
||||
float error = (estAlt - kf.estimatedValue);
|
||||
|
||||
if (fabsf(error) > GPS_RAPID_ERR_THRESH) { // if the error is too high, update the offset directly
|
||||
sensor->zeroAltOffsetCm = sensor->zeroAltOffsetCm + error;
|
||||
return;
|
||||
} else { // otherwise, add the error to the accumlated error and smoothly update the offset if the error is high enough
|
||||
accError += error;
|
||||
if (accError > GPS_ACC_ERROR_THRESH) {
|
||||
sensor->zeroAltOffsetCm = 0.99f * sensor->zeroAltOffsetCm + 0.01f * (estAlt - kf.estimatedValue);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void applyGpsFilter(float *value) {
|
||||
static pt2Filter_t filter;
|
||||
static bool firstRun = true;
|
||||
|
||||
if (firstRun) {
|
||||
pt2FilterInit(&filter, 0.5);
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
*value = pt2FilterApply(&filter, *value);
|
||||
}
|
||||
|
||||
#endif // USE_GPS
|
||||
|
|
@ -369,10 +526,10 @@ void updateGpsVariance(sensorState_t *sensor) {
|
|||
#ifdef USE_RANGEFINDER
|
||||
|
||||
void updateRangefinderReading(sensorState_t *sensor) {
|
||||
static timeMs_t prevTime = 0;
|
||||
static bool firstRun = true;
|
||||
static float previousAltitude = 0.0f;
|
||||
int32_t rfAlt = rangefinderGetLatestAltitude();
|
||||
|
||||
|
||||
sensor->isValid = rangefinderIsHealthy() && sensors(SENSOR_RANGEFINDER) && rfAlt >= 0;
|
||||
|
||||
if (!sensor->isValid) {
|
||||
|
|
@ -380,14 +537,15 @@ void updateRangefinderReading(sensorState_t *sensor) {
|
|||
}
|
||||
|
||||
if (firstRun) {
|
||||
prevTime = millis();
|
||||
previousAltitude = rfAlt;
|
||||
sensor->zeroAltOffsetCm = rfAlt;
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
sensor->previousAltReadingCm = sensor->currentAltReadingCm;
|
||||
sensor->currentAltReadingCm = rfAlt;
|
||||
sensor->deltaTimeMs = millis() - prevTime;
|
||||
prevTime = millis();
|
||||
sensor->currentAltReadingCm = rfAlt;
|
||||
// applyRangefinderFilter(&sensor->currentAltReadingCm);
|
||||
sensor->velocityCmS = (sensor->currentAltReadingCm - previousAltitude) * TASK_ALTITUDE_RATE_HZ;
|
||||
previousAltitude = sensor->currentAltReadingCm;
|
||||
}
|
||||
|
||||
void updateRangefinderVariance(sensorState_t *sensor) {
|
||||
|
|
@ -395,7 +553,8 @@ void updateRangefinderVariance(sensorState_t *sensor) {
|
|||
return;
|
||||
}
|
||||
|
||||
sensor->variance = 1.0f; // is there a better way to get the variance of the rangefinder?
|
||||
float newVariance = RANGEFINDER_VAR_VEL_ERROR_COEFF * sensor->velError + RANGEFINDER_CONST_VAR; // is there a better way to get the variance of the rangefinder?
|
||||
sensor->variance = 0.9f * sensor->variance + 0.1f * newVariance;
|
||||
}
|
||||
|
||||
void updateRangefinderOffset(sensorState_t *sensor) {
|
||||
|
|
@ -404,30 +563,33 @@ void updateRangefinderOffset(sensorState_t *sensor) {
|
|||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) { // default offset update when not armed
|
||||
updateSensorOffset(sensor);
|
||||
} else { // when armed the offset should be updated using the altitude state
|
||||
static float accError = 0;
|
||||
float zeroedAlt = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
|
||||
float error = (zeroedAlt - altitudeState.value);
|
||||
sensor->zeroAltOffsetCm = 0.2f * sensor->currentAltReadingCm + 0.8f * sensor->zeroAltOffsetCm;
|
||||
} else { // when armed the offset should be updated according to the velocity error value
|
||||
float estAlt = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
|
||||
float error = (estAlt - kf.estimatedValue);
|
||||
if (sensor->velError > SENSOR_VEL_ERROR_THRESH) {
|
||||
sensor->zeroAltOffsetCm += error;
|
||||
} else { // update the offset smoothly using the error value, if the error is 0 then no update is done
|
||||
// this is not effective, we should find a way to fix the position drift of the rangefinder or gps
|
||||
|
||||
if (fabsf(error) > RANGEFINDER_RAPID_ERR_THRESH) { // if the error is too high, update the offset directly
|
||||
sensor->zeroAltOffsetCm = sensor->zeroAltOffsetCm + error;
|
||||
return;
|
||||
} else { // otherwise, add the error to the accumlated error and smoothly update the offset if the error is high enough
|
||||
accError += error;
|
||||
if (accError > RANGEFINDER_ACC_ERROR_THRESH) {
|
||||
sensor->zeroAltOffsetCm = 0.99f * sensor->zeroAltOffsetCm + 0.01f * (sensor->currentAltReadingCm - altitudeState.value);
|
||||
}
|
||||
float correctnessRatio = (float)(SENSOR_VEL_MAX_ERROR - sensor->velError) / SENSOR_VEL_MAX_ERROR;
|
||||
sensor->zeroAltOffsetCm = (correctnessRatio * sensor->zeroAltOffsetCm)
|
||||
+ ((1.0f - correctnessRatio) * (estAlt - kf.estimatedValue));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void applyRangefinderFilter(float *value) {
|
||||
static pt2Filter_t filter;
|
||||
static bool firstRun = true;
|
||||
|
||||
if (firstRun) {
|
||||
pt2FilterInit(&filter, 0.5);
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
*value = pt2FilterApply(&filter, *value);
|
||||
}
|
||||
#endif // USE_RANGEFINDER
|
||||
|
||||
void updateSensorOffset(sensorState_t *sensor) {
|
||||
if (!sensor->isValid || ARMING_FLAG(ARMED)) {
|
||||
return;
|
||||
}
|
||||
sensor->zeroAltOffsetCm = 0.2f * sensor->currentAltReadingCm + 0.8f * sensor->zeroAltOffsetCm;
|
||||
}
|
||||
|
||||
#endif // USE_BARO || USE_GPS || USE_RANGEFINDER
|
||||
|
|
@ -18,9 +18,9 @@
|
|||
#pragma once
|
||||
|
||||
typedef struct altitudeState_s {
|
||||
float value;
|
||||
float distCm;
|
||||
float variance;
|
||||
float velocity;
|
||||
float velocityCm;
|
||||
} altitudeState_t;
|
||||
|
||||
#ifdef USE_ACC
|
||||
|
|
|
|||
|
|
@ -138,7 +138,7 @@ void calculateEstimatedAltitude(void) {
|
|||
#endif
|
||||
|
||||
if(!kfInitDone) { //first kf init
|
||||
kf_init(&kf, 0.0f, 0.5, 0.5); //to do: init variance
|
||||
kf_init(&kf, 1.0f, 0.5, 0.5); //to do: init variance
|
||||
kfInitDone = true;
|
||||
}
|
||||
|
||||
|
|
@ -161,7 +161,7 @@ void calculateEstimatedAltitude(void) {
|
|||
}
|
||||
zeroedGpsAltitudeCm = 0.0f; // always hold relativeAltitude at zero while disarmed
|
||||
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm / 100.0f); // Absolute altitude ASL in metres, max 32,767m
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm / 100.0f); // Absolute altitude ASL in metres, max 32,767m
|
||||
// *** ARMED ***
|
||||
} else {
|
||||
if (!wasArmed) { // things to run once, on arming, after being disarmed
|
||||
|
|
@ -183,7 +183,7 @@ void calculateEstimatedAltitude(void) {
|
|||
gpsTrust = 0.0f;
|
||||
// TO DO - smoothly reduce GPS trust, rather than immediately dropping to zero for what could be only a very brief loss of 3D fix
|
||||
}
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedGpsAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedGpsAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
|
||||
|
||||
// apply Kalman filter on available sensors
|
||||
kf_update_variance(&kf);
|
||||
|
|
@ -236,12 +236,12 @@ void calculateEstimatedAltitude(void) {
|
|||
#endif
|
||||
|
||||
// *** set debugs
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(baroAltCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(baroAltCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
|
||||
#ifdef USE_VARIO
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
|
||||
// DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
|
||||
// DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedFusedAltitudeCm));
|
||||
|
||||
altitudeAvailable = haveGpsAlt || haveBaroAlt || haveRangefinderAlt;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue