mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
Merge ef8e085f39 into acbab53d13
This commit is contained in:
Ahmed Mansour
GitHub
commit
462d1137ea
17 changed files with 789 additions and 149 deletions
|
|
@ -160,6 +160,8 @@ COMMON_SRC = \
|
|||
flight/alt_hold_wing.c \
|
||||
flight/autopilot_multirotor.c \
|
||||
flight/autopilot_wing.c \
|
||||
flight/kalman_filter_1d.c \
|
||||
flight/altitude.c \
|
||||
flight/dyn_notch_filter.c \
|
||||
flight/failsafe.c \
|
||||
flight/gps_rescue_multirotor.c \
|
||||
|
|
|
|||
|
|
@ -165,12 +165,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);
|
||||
|
|
@ -198,6 +209,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);
|
||||
|
|
|
|||
|
|
@ -60,12 +60,12 @@
|
|||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/pos_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"
|
||||
|
|
@ -159,6 +159,7 @@ static void taskBatteryAlerts(timeUs_t currentTimeUs)
|
|||
static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
|
||||
{
|
||||
accUpdate(currentTimeUs);
|
||||
updateAccItegralCallback(currentTimeUs);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -262,6 +263,7 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
|
|||
if (newDeadline != 0) {
|
||||
rescheduleTask(TASK_SELF, newDeadline);
|
||||
}
|
||||
updateBaroStateCallback();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -475,9 +477,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)
|
||||
|
|
@ -667,8 +666,4 @@ void tasksInit(void)
|
|||
#ifdef USE_RC_STATS
|
||||
setTaskEnabled(TASK_RC_STATS, true);
|
||||
#endif
|
||||
|
||||
#ifdef USE_GIMBAL
|
||||
setTaskEnabled(TASK_GIMBAL, true);
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
589
src/main/flight/altitude.c
Normal file
589
src/main/flight/altitude.c
Normal file
|
|
@ -0,0 +1,589 @@
|
|||
/*
|
||||
* This file is part of Betaflight.
|
||||
*
|
||||
* Betaflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Betaflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <limits.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/time.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
#include "flight/imu.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/kalman_filter_1d.h"
|
||||
#include "flight/altitude.h"
|
||||
|
||||
|
||||
#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 10
|
||||
#define SENSOR_MAX_OFFSET_ERROR 1000.0f
|
||||
|
||||
#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 BARO_TASK_INTERVAL_CALC_ITER 10
|
||||
|
||||
#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
|
||||
|
||||
#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
|
||||
#define GPS_PDOP_MIN_THRESHOLD 400.0f
|
||||
|
||||
typedef enum {
|
||||
#ifdef USE_ACC
|
||||
SF_ACC,
|
||||
#endif
|
||||
#ifdef USE_BARO
|
||||
SF_BARO,
|
||||
#endif
|
||||
#ifdef USE_GPS
|
||||
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_RANGEFINDER
|
||||
SF_RANGEFINDER,
|
||||
#endif
|
||||
SENSOR_COUNT
|
||||
} altSensor_e;
|
||||
|
||||
typedef struct sensorState_s {
|
||||
float currentAltReadingCm;
|
||||
float zeroAltOffsetCm;
|
||||
float velocityAltCmS;
|
||||
float variance;
|
||||
uint32_t deltaTimeMs;
|
||||
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 updateBaroOffset(sensorState_t *sensor);
|
||||
void applyBaroFilter(float *dst, float newValue);
|
||||
#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);
|
||||
#endif
|
||||
|
||||
void updateSensorOffset(sensorState_t *sensor);
|
||||
void updateVelError(sensorState_t *sensor);
|
||||
void updateSensorFusability(sensorState_t *sensor);
|
||||
bool sensorUpdateIteration(sensorState_t *sensor);
|
||||
void doNothing(sensorState_t *sensor);
|
||||
|
||||
static sensorState_t altSenFusSources[SENSOR_COUNT];
|
||||
static KalmanFilter kf;
|
||||
static velocity3D_t velocity3DCmS;
|
||||
static altitudeState_t altitudeState;
|
||||
|
||||
void altSensorFusionInit(void) {
|
||||
velocity3DCmS.value = 0;
|
||||
velocity3DCmS.isValid = false;
|
||||
|
||||
#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;
|
||||
velocity3DCmS.isValid = true;
|
||||
#endif
|
||||
|
||||
#ifdef USE_BARO
|
||||
altSenFusSources[SF_BARO].updateReading = updateBaroReading;
|
||||
altSenFusSources[SF_BARO].updateVariance = updateBaroVariance;
|
||||
altSenFusSources[SF_BARO].updateOffset = updateBaroOffset;
|
||||
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].type = SF_GPS;
|
||||
altSenFusSources[SF_GPS].isValid = false;
|
||||
altSenFusSources[SF_GPS].toFuse = true;
|
||||
velocity3DCmS.isValid = true;
|
||||
#endif
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
altSenFusSources[SF_RANGEFINDER].updateReading = updateRangefinderReading;
|
||||
altSenFusSources[SF_RANGEFINDER].updateVariance = updateRangefinderVariance;
|
||||
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, 10.0f);
|
||||
}
|
||||
|
||||
bool sensorUpdateIteration(sensorState_t *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);
|
||||
|
||||
if (sensor->isValid && sensor->toFuse) {
|
||||
SensorMeasurement tempSensorMeas;
|
||||
tempSensorMeas.value = sensor->currentAltReadingCm - sensor->zeroAltOffsetCm;
|
||||
tempSensorMeas.variance = sensor->variance;
|
||||
#ifdef USE_GPS
|
||||
if (sensor->type == SF_GPS) { // ignore the GPS for now, TODO: add the gps to the fusion
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
kf_update(&kf, tempSensorMeas);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool altSensorFusionUpdate(void) {
|
||||
static timeMs_t prevTimeMs = 0;
|
||||
timeMs_t deltaTimeMs = millis() - prevTimeMs;
|
||||
prevTimeMs = millis();
|
||||
|
||||
kf_update_variance(&kf);
|
||||
float previousAltitude = altitudeState.distCm;
|
||||
bool haveAltitude = false;
|
||||
for (sensorState_t * sensor = altSenFusSources; sensor < altSenFusSources + SENSOR_COUNT; sensor++) {
|
||||
haveAltitude |= sensorUpdateIteration(sensor);
|
||||
}
|
||||
|
||||
altitudeState.distCm = kf.estimatedValue;
|
||||
altitudeState.variance = kf.estimatedVariance;
|
||||
altitudeState.velocityCm = (altitudeState.distCm - previousAltitude) * 1000 / deltaTimeMs;
|
||||
previousAltitude = altitudeState.distCm;
|
||||
|
||||
#ifdef USE_ACC
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, lrintf(altSenFusSources[SF_ACC].velocityAltCmS));
|
||||
#endif
|
||||
|
||||
#ifdef USE_BARO
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(altSenFusSources[SF_BARO].currentAltReadingCm - altSenFusSources[SF_BARO].zeroAltOffsetCm));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 5, lrintf(altSenFusSources[SF_BARO].zeroAltOffsetCm));
|
||||
#endif
|
||||
|
||||
#ifdef USE_GPS
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 0, lrintf(10000 - gpsSol.dop.pdop));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(altSenFusSources[SF_GPS].currentAltReadingCm - altSenFusSources[SF_GPS].zeroAltOffsetCm));
|
||||
#endif
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 6, lrintf(altSenFusSources[SF_RANGEFINDER].zeroAltOffsetCm));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 4, lrintf(altSenFusSources[SF_RANGEFINDER].currentAltReadingCm - altSenFusSources[SF_RANGEFINDER].zeroAltOffsetCm));
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 7, lrintf(altitudeState.distCm));
|
||||
|
||||
|
||||
return haveAltitude;
|
||||
}
|
||||
|
||||
void updateSensorOffset(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
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 newOffset = sensor->currentAltReadingCm - kf.estimatedValue;
|
||||
if (sensor->penalityIters > 0) {
|
||||
sensor->zeroAltOffsetCm = 0.5f * (newOffset + sensor->zeroAltOffsetCm);
|
||||
} else { // detect a ramp in the sensor readings by accumulating the error
|
||||
sensor->offsetError += newOffset - sensor->zeroAltOffsetCm;
|
||||
if (fabsf(sensor->offsetError) > SENSOR_MAX_OFFSET_ERROR) {
|
||||
sensor->zeroAltOffsetCm = 0.01f * newOffset + 0.99f * sensor->zeroAltOffsetCm;
|
||||
sensor->offsetError = 0.99f * sensor->offsetError; // decaying the error
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void updateVelError(sensorState_t *sensor) {
|
||||
if (!sensor->isValid || sensor->type == SF_ACC) {
|
||||
return;
|
||||
}
|
||||
sensor->velError = 0.1f * (sq(altSenFusSources[SF_ACC].velocityAltCmS - sensor->velocityAltCmS) / (100.f))
|
||||
+ 0.9f * 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;
|
||||
}
|
||||
|
||||
if (sensor->velError > SENSOR_VEL_ERROR_THRESH) {
|
||||
sensor->penalityIters = SENSOR_MAX_PENALITY_ITERS;
|
||||
} else if (sensor->penalityIters > 0) {
|
||||
sensor->penalityIters--;
|
||||
}
|
||||
|
||||
sensor->toFuse = (sensor->penalityIters == 0);
|
||||
}
|
||||
|
||||
altitudeState_t *getAltitudeState(void) {
|
||||
return &altitudeState;
|
||||
}
|
||||
|
||||
void doNothing(sensorState_t *sensor) {
|
||||
UNUSED(sensor);
|
||||
}
|
||||
|
||||
// ======================================================================================================
|
||||
// ==================================== Sensor specific functions =======================================
|
||||
// ======================================================================================================
|
||||
void updateAccItegralCallback(timeUs_t currentTimeUs) { // this is called in the acc update task
|
||||
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.vel[XYZ_AXIS_COUNT] += (acc.accMagnitude - 1.0f) * (float)deltaTimeUs / 1e6f;
|
||||
accIntegral.deltaTimeUs += deltaTimeUs;
|
||||
prevTimeUs = currentTimeUs;
|
||||
}
|
||||
|
||||
void updateAccReading(sensorState_t *sensor) {
|
||||
static float velDriftZ = 0.0f;
|
||||
|
||||
static float accVelZ = 0.0f;
|
||||
// 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);
|
||||
|
||||
float cosPitch = cosf(pitch);
|
||||
|
||||
float velWorldZ = - accIntegral.vel[0] * sinf(pitch)
|
||||
+ accIntegral.vel[1] * cosPitch * sinf(roll)
|
||||
+ accIntegral.vel[2] * cosPitch * cosf(roll);
|
||||
|
||||
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.005f * accVelZ + (1.0f - 0.005f) * velDriftZ;
|
||||
|
||||
sensor->velocityAltCmS = accVelZ - velDriftZ;
|
||||
|
||||
velocity3DCmS.value = (0.1f * fabsf(accIntegral.vel[XYZ_AXIS_COUNT] * 981.0f)) + (0.9f * velocity3DCmS.value);
|
||||
// applyAccVelFilter(&sensor->velocityAltCmS);
|
||||
// sensor->currentAltReadingCm += sensor->velocityAltCmS * ((float)accIntegral.deltaTimeUs/1e6f);
|
||||
|
||||
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
|
||||
void updateBaroStateCallback(void) {
|
||||
applyBaroFilter(&altSenFusSources[SF_BARO].currentAltReadingCm, getBaroAltitude());
|
||||
}
|
||||
|
||||
void updateBaroReading(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
static bool firstRun = true;
|
||||
static float previousAltitude = 0.0f;
|
||||
static uint32_t prevTimeMs = 0;
|
||||
|
||||
if (firstRun) { // init
|
||||
previousAltitude = sensor->currentAltReadingCm;
|
||||
sensor->zeroAltOffsetCm = sensor->currentAltReadingCm; // init the offset with the first reading
|
||||
firstRun = false;
|
||||
prevTimeMs = millis();
|
||||
}
|
||||
|
||||
sensor->deltaTimeMs = millis() - prevTimeMs;
|
||||
|
||||
sensor->velocityAltCmS = (sensor->currentAltReadingCm - previousAltitude) * 1000 / sensor->deltaTimeMs;
|
||||
previousAltitude = sensor->currentAltReadingCm;
|
||||
|
||||
prevTimeMs = millis();
|
||||
}
|
||||
|
||||
void updateBaroVariance(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
static float stationaryVariance = 0;
|
||||
static float stationaryMean = 0;
|
||||
static uint16_t n = 0;
|
||||
|
||||
float velocity = 0;
|
||||
|
||||
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 {
|
||||
velocity = velocity3DCmS.isValid ? (float)velocity3DCmS.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 updateBaroOffset(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) { // default offset update when not armed
|
||||
sensor->zeroAltOffsetCm = 0.2f * sensor->currentAltReadingCm + 0.8f * sensor->zeroAltOffsetCm;
|
||||
}
|
||||
}
|
||||
|
||||
void applyBaroFilter(float *dst, float newValue) {
|
||||
static pt2Filter_t baroLpfFilter;
|
||||
static bool firstRun = true;
|
||||
// calculate the task interval for the first few iterations in ms (can this be done better ? from the baro dev ?)
|
||||
static int8_t taskIntervalIter = BARO_TASK_INTERVAL_CALC_ITER;
|
||||
static uint16_t taskInterval = 0;
|
||||
if (taskIntervalIter > 0) {
|
||||
static bool firstIter = true;
|
||||
static uint16_t prevTimeMs = 0;
|
||||
if (firstIter) {
|
||||
prevTimeMs = millis();
|
||||
firstIter = false;
|
||||
return;
|
||||
}
|
||||
taskInterval += millis() - prevTimeMs;
|
||||
taskIntervalIter--;
|
||||
prevTimeMs = millis();
|
||||
if (taskIntervalIter == 0) {
|
||||
taskInterval /= BARO_TASK_INTERVAL_CALC_ITER;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (firstRun) {
|
||||
const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
|
||||
const float altitudeGain = pt2FilterGain(altitudeCutoffHz, taskInterval);
|
||||
pt2FilterInit(&baroLpfFilter, altitudeGain);
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
*dst = pt2FilterApply(&baroLpfFilter, newValue);
|
||||
}
|
||||
#endif // USE_BARO
|
||||
|
||||
#ifdef USE_GPS
|
||||
void updateGpsReading(sensorState_t *sensor) {
|
||||
static uint32_t prevTimeStamp = 0;
|
||||
static bool firstRun = true;
|
||||
static float previousAltitude = 0.0f;
|
||||
bool hasNewData = gpsSol.time != prevTimeStamp;
|
||||
bool dopIsGood = (gpsSol.dop.pdop > 0 && gpsSol.dop.pdop < GPS_PDOP_MIN_THRESHOLD)
|
||||
|| (gpsSol.dop.hdop > 0 && gpsSol.dop.hdop < GPS_PDOP_MIN_THRESHOLD);
|
||||
|
||||
sensor->isValid = gpsIsHealthy()
|
||||
&& sensors(SENSOR_GPS)
|
||||
&& STATE(GPS_FIX)
|
||||
&& hasNewData
|
||||
&& dopIsGood;
|
||||
|
||||
if (!sensor->isValid) {
|
||||
#ifndef USE_ACC
|
||||
velocity3DCmS.isValid = false;
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
|
||||
if (firstRun) {
|
||||
previousAltitude = gpsSol.llh.altCm;
|
||||
sensor->zeroAltOffsetCm = previousAltitude;
|
||||
prevTimeStamp = gpsSol.time;
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
sensor->currentAltReadingCm = gpsSol.llh.altCm;
|
||||
|
||||
#ifndef USE_ACC
|
||||
velocity3DCmS.value = gpsSol.speed3d * 10;
|
||||
velocity3DCmS.isValid = true;
|
||||
#endif
|
||||
sensor->deltaTimeMs = gpsSol.time - prevTimeStamp;
|
||||
sensor->velocityAltCmS = ((sensor->currentAltReadingCm - previousAltitude) * 1000.0f) / sensor->deltaTimeMs;
|
||||
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) {
|
||||
newVariance += GPS_VAR_DOP_COEFF * gpsSol.dop.vdop;
|
||||
} else if (gpsSol.dop.pdop != 0) {
|
||||
newVariance += GPS_VAR_DOP_COEFF * gpsSol.dop.pdop;
|
||||
} else {
|
||||
newVariance += 10000.0f;
|
||||
}
|
||||
|
||||
sensor->variance = newVariance;
|
||||
}
|
||||
|
||||
#endif // USE_GPS
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
|
||||
void updateRangefinderReading(sensorState_t *sensor) {
|
||||
static bool firstRun = true;
|
||||
static float previousAltitude = 0.0f;
|
||||
static float prevReadingTime = 0;
|
||||
int32_t rfAlt = getRangefinder()->calculatedAltitude;
|
||||
bool hasNewData = getRangefinder()->lastValidResponseTimeMs != prevReadingTime;
|
||||
|
||||
sensor->isValid = rangefinderIsHealthy()
|
||||
&& sensors(SENSOR_RANGEFINDER)
|
||||
&& rfAlt >= 0
|
||||
&& hasNewData;
|
||||
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (firstRun) {
|
||||
previousAltitude = rfAlt;
|
||||
firstRun = false;
|
||||
prevReadingTime = getRangefinder()->lastValidResponseTimeMs;
|
||||
}
|
||||
|
||||
sensor->deltaTimeMs = getRangefinder()->lastValidResponseTimeMs - prevReadingTime;
|
||||
sensor->currentAltReadingCm = rfAlt;
|
||||
sensor->velocityAltCmS = (sensor->currentAltReadingCm - previousAltitude) * 1000.0f / sensor->deltaTimeMs;
|
||||
previousAltitude = sensor->currentAltReadingCm;
|
||||
prevReadingTime = getRangefinder()->lastValidResponseTimeMs;
|
||||
}
|
||||
|
||||
void updateRangefinderVariance(sensorState_t *sensor) {
|
||||
if (!sensor->isValid) {
|
||||
return;
|
||||
}
|
||||
|
||||
float newVariance = RANGEFINDER_VAR_VEL_ERROR_COEFF * sensor->velError + RANGEFINDER_CONST_VAR;
|
||||
sensor->variance = 0.9f * sensor->variance + 0.1f * newVariance;
|
||||
}
|
||||
|
||||
#endif // USE_RANGEFINDER
|
||||
32
src/main/flight/altitude.h
Normal file
32
src/main/flight/altitude.h
Normal file
|
|
@ -0,0 +1,32 @@
|
|||
/*
|
||||
* This file is part of Betaflight.
|
||||
*
|
||||
* Betaflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Betaflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
typedef struct altitudeState_s {
|
||||
float distCm;
|
||||
float variance;
|
||||
float velocityCm;
|
||||
} altitudeState_t;
|
||||
|
||||
#ifdef USE_ACC
|
||||
void updateAccItegralCallback(timeUs_t currentTimeUs);
|
||||
#endif
|
||||
void altSensorFusionInit(void);
|
||||
bool altSensorFusionUpdate(void);
|
||||
void updateBaroStateCallback(void);
|
||||
altitudeState_t * getAltitudeState(void);
|
||||
42
src/main/flight/kalman_filter_1d.c
Normal file
42
src/main/flight/kalman_filter_1d.c
Normal file
|
|
@ -0,0 +1,42 @@
|
|||
/*
|
||||
* This file is part of Betaflight.
|
||||
*
|
||||
* Betaflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Betaflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "platform.h"
|
||||
#include "math.h"
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "kalman_filter_1d.h"
|
||||
|
||||
|
||||
void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance) {
|
||||
kf->estimatedValue = initialValue;
|
||||
kf->estimatedVariance = initialVariance;
|
||||
kf->processVariance = processVariance;
|
||||
}
|
||||
|
||||
void kf_update_variance(KalmanFilter *kf) {
|
||||
// Increase the estimated variance by the process variance
|
||||
kf->estimatedVariance += kf->processVariance;
|
||||
}
|
||||
|
||||
void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas) {
|
||||
float kalmanGain = kf->estimatedVariance / (kf->estimatedVariance + sensorMeas.variance);
|
||||
|
||||
kf->estimatedValue += kalmanGain * (sensorMeas.value - kf->estimatedValue);
|
||||
|
||||
kf->estimatedVariance = (1 - kalmanGain) * kf->estimatedVariance;
|
||||
}
|
||||
36
src/main/flight/kalman_filter_1d.h
Normal file
36
src/main/flight/kalman_filter_1d.h
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
/*
|
||||
* This file is part of Betaflight.
|
||||
*
|
||||
* Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
typedef struct {
|
||||
float estimatedValue;
|
||||
float estimatedVariance;
|
||||
float processVariance;
|
||||
} KalmanFilter;
|
||||
|
||||
typedef struct {
|
||||
float value;
|
||||
float variance;
|
||||
} SensorMeasurement;
|
||||
|
||||
void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance);
|
||||
void kf_update_variance(KalmanFilter *kf);
|
||||
void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas);
|
||||
|
|
@ -36,6 +36,8 @@
|
|||
#include "flight/position.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/kalman_filter_1d.h"
|
||||
#include "flight/altitude.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
|
|
@ -43,34 +45,24 @@
|
|||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
|
||||
static float displayAltitudeCm = 0.0f;
|
||||
static bool altitudeAvailable = false;
|
||||
|
||||
static float zeroedAltitudeCm = 0.0f;
|
||||
static float zeroedAltitudeDerivative = 0.0f;
|
||||
|
||||
static pt2Filter_t altitudeLpf;
|
||||
static pt2Filter_t altitudeDerivativeLpf;
|
||||
|
||||
#ifdef USE_VARIO
|
||||
static int16_t estimatedVario = 0; // in cm/s
|
||||
#endif
|
||||
|
||||
void positionInit(void)
|
||||
{
|
||||
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(&altitudeLpf, altitudeGain);
|
||||
|
||||
const float altitudeDerivativeCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
|
||||
const float altitudeDerivativeGain = pt2FilterGain(altitudeDerivativeCutoffHz, sampleTimeS);
|
||||
pt2FilterInit(&altitudeDerivativeLpf, altitudeDerivativeGain);
|
||||
void positionInit(void) {
|
||||
altSensorFusionInit();
|
||||
}
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -88,134 +80,20 @@ PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
|
|||
.altitude_d_lpf = 100,
|
||||
);
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
void calculateEstimatedAltitude(void)
|
||||
{
|
||||
static bool wasArmed = false;
|
||||
static bool useZeroedGpsAltitude = false; // whether a zero for the GPS altitude value exists
|
||||
static float gpsAltCm = 0.0f; // will hold last value on transient loss of 3D fix
|
||||
static float gpsAltOffsetCm = 0.0f;
|
||||
static float baroAltOffsetCm = 0.0f;
|
||||
static float newBaroAltOffsetCm = 0.0f;
|
||||
|
||||
float baroAltCm = 0.0f;
|
||||
float gpsTrust = 0.3f; // if no pDOP value, use 0.3, intended range 0-1;
|
||||
bool haveBaroAlt = false; // true if baro exists and has been calibrated on power up
|
||||
bool haveGpsAlt = false; // true if GPS is connected and while it has a 3D fix, set each run to false
|
||||
|
||||
// *** Get sensor data
|
||||
#ifdef USE_BARO
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
baroAltCm = getBaroAltitude();
|
||||
haveBaroAlt = true; // false only if there is no sensor on the board, or it has failed
|
||||
}
|
||||
#endif
|
||||
#ifdef USE_GPS
|
||||
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
|
||||
// GPS_FIX means a 3D fix, which requires min 4 sats.
|
||||
// On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero.
|
||||
gpsAltCm = gpsSol.llh.altCm; // static, so hold last altitude value if 3D fix is lost to prevent fly to moon
|
||||
haveGpsAlt = true; // goes false and stays false if no 3D fix
|
||||
if (gpsSol.dop.pdop != 0) {
|
||||
// pDOP of 1.0 is good. 100 is very bad. Our gpsSol.dop.pdop values are *100
|
||||
// When pDOP is a value less than 3.3, GPS trust will be stronger than default.
|
||||
gpsTrust = 100.0f / gpsSol.dop.pdop;
|
||||
// *** TO DO - investigate if we should use vDOP or vACC with UBlox units;
|
||||
}
|
||||
// always use at least 10% of other sources besides gps if available
|
||||
gpsTrust = MIN(gpsTrust, 0.9f);
|
||||
}
|
||||
#endif
|
||||
|
||||
// *** DISARMED ***
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (wasArmed) { // things to run once, on disarming, after being armed
|
||||
useZeroedGpsAltitude = false; // reset, and wait for valid GPS data to zero the GPS signal
|
||||
wasArmed = false;
|
||||
}
|
||||
|
||||
newBaroAltOffsetCm = 0.2f * baroAltCm + 0.8f * newBaroAltOffsetCm; // smooth some recent baro samples
|
||||
displayAltitudeCm = baroAltCm - baroAltOffsetCm; // if no GPS altitude, show un-smoothed Baro altitude in OSD and sensors tab, using most recent offset.
|
||||
|
||||
if (haveGpsAlt) { // watch for valid GPS altitude data to get a zero value from
|
||||
gpsAltOffsetCm = gpsAltCm; // update the zero offset value with the most recent valid gps altitude reading
|
||||
useZeroedGpsAltitude = true; // we can use this offset to zero the GPS altitude on arming
|
||||
if (!(positionConfig()->altitude_source == BARO_ONLY)) {
|
||||
displayAltitudeCm = gpsAltCm; // estimatedAltitude shows most recent ASL GPS altitude in OSD and sensors, while disarmed
|
||||
}
|
||||
}
|
||||
zeroedAltitudeCm = 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
|
||||
// *** ARMED ***
|
||||
} else {
|
||||
if (!wasArmed) { // things to run once, on arming, after being disarmed
|
||||
baroAltOffsetCm = newBaroAltOffsetCm;
|
||||
wasArmed = true;
|
||||
}
|
||||
|
||||
baroAltCm -= baroAltOffsetCm; // use smoothed baro with most recent zero from disarm period
|
||||
|
||||
if (haveGpsAlt) { // update relativeAltitude with every new gpsAlt value, or hold the previous value until 3D lock recovers
|
||||
if (!useZeroedGpsAltitude && haveBaroAlt) { // armed without zero offset, can use baro values to zero later
|
||||
gpsAltOffsetCm = gpsAltCm - baroAltCm; // not very accurate
|
||||
useZeroedGpsAltitude = true;
|
||||
}
|
||||
if (useZeroedGpsAltitude) { // normal situation
|
||||
zeroedAltitudeCm = gpsAltCm - gpsAltOffsetCm; // now that we have a GPS offset value, we can use it to zero relativeAltitude
|
||||
}
|
||||
} else {
|
||||
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(zeroedAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
|
||||
|
||||
// Empirical mixing of GPS and Baro altitudes
|
||||
if (useZeroedGpsAltitude && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == GPS_ONLY)) {
|
||||
if (haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
|
||||
// mix zeroed GPS with Baro altitude data, if Baro data exists if are in default altitude control mode
|
||||
const float absDifferenceM = fabsf(zeroedAltitudeCm - baroAltCm) / 100.0f * positionConfig()->altitude_prefer_baro / 100.0f;
|
||||
if (absDifferenceM > 1.0f) { // when there is a large difference, favour Baro
|
||||
gpsTrust /= absDifferenceM;
|
||||
}
|
||||
zeroedAltitudeCm = zeroedAltitudeCm * gpsTrust + baroAltCm * (1.0f - gpsTrust);
|
||||
}
|
||||
} else if (haveBaroAlt && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == BARO_ONLY)) {
|
||||
zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
|
||||
}
|
||||
}
|
||||
|
||||
zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
|
||||
// NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming
|
||||
|
||||
if (wasArmed) {
|
||||
displayAltitudeCm = zeroedAltitudeCm; // while armed, show filtered relative altitude in OSD / sensors tab
|
||||
}
|
||||
|
||||
// *** calculate Vario signal
|
||||
static float previousZeroedAltitudeCm = 0.0f;
|
||||
zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
|
||||
previousZeroedAltitudeCm = zeroedAltitudeCm;
|
||||
|
||||
zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative);
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_GPS) || defined(USE_RANGEFINDER)
|
||||
void calculateEstimatedAltitude(void) {
|
||||
altitudeAvailable = altSensorFusionUpdate();
|
||||
if (altitudeAvailable) {
|
||||
zeroedAltitudeCm = getAltitudeState()->distCm;
|
||||
zeroedAltitudeDerivative = getAltitudeState()->velocityCm;
|
||||
displayAltitudeCm = zeroedAltitudeCm;
|
||||
#ifdef USE_VARIO
|
||||
estimatedVario = lrintf(zeroedAltitudeDerivative);
|
||||
estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
|
||||
estimatedVario = applyDeadband(lrintf(zeroedAltitudeDerivative), 10);
|
||||
#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
|
||||
#ifdef USE_VARIO
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
|
||||
#endif
|
||||
DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
|
||||
DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedAltitudeCm));
|
||||
|
||||
altitudeAvailable = haveGpsAlt || haveBaroAlt;
|
||||
}
|
||||
}
|
||||
|
||||
#endif //defined(USE_BARO) || defined(USE_GPS)
|
||||
#endif //defined(USE_BARO) || defined(USE_GPS) || defined(USE_RANGEFINDER)
|
||||
|
||||
float getAltitudeCm(void)
|
||||
{
|
||||
|
|
@ -224,7 +102,7 @@ float getAltitudeCm(void)
|
|||
|
||||
float getAltitudeDerivative(void)
|
||||
{
|
||||
return zeroedAltitudeDerivative; // cm/s
|
||||
return zeroedAltitudeDerivative;
|
||||
}
|
||||
|
||||
bool isAltitudeAvailable(void) {
|
||||
|
|
@ -233,7 +111,7 @@ bool isAltitudeAvailable(void) {
|
|||
|
||||
int32_t getEstimatedAltitudeCm(void)
|
||||
{
|
||||
return lrintf(displayAltitudeCm);
|
||||
return displayAltitudeCm; // cm
|
||||
}
|
||||
|
||||
#ifdef USE_GPS
|
||||
|
|
|
|||
|
|
@ -41,4 +41,3 @@ int32_t getEstimatedAltitudeCm(void);
|
|||
float getAltitudeAsl(void);
|
||||
int16_t getEstimatedVario(void);
|
||||
bool isAltitudeAvailable(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -527,4 +527,9 @@ static void performBaroCalibrationCycle(const float altitude)
|
|||
}
|
||||
}
|
||||
|
||||
int32_t getBaroTemperature(void)
|
||||
{
|
||||
return baro.temperature;
|
||||
}
|
||||
|
||||
#endif /* BARO */
|
||||
|
|
|
|||
|
|
@ -70,3 +70,4 @@ void baroSetGroundLevel(void);
|
|||
uint32_t baroUpdate(timeUs_t currentTimeUs);
|
||||
bool isBaroReady(void);
|
||||
float getBaroAltitude(void);
|
||||
int32_t getBaroTemperature(void);
|
||||
|
|
|
|||
|
|
@ -353,5 +353,10 @@ bool rangefinderIsHealthy(void)
|
|||
{
|
||||
return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
|
||||
}
|
||||
|
||||
rangefinder_t * getRangefinder(void)
|
||||
{
|
||||
return &rangefinder;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -65,3 +65,4 @@ int32_t rangefinderGetLatestRawAltitude(void);
|
|||
void rangefinderUpdate(void);
|
||||
bool rangefinderProcess(float cosTiltAngle);
|
||||
bool rangefinderIsHealthy(void);
|
||||
rangefinder_t * getRangefinder(void);
|
||||
|
|
|
|||
|
|
@ -43,6 +43,7 @@ extern "C" {
|
|||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/servos.h"
|
||||
#include "flight/altitude.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/gps.h"
|
||||
|
|
|
|||
|
|
@ -46,6 +46,8 @@ extern "C" {
|
|||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/kalman_filter_1d.h"
|
||||
#include "flight/altitude.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
|
||||
|
|
@ -68,6 +70,36 @@ extern "C" {
|
|||
const float dcmKpGain);
|
||||
float imuCalcMagErr(void);
|
||||
float imuCalcCourseErr(float courseOverGround);
|
||||
void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance){
|
||||
UNUSED(kf);
|
||||
UNUSED(initialValue);
|
||||
UNUSED(initialVariance);
|
||||
UNUSED(processVariance);
|
||||
}
|
||||
void kf_update_variance(KalmanFilter *kf) {
|
||||
UNUSED(kf);
|
||||
}
|
||||
void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas) {
|
||||
UNUSED(sensorMeas);
|
||||
UNUSED(kf);
|
||||
}
|
||||
void updateBaroVariance(SensorMeasurement * sensorMeas, float baroNonZeroedAlt) {
|
||||
UNUSED(sensorMeas);
|
||||
UNUSED(baroNonZeroedAlt);
|
||||
}
|
||||
void altSensorFusionInit(void) {
|
||||
|
||||
}
|
||||
bool altSensorFusionUpdate(void) {
|
||||
return true;
|
||||
}
|
||||
void updateBaroStateCallback(void) {
|
||||
|
||||
}
|
||||
altitudeState_t * getAltitudeState(void) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
extern quaternion_t q;
|
||||
extern matrix33_t rMat;
|
||||
extern bool attitudeIsEstablished;
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@ extern "C" {
|
|||
#include "platform.h"
|
||||
#include "scheduler/scheduler.h"
|
||||
#include "scheduler_stubs.h"
|
||||
#include "flight/altitude.h"
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue