mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 21:35:44 +03:00
120hz altitude data used in GPS Rescue
Disarm on crash flip recovery or impact while in do nothing mode add blackbox headers for new fields rearrange altitude factors fix ibus uint16 for estimatedVario update GPS altitude at 120hz get GPS derivative from error PT2 filter on GPS derivative remove acceleration element move altitude filtering to position.c calculate altitude derivative in position.c for vario filter vario with PT1 update pg in position and baro field and test tuning from field tests land 3x faster from higher altitude PT2 vario for checking GPS
This commit is contained in:
ctzsnooze
parent
e1cafc0434
commit
b88e73d137
12 changed files with 261 additions and 251 deletions
|
|
@ -1448,11 +1448,12 @@ static bool blackboxWriteSysinfo(void)
|
|||
#endif
|
||||
#ifdef USE_BARO
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d", barometerConfig()->baro_hardware);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_NOISE_LPF, "%d", barometerConfig()->baro_noise_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_VARIO_LPF, "%d", barometerConfig()->baro_vario_lpf);
|
||||
#endif
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALT_SOURCE, "%d", positionConfig()->altSource);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALT_PREFER_BARO, "%d", positionConfig()->altPreferBaro);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_SOURCE, "%d", positionConfig()->altitude_source);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_LPF, "%d", positionConfig()->altitude_lpf);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_D_LPF, "%d", positionConfig()->altitude_d_lpf);
|
||||
|
||||
#ifdef USE_MAG
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d", compassConfig()->mag_hardware);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -467,7 +467,7 @@ static const char * const lookupTableGyroFilterDebug[] = {
|
|||
"ROLL", "PITCH", "YAW"
|
||||
};
|
||||
|
||||
static const char * const lookupTablePositionAltSource[] = {
|
||||
static const char * const lookupTablePositionAltitudeSource[] = {
|
||||
"DEFAULT", "BARO_ONLY", "GPS_ONLY"
|
||||
};
|
||||
|
||||
|
|
@ -630,7 +630,7 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
|
||||
LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug),
|
||||
|
||||
LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
|
||||
LOOKUP_TABLE_ENTRY(lookupTablePositionAltitudeSource),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableOffOnAuto),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableFeedforwardAveraging),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableDshotBitbangedTimer),
|
||||
|
|
@ -733,8 +733,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "baro_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) },
|
||||
{ "baro_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) },
|
||||
{ PARAM_NAME_BARO_HARDWARE, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) },
|
||||
{ "baro_noise_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) },
|
||||
{ "baro_vario_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_vario_lpf) },
|
||||
#endif
|
||||
|
||||
// PG_RX_CONFIG
|
||||
|
|
@ -1669,8 +1667,11 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
// PG_POSITION
|
||||
{ "position_alt_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
|
||||
{ "position_alt_prefer_baro", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altPreferBaro) },
|
||||
{ "altitude_source", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altitude_source) },
|
||||
{ "altitude_prefer_baro", VAR_INT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altitude_prefer_baro) },
|
||||
{ "altitude_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_lpf) },
|
||||
{ "altitude_d_lpf", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) },
|
||||
|
||||
// PG_MODE_ACTIVATION_CONFIG
|
||||
#if defined(USE_CUSTOM_BOX_NAMES)
|
||||
{ "box_user_1_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_1_name) },
|
||||
|
|
|
|||
|
|
@ -34,8 +34,6 @@
|
|||
#define PARAM_NAME_ACC_LPF_HZ "acc_lpf_hz"
|
||||
#define PARAM_NAME_MAG_HARDWARE "mag_hardware"
|
||||
#define PARAM_NAME_BARO_HARDWARE "baro_hardware"
|
||||
#define PARAM_NAME_BARO_NOISE_LPF "baro_noise_lpf"
|
||||
#define PARAM_NAME_BARO_VARIO_LPF "baro_vario_lpf"
|
||||
#define PARAM_NAME_RC_SMOOTHING "rc_smoothing"
|
||||
#define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR "rc_smoothing_auto_factor"
|
||||
#define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE "rc_smoothing_auto_factor_throttle"
|
||||
|
|
@ -119,8 +117,10 @@
|
|||
#define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz"
|
||||
#define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz"
|
||||
#define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz"
|
||||
#define PARAM_NAME_POSITION_ALT_SOURCE "position_alt_source"
|
||||
#define PARAM_NAME_POSITION_ALT_PREFER_BARO "position_alt_prefer_baro"
|
||||
#define PARAM_NAME_POSITION_ALTITUDE_SOURCE "altitude_source"
|
||||
#define PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO "altitude_prefer_baro"
|
||||
#define PARAM_NAME_POSITION_ALTITUDE_LPF "altitude_lpf"
|
||||
#define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf"
|
||||
|
||||
#ifdef USE_GPS
|
||||
#define PARAM_NAME_GPS_PROVIDER "gps_provider"
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h" // remove this later, only here temporarily for the cutoff for D
|
||||
|
||||
#include "gps_rescue.h"
|
||||
|
||||
|
|
@ -91,11 +92,12 @@ typedef struct {
|
|||
bool updateYaw;
|
||||
float descentDistanceM;
|
||||
int8_t secondsFailing;
|
||||
float altitudeStep;
|
||||
} rescueIntent_s;
|
||||
|
||||
typedef struct {
|
||||
int32_t maxAltitudeCm;
|
||||
int32_t currentAltitudeCm;
|
||||
float currentAltitudeCm;
|
||||
float distanceToHomeCm;
|
||||
float distanceToHomeM;
|
||||
uint16_t groundSpeedCmS; //cm/s
|
||||
|
|
@ -104,9 +106,9 @@ typedef struct {
|
|||
bool healthy;
|
||||
float errorAngle;
|
||||
float gpsDataIntervalSeconds;
|
||||
float altitudeDataIntervalSeconds;
|
||||
float velocityToHomeCmS;
|
||||
float ascendStepCm;
|
||||
float descendStepCm;
|
||||
float alitutudeStepCm;
|
||||
float maxPitchStep;
|
||||
float filterK;
|
||||
float absErrorAngle;
|
||||
|
|
@ -141,16 +143,16 @@ typedef enum {
|
|||
PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
||||
.angle = 32,
|
||||
.angle = 40,
|
||||
.initialAltitudeM = 30,
|
||||
.descentDistanceM = 20,
|
||||
.rescueGroundspeed = 500,
|
||||
.throttleP = 20,
|
||||
.throttleI = 20,
|
||||
.throttleD = 10,
|
||||
.throttleP = 15,
|
||||
.throttleI = 15,
|
||||
.throttleD = 15,
|
||||
.velP = 6,
|
||||
.velI = 20,
|
||||
.velD = 70,
|
||||
.velD = 50,
|
||||
.yawP = 25,
|
||||
.throttleMin = 1100,
|
||||
.throttleMax = 1600,
|
||||
|
|
@ -162,7 +164,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
|
|||
.targetLandingAltitudeM = 5,
|
||||
.altitudeMode = MAX_ALT,
|
||||
.ascendRate = 500, // cm/s, for altitude corrections on ascent
|
||||
.descendRate = 125, // cm/s, for descent and landing phase, or negative ascent
|
||||
.descendRate = 100, // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
|
||||
.rescueAltitudeBufferM = 10,
|
||||
.rollMix = 100
|
||||
);
|
||||
|
|
@ -172,9 +174,20 @@ static float rescueYaw;
|
|||
float gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
|
||||
bool magForceDisable = false;
|
||||
static bool newGPSData = false;
|
||||
static pt2Filter_t throttleDLpf;
|
||||
|
||||
rescueState_s rescueState;
|
||||
|
||||
void gpsRescueInit(void)
|
||||
{
|
||||
const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
|
||||
|
||||
const float throttleDCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
|
||||
const float throttleDCutoffGain = pt2FilterGain(throttleDCutoffHz, sampleTimeS);
|
||||
pt2FilterInit(&throttleDLpf, throttleDCutoffGain);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
If we have new GPS data, update home heading if possible and applicable.
|
||||
*/
|
||||
|
|
@ -239,11 +252,8 @@ static void rescueAttainPosition()
|
|||
static float velocityI = 0.0f;
|
||||
static float previousVelocityD = 0.0f; // for smoothing
|
||||
static float previousPitchAdjustment = 0.0f;
|
||||
static float previousAltitudeError = 0.0f;
|
||||
static float throttleI = 0.0f;
|
||||
static float previousThrottleD = 0.0f; // for jerk calc from raw Derivative
|
||||
static float previousThrottleDVal = 0.0f; // for moving average of D and jerk
|
||||
static float previousThrottleD2 = 0.0f; // for additional D first order smoothing
|
||||
static float previousAltitudeError = 0.0f;
|
||||
static int16_t throttleAdjustment = 0;
|
||||
|
||||
switch (rescueState.phase) {
|
||||
|
|
@ -260,22 +270,60 @@ static void rescueAttainPosition()
|
|||
velocityI = 0.0f;
|
||||
previousVelocityD = 0.0f;
|
||||
previousPitchAdjustment = 0.0f;
|
||||
previousAltitudeError = 0.0f;
|
||||
throttleI = 0.0f;
|
||||
previousThrottleD = 0.0f;
|
||||
previousThrottleDVal = 0.0f;
|
||||
previousThrottleD2 = 0.0f;
|
||||
previousAltitudeError = 0.0f;
|
||||
throttleAdjustment = 0;
|
||||
return;
|
||||
case RESCUE_DO_NOTHING:
|
||||
gpsRescueAngle[AI_PITCH] = 0.0f;
|
||||
gpsRescueAngle[AI_ROLL] = 0.0f;
|
||||
rescueThrottle = gpsRescueConfig()->throttleHover;
|
||||
rescueThrottle = gpsRescueConfig()->throttleHover - 50;
|
||||
return;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
/**
|
||||
Altitude (throttle) controller
|
||||
*/
|
||||
// currentAltitudeCm is updated at TASK_GPS_RATE since GPS initiates updateGPSRescueState()
|
||||
const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f;
|
||||
// height above target in metres (negative means too low)
|
||||
// at the start, the target starts at current altitude plus one step. Increases stepwise to intended value.
|
||||
|
||||
// P component
|
||||
const float throttleP = gpsRescueConfig()->throttleP * altitudeError;
|
||||
|
||||
// I component
|
||||
throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
|
||||
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
|
||||
// up to 20% increase in throttle from I alone
|
||||
|
||||
// D component from position.c
|
||||
// float throttleD = rescueState.sensor.currentAltitudeDerivative;
|
||||
|
||||
// is error based, so includes positive boost when climbing and negative boost on descent
|
||||
const float throttleDRaw = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
|
||||
previousAltitudeError = altitudeError;
|
||||
float throttleD = pt2FilterApply(&throttleDLpf, throttleDRaw);
|
||||
throttleD = gpsRescueConfig()->throttleD * throttleD;
|
||||
|
||||
// acceleration component not implemented - was needed previously due to GPS lag, maybe not needed now.
|
||||
|
||||
float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
|
||||
tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000);
|
||||
// if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful
|
||||
// too much and landings with lots of pitch adjustment, eg windy days, can be a problem
|
||||
|
||||
throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
|
||||
|
||||
rescueThrottle = gpsRescueConfig()->throttleHover + throttleAdjustment;
|
||||
rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD);
|
||||
|
||||
|
||||
// *** other updates occur only with new GPS data ***
|
||||
if (!newGPSData) {
|
||||
return;
|
||||
}
|
||||
|
|
@ -372,56 +420,6 @@ static void rescueAttainPosition()
|
|||
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, velocityD);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, rescueState.intent.targetVelocityCmS);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 1, rescueState.intent.targetVelocityCmS);
|
||||
|
||||
/**
|
||||
Altitude (throttle) controller
|
||||
*/
|
||||
// note that currentAltitudeCm can be updated more frequently than GPS rate from Baro, but this code requires GPS data to update
|
||||
// ToDo: use a delta time for changes in currentAltitudeCm, and run more frequently than GPS rate
|
||||
const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f;
|
||||
// height above target in metres (negative means too low)
|
||||
// at the start, the target starts at current altitude plus one step. Increases stepwise to intended value.
|
||||
|
||||
// P component
|
||||
const float throttleP = gpsRescueConfig()->throttleP * altitudeError;
|
||||
|
||||
// I component
|
||||
throttleI += 0.01f * gpsRescueConfig()->throttleI * altitudeError * sampleIntervalNormaliseFactor;
|
||||
throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
|
||||
// up to 20% increase in throttle from I alone
|
||||
|
||||
// D component
|
||||
// is error based, so includes positive boost when climbing and negative boost on descent
|
||||
float throttleD = ((altitudeError - previousAltitudeError) / sampleIntervalNormaliseFactor);
|
||||
previousAltitudeError = altitudeError;
|
||||
|
||||
// Acceleration (Jerk) component
|
||||
const float throttleDJerk = 2.0f * (throttleD - previousThrottleD);
|
||||
previousThrottleD = throttleD;
|
||||
throttleD += throttleDJerk;
|
||||
|
||||
// D Smoothing
|
||||
const float movingAvgAltitudeD = 0.5f * (previousThrottleDVal + throttleD);
|
||||
// moving average seems to work best here, a lot of sequential up and down in altitude data
|
||||
previousThrottleDVal = throttleD;
|
||||
throttleD = movingAvgAltitudeD;
|
||||
throttleD = previousThrottleD2 + rescueState.sensor.filterK * (throttleD - previousThrottleD2);
|
||||
// additional final first order D throttle smoothing
|
||||
previousThrottleD2 = throttleD;
|
||||
|
||||
throttleD = 10.0f * gpsRescueConfig()->throttleD * throttleD;
|
||||
|
||||
float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
|
||||
tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000);
|
||||
// if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful
|
||||
// too much and landings with lots of pitch adjustment, eg windy days, can be a problem
|
||||
|
||||
throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
|
||||
|
||||
rescueThrottle = gpsRescueConfig()->throttleHover + throttleAdjustment;
|
||||
rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD);
|
||||
}
|
||||
|
||||
static void performSanityChecks()
|
||||
|
|
@ -456,9 +454,11 @@ static void performSanityChecks()
|
|||
}
|
||||
}
|
||||
|
||||
// Check if crash recovery mode is active
|
||||
// If crash recovery is triggered during a rescue, immediately disarm, ignoring the crash flip recovery settings.
|
||||
if (crashRecoveryModeActive()) {
|
||||
rescueState.failure = RESCUE_CRASH_FLIP_DETECTED;
|
||||
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
|
||||
disarm(DISARM_REASON_CRASH_PROTECTION);
|
||||
rescueStop();
|
||||
}
|
||||
|
||||
// Check if GPS comms are healthy
|
||||
|
|
@ -533,12 +533,17 @@ static void performSanityChecks()
|
|||
|
||||
static void sensorUpdate()
|
||||
{
|
||||
static timeUs_t previousDataTimeUs = 0;
|
||||
static float prevDistanceToHomeCm = 0.0f;
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
|
||||
static timeUs_t previousAltitudeDataTimeUs = 0;
|
||||
const timeDelta_t altitudeDataIntervalUs = cmpTimeUs(currentTimeUs, previousAltitudeDataTimeUs);
|
||||
rescueState.sensor.altitudeDataIntervalSeconds = altitudeDataIntervalUs * 0.000001f;
|
||||
previousAltitudeDataTimeUs = currentTimeUs;
|
||||
|
||||
rescueState.sensor.currentAltitudeCm = getAltitude();
|
||||
|
||||
rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm();
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, rescueState.sensor.currentAltitudeCm);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
|
||||
// may be updated more frequently than GPS data
|
||||
|
||||
rescueState.sensor.healthy = gpsIsHealthy();
|
||||
|
|
@ -565,11 +570,11 @@ static void sensorUpdate()
|
|||
}
|
||||
rescueState.sensor.absErrorAngle = fabsf(rescueState.sensor.errorAngle);
|
||||
|
||||
const timeUs_t currentTimeUs = micros();
|
||||
const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousDataTimeUs);
|
||||
static timeUs_t previousGPSDataTimeUs = 0;
|
||||
const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs);
|
||||
rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f);
|
||||
// Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
|
||||
previousDataTimeUs = currentTimeUs;
|
||||
previousGPSDataTimeUs = currentTimeUs;
|
||||
|
||||
rescueState.sensor.filterK = pt1FilterGain(0.8, rescueState.sensor.gpsDataIntervalSeconds);
|
||||
// 0.8341 for 1hz, 0.5013 for 5hz, 0.3345 for 10hz, 0.1674 for 25Hz, etc
|
||||
|
|
@ -578,8 +583,6 @@ static void sensorUpdate()
|
|||
// positive = towards home. First value is useless since prevDistanceToHomeCm was zero.
|
||||
prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
|
||||
|
||||
rescueState.sensor.ascendStepCm = rescueState.sensor.gpsDataIntervalSeconds * gpsRescueConfig()->ascendRate;
|
||||
rescueState.sensor.descendStepCm = rescueState.sensor.gpsDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw); // degrees * 10
|
||||
|
|
@ -638,6 +641,22 @@ static bool checkGPSRescueIsAvailable(void)
|
|||
return result;
|
||||
}
|
||||
|
||||
void disarmOnImpact(void)
|
||||
{
|
||||
if (rescueState.sensor.accMagnitude > 2.0f) {
|
||||
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
|
||||
disarm(DISARM_REASON_GPS_RESCUE);
|
||||
rescueStop();
|
||||
}
|
||||
}
|
||||
|
||||
void altitudeAchieved(void)
|
||||
{
|
||||
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
|
||||
rescueState.intent.altitudeStep = 0;
|
||||
rescueState.phase = RESCUE_ROTATE;
|
||||
}
|
||||
|
||||
void updateGPSRescueState(void)
|
||||
// this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active
|
||||
{
|
||||
|
|
@ -651,12 +670,15 @@ void updateGPSRescueState(void)
|
|||
|
||||
// Will now be in RESCUE_INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE
|
||||
|
||||
sensorUpdate(); // always get latest GPS / Altitude data
|
||||
sensorUpdate(); // always get latest GPS / Altitude data; update ascend and descend rates
|
||||
|
||||
uint8_t halfAngle = gpsRescueConfig()->angle / 2;
|
||||
const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
|
||||
float proximityToLandingArea = 0.0f;
|
||||
bool startedLow = true;
|
||||
rescueState.isAvailable = checkGPSRescueIsAvailable();
|
||||
|
||||
|
||||
switch (rescueState.phase) {
|
||||
case RESCUE_IDLE:
|
||||
// in Idle phase = NOT in GPS Rescue
|
||||
|
|
@ -677,7 +699,8 @@ void updateGPSRescueState(void)
|
|||
// alternative is to prevent the rescue by returning to IDLE, but this could cause flyaways
|
||||
} else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
|
||||
// Attempt to initiate inside minimum activation distance -> landing mode
|
||||
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm - rescueState.sensor.descendStepCm;
|
||||
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
|
||||
rescueState.phase = RESCUE_LANDING;
|
||||
// start landing from current altitude
|
||||
} else {
|
||||
|
|
@ -688,6 +711,7 @@ void updateGPSRescueState(void)
|
|||
rescueState.intent.targetVelocityCmS = 0; // zero forward velocity while climbing
|
||||
rescueState.intent.pitchAngleLimitDeg = halfAngle; // only half pitch authority
|
||||
rescueState.intent.rollAngleLimitDeg = 0; // don't roll yet
|
||||
rescueState.intent.altitudeStep = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
|
|
@ -696,21 +720,19 @@ void updateGPSRescueState(void)
|
|||
// also require that the final target altitude has been achieved before moving on
|
||||
// sanity check will abort if altitude gain is blocked for a cumulative period
|
||||
// TO DO: if overshoots are a problem after craft achieves target altitude changes, adjust termination threshold with current vertical velocity
|
||||
if (newGPSData) {
|
||||
if (startedLow) {
|
||||
if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
|
||||
rescueState.intent.targetAltitudeCm += rescueState.sensor.ascendStepCm;
|
||||
rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
} else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
|
||||
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
|
||||
rescueState.phase = RESCUE_ROTATE;
|
||||
altitudeAchieved();
|
||||
}
|
||||
} else {
|
||||
if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
|
||||
rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm;
|
||||
rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
} else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
|
||||
rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
|
||||
rescueState.phase = RESCUE_ROTATE;
|
||||
}
|
||||
altitudeAchieved();
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
|
@ -740,6 +762,7 @@ void updateGPSRescueState(void)
|
|||
if (newGPSData) {
|
||||
if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
|
||||
rescueState.phase = RESCUE_DESCENT;
|
||||
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; // negative step dsescending
|
||||
rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
|
||||
}
|
||||
}
|
||||
|
|
@ -748,42 +771,35 @@ void updateGPSRescueState(void)
|
|||
case RESCUE_DESCENT:
|
||||
// attenuate velocity and altitude targets while updating the heading to home
|
||||
// once inside the landing box, stop rotating, just descend
|
||||
if (newGPSData) {
|
||||
const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
|
||||
if (rescueState.sensor.currentAltitudeCm < targetLandingAltitudeCm) {
|
||||
// enter landing mode once below landing altitude
|
||||
rescueState.phase = RESCUE_LANDING;
|
||||
rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm;
|
||||
rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
|
||||
rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
|
||||
rescueState.intent.targetVelocityCmS = 0; // zero velocity to home
|
||||
rescueState.intent.pitchAngleLimitDeg = halfAngle; // reduced pitch angles
|
||||
rescueState.intent.rollAngleLimitDeg = 0; // no roll while landing
|
||||
} else {
|
||||
const float distanceToLandingAreaM = MAX(rescueState.sensor.distanceToHomeM - 2.0f, 0.0f);
|
||||
// considers home to be within a 2m circle of home to avoid searching around when crossing home
|
||||
const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
|
||||
rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm * (1.0f + proximityToLandingArea);
|
||||
// reduce current altitude inexorably, by not less than descendStepCm and not more than 2*descendStepCm
|
||||
}
|
||||
if (newGPSData) {
|
||||
const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - 2.0f;
|
||||
// considers home to be a 2m circle around home to avoid overshooting home point
|
||||
proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
|
||||
rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
|
||||
// reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
|
||||
// if quad drifts further than 2m away from home, should by then have rotated towards home, and pitch is allowed
|
||||
rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea;
|
||||
// reduce roll capability when closer to home, none within final 2m
|
||||
}
|
||||
}
|
||||
const float proximityToGround = constrainf(rescueState.intent.targetAltitudeCm / 3000.0f, 0.0f, 2.0f);
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep * (1.0f + proximityToGround);
|
||||
// reduce current altitude inexorably, by not less than descendStepCm and not more than 5 * altitudeStep
|
||||
break;
|
||||
|
||||
case RESCUE_LANDING:
|
||||
// keep reducing target altitude, keep nose to home, zero velocity target with limited pitch control, no roll
|
||||
if (newGPSData) {
|
||||
rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm;
|
||||
rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
|
||||
// take one step off target altitude every time we get new GPS data
|
||||
}
|
||||
if (rescueState.sensor.accMagnitude > 2.0f) {
|
||||
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
|
||||
disarm(DISARM_REASON_GPS_RESCUE);
|
||||
rescueState.phase = RESCUE_COMPLETE;
|
||||
}
|
||||
disarmOnImpact();
|
||||
break;
|
||||
|
||||
case RESCUE_COMPLETE:
|
||||
|
|
@ -797,6 +813,7 @@ void updateGPSRescueState(void)
|
|||
break;
|
||||
|
||||
case RESCUE_DO_NOTHING:
|
||||
disarmOnImpact();
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -804,6 +821,7 @@ void updateGPSRescueState(void)
|
|||
}
|
||||
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, rescueState.intent.targetAltitudeCm);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
|
||||
DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.intent.targetAltitudeCm);
|
||||
DEBUG_SET(DEBUG_RTH, 1, rescueState.phase);
|
||||
|
||||
|
|
|
|||
|
|
@ -48,6 +48,7 @@ PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
|
|||
|
||||
extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES
|
||||
|
||||
void gpsRescueInit(void);
|
||||
void updateGPSRescueState(void);
|
||||
void rescueNewGpsData(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -47,34 +47,44 @@
|
|||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
static int32_t estimatedAltitudeCm = 0; // in cm
|
||||
#ifdef USE_BARO
|
||||
static pt2Filter_t baroDerivativeLpf;
|
||||
static float estimatedAltitudeCm = 0;
|
||||
static float estimatedAltitudeDerivative = 0;
|
||||
|
||||
#ifdef USE_VARIO
|
||||
static int16_t estimatedVario = 0; // in cm/s
|
||||
#endif
|
||||
|
||||
static pt2Filter_t altitudeLpf;
|
||||
static pt2Filter_t altitudeDerivativeLpf;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
typedef enum {
|
||||
DEFAULT = 0,
|
||||
BARO_ONLY,
|
||||
GPS_ONLY
|
||||
} altSource_e;
|
||||
} altitude_source_e;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 3);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 4);
|
||||
|
||||
PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
|
||||
.altSource = DEFAULT,
|
||||
.altPreferBaro = 100,
|
||||
.altitude_source = DEFAULT,
|
||||
.altitude_prefer_baro = 100,
|
||||
.altitude_lpf = 400,
|
||||
.altitude_d_lpf = 100,
|
||||
);
|
||||
|
||||
#ifdef USE_VARIO
|
||||
static int16_t estimatedVario = 0; // in cm/s
|
||||
|
||||
int16_t calculateEstimatedVario(float baroAltVelocity)
|
||||
{
|
||||
baroAltVelocity = applyDeadband(baroAltVelocity, 10.0f); // cm/s, so ignore climb rates less than 0.1 m/s
|
||||
return constrain(lrintf(baroAltVelocity), -1500, 1500);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_GPS)
|
||||
static bool altitudeOffsetSetBaro = false;
|
||||
static bool altitudeOffsetSetGPS = false;
|
||||
|
|
@ -83,34 +93,34 @@ void calculateEstimatedAltitude()
|
|||
{
|
||||
float baroAltCm = 0;
|
||||
float gpsAltCm = 0;
|
||||
float baroAltVelocity = 0;
|
||||
|
||||
float gpsTrust = 0.3; //conservative default
|
||||
bool haveBaroAlt = false;
|
||||
bool haveGpsAlt = false;
|
||||
#if defined(USE_GPS) && defined(USE_VARIO)
|
||||
float gpsVertSpeed = 0;
|
||||
#endif
|
||||
|
||||
// *** Set baroAlt once its calibration is complete (cannot arm until it is)
|
||||
#ifdef USE_BARO
|
||||
if (sensors(SENSOR_BARO)) {
|
||||
static float lastBaroAltCm = 0;
|
||||
static bool initBaroFilter = false;
|
||||
if (!initBaroFilter) {
|
||||
const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f;
|
||||
const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
|
||||
const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
|
||||
pt2FilterInit(&baroDerivativeLpf, gain);
|
||||
initBaroFilter = true;
|
||||
}
|
||||
baroAltCm = baroUpsampleAltitude();
|
||||
const float baroAltVelocityRaw = (baroAltCm - lastBaroAltCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
|
||||
baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw);
|
||||
lastBaroAltCm = baroAltCm;
|
||||
baroAltCm = getBaroAltitude();
|
||||
if (baroIsCalibrated()) {
|
||||
haveBaroAlt = true;
|
||||
}
|
||||
}
|
||||
|
||||
// *** Zero Baro Altitude on arming (every time we re-arm, also)
|
||||
// note that arming is blocked until baro 'calibration' (baro ground zeroing) is complete, no need to check status of haveBaroAlt
|
||||
// this code adds a secondary zeroing to whatever baro altitude value exists on arming
|
||||
// since props spin on arming, we want the last value before arming
|
||||
// provided that this function is called before significant motor spin-up has occured, this may not be a big problem
|
||||
|
||||
static float baroAltOffsetCm = 0;
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
baroAltCm -= baroAltOffsetCm; // use the last offset value from the disarm period
|
||||
altitudeOffsetSetBaro = true; // inevitable, but needed if no GPS
|
||||
} else {
|
||||
baroAltOffsetCm = baroAltCm; // while disarmed, keep capturing current altitude to zero any offset once armed
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// *** Check GPS for 3D fix, set haveGpsAlt if we have a 3D fix, and GPS Trust based on hdop, or leave at default of 0.3
|
||||
|
|
@ -119,39 +129,18 @@ void calculateEstimatedAltitude()
|
|||
// Need a 3D fix, which requires min 4 sats
|
||||
// if not, gpsAltCm remains zero, haveGpsAlt stays false, and gpsTrust is zero.
|
||||
gpsAltCm = gpsSol.llh.altCm;
|
||||
#ifdef USE_VARIO
|
||||
gpsVertSpeed = GPS_verticalSpeedInCmS;
|
||||
#endif
|
||||
haveGpsAlt = true; // remains false if no 3D fix
|
||||
|
||||
#ifdef USE_GPS
|
||||
if (gpsSol.hdop != 0) {
|
||||
gpsTrust = 100.0 / gpsSol.hdop;
|
||||
// *** TO DO - investigate if we should use vDOP or vACC with UBlox units; this hDOP value is actually pDOP in UBLox code !!!
|
||||
}
|
||||
#endif
|
||||
// always use at least 10% of other sources besides gps if available; limit effect of HDOP
|
||||
gpsTrust = MIN(gpsTrust, 0.9f);
|
||||
// With a 3D fix, GPS trust starts at 0.3
|
||||
} else {
|
||||
gpsTrust = 0.0f; // don't trust GPS if no sensor or 3D fix
|
||||
}
|
||||
#endif
|
||||
|
||||
// *** Zero Baro Altitude on arming (every time we re-arm, also)
|
||||
// note that arming is blocked until baro 'calibration' (baro ground zeroing) is complete, no need to check status of haveBaroAlt
|
||||
// this code adds a secondary zeroing to whatever baro altitude value exists on arming
|
||||
// since props spin on arming, we want the last value before arming
|
||||
// provided that this function is called before significant motor spin-up has occured, this may not be a big problem
|
||||
#ifdef USE_BARO
|
||||
static float baroAltOffsetCm = 0;
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
baroAltCm -= baroAltOffsetCm; // use the last offset value from the disarm period
|
||||
altitudeOffsetSetBaro = true; // inevitable, but needed if no GPS
|
||||
} else {
|
||||
baroAltOffsetCm = baroAltCm; // while disarmed, keep capturing current altitude to zero any offset once armed
|
||||
}
|
||||
#endif
|
||||
|
||||
// *** Zero GPS Altitude on every arm or re-arm using most recent disarmed values
|
||||
// but do not use GPS if there are not the required satellites
|
||||
|
|
@ -168,7 +157,7 @@ void calculateEstimatedAltitude()
|
|||
// If we don't, we wait until we get a 3D fix, and then correct the offset using the baro value
|
||||
// This won't be very accurate, but all we need is a 3D fix.
|
||||
// Note that without the 3D fix, GPS trust will be zero, and on getting a 3D fix, will depend on hDOP
|
||||
#ifdef USE_GPS
|
||||
|
||||
static float gpsAltOffsetCm = 0;
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
gpsAltCm -= gpsAltOffsetCm; // while armed, use the last offset value from the disarm period
|
||||
|
|
@ -187,14 +176,14 @@ void calculateEstimatedAltitude()
|
|||
// *** adjust gpsTrust, favouring Baro increasingly when there is a discrepancy of more than a meter
|
||||
// favour GPS if Baro reads negative, this happens due to ground effects
|
||||
float gpsTrustModifier = gpsTrust;
|
||||
const float absDifferenceM = fabsf(gpsAltCm - baroAltCm) * positionConfig()->altPreferBaro / 10000.0f;
|
||||
const float absDifferenceM = fabsf(gpsAltCm - baroAltCm) * positionConfig()->altitude_prefer_baro / 10000.0f;
|
||||
if (absDifferenceM > 1.0f && baroAltCm > -100.0f) { // significant difference, and baro altitude not negative
|
||||
gpsTrustModifier /= absDifferenceM;
|
||||
}
|
||||
// eg if discrepancy is 3m and GPS trust was 0.9, it would now be 0.3
|
||||
|
||||
// *** If we have a GPS with 3D fix and a Baro signal, blend them
|
||||
if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
|
||||
if (haveGpsAlt && haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
estimatedAltitudeCm = gpsAltCm * gpsTrustModifier + baroAltCm * (1 - gpsTrustModifier);
|
||||
} else {
|
||||
|
|
@ -202,34 +191,32 @@ void calculateEstimatedAltitude()
|
|||
//absolute GPS altitude is shown before arming, ignoring baro (I have no clue why this is)
|
||||
}
|
||||
|
||||
#ifdef USE_VARIO
|
||||
// baro is a better source for vario, so ignore gpsVertSpeed
|
||||
estimatedVario = calculateEstimatedVario(baroAltVelocity);
|
||||
#endif
|
||||
|
||||
// *** If we have a GPS but no baro, and are in Default or GPS_ONLY modes, use GPS values
|
||||
} else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
|
||||
} else if (haveGpsAlt && (positionConfig()->altitude_source == GPS_ONLY || positionConfig()->altitude_source == DEFAULT )) {
|
||||
estimatedAltitudeCm = gpsAltCm;
|
||||
#if defined(USE_VARIO) && defined(USE_GPS)
|
||||
estimatedVario = gpsVertSpeed;
|
||||
#endif
|
||||
|
||||
// *** If we have a Baro, and can work with it in Default or Baro Only modes
|
||||
} else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
|
||||
} else if (haveBaroAlt && (positionConfig()->altitude_source == BARO_ONLY || positionConfig()->altitude_source == DEFAULT)) {
|
||||
estimatedAltitudeCm = baroAltCm;
|
||||
|
||||
#ifdef USE_VARIO
|
||||
estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s
|
||||
#endif
|
||||
}
|
||||
|
||||
// Note that if we have no GPS but user chooses GPS Only, or no Baro but user chooses Baro only, then the reported altitude will be zero
|
||||
// The latter may cause GPS rescue to fail, but the user should notice an absence of altitude values.
|
||||
estimatedAltitudeCm = pt2FilterApply(&altitudeLpf, estimatedAltitudeCm);
|
||||
|
||||
static float previousEstimatedAltitudeCm = 0;
|
||||
const float altitudeDerivativeRaw = (estimatedAltitudeCm - previousEstimatedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
|
||||
previousEstimatedAltitudeCm = estimatedAltitudeCm;
|
||||
estimatedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, altitudeDerivativeRaw);
|
||||
|
||||
#ifdef USE_VARIO
|
||||
estimatedVario = lrintf(estimatedAltitudeDerivative);
|
||||
estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
|
||||
estimatedVario = constrain(estimatedVario, -1500, 1500);
|
||||
#endif
|
||||
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 1, baroAltCm);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedAltitudeCm);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 2, estimatedAltitudeCm);
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedAltitudeDerivative); // cm/s
|
||||
}
|
||||
|
||||
bool isAltitudeOffset(void)
|
||||
|
|
@ -239,6 +226,11 @@ bool isAltitudeOffset(void)
|
|||
#endif
|
||||
|
||||
int32_t getEstimatedAltitudeCm(void)
|
||||
{
|
||||
return lrintf(estimatedAltitudeCm);
|
||||
}
|
||||
|
||||
float getAltitude(void)
|
||||
{
|
||||
return estimatedAltitudeCm;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,13 +24,17 @@
|
|||
|
||||
#define TASK_ALTITUDE_RATE_HZ 120
|
||||
typedef struct positionConfig_s {
|
||||
uint8_t altSource;
|
||||
uint8_t altPreferBaro;
|
||||
uint8_t altitude_source;
|
||||
uint8_t altitude_prefer_baro;
|
||||
uint16_t altitude_lpf; // lowpass cutoff (value / 100) Hz for baro smoothing
|
||||
uint16_t altitude_d_lpf; // lowpass for (value / 100) Hz baro derivative smoothing
|
||||
} positionConfig_t;
|
||||
|
||||
PG_DECLARE(positionConfig_t, positionConfig);
|
||||
|
||||
bool isAltitudeOffset(void);
|
||||
void calculateEstimatedAltitude();
|
||||
void positionInit(void);
|
||||
int32_t getEstimatedAltitudeCm(void);
|
||||
float getAltitude(void);
|
||||
int16_t getEstimatedVario(void);
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@ extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N]; // Carrier to Noise Rati
|
|||
#define GPS_DBHZ_MIN 0
|
||||
#define GPS_DBHZ_MAX 55
|
||||
|
||||
#define TASK_GPS_RATE 100
|
||||
#define TASK_GPS_RATE 120
|
||||
#define TASK_GPS_RATE_FAST 1000
|
||||
|
||||
void gpsInit(void);
|
||||
|
|
|
|||
|
|
@ -59,12 +59,10 @@
|
|||
|
||||
baro_t baro; // barometer access functions
|
||||
|
||||
PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
|
||||
PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);
|
||||
|
||||
void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
|
||||
{
|
||||
barometerConfig->baro_noise_lpf = 50;
|
||||
barometerConfig->baro_vario_lpf = 10;
|
||||
barometerConfig->baro_hardware = BARO_DEFAULT;
|
||||
|
||||
// For backward compatibility; ceate a valid default value for bus parameters
|
||||
|
|
@ -145,7 +143,6 @@ static int32_t baroPressure = 0;
|
|||
static int32_t baroTemperature = 0;
|
||||
static float baroGroundAltitude = 0.0f;
|
||||
static float baroAltitudeRaw = 0.0f;
|
||||
static pt2Filter_t baroUpsampleLpf;
|
||||
|
||||
|
||||
#define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
|
||||
|
|
@ -163,20 +160,6 @@ void baroPreInit(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse);
|
||||
|
||||
void baroInit(void)
|
||||
{
|
||||
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
|
||||
|
||||
const float cutoffHz = barometerConfig()->baro_noise_lpf / 100.0f;
|
||||
const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
|
||||
const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
|
||||
pt2FilterInit(&baroUpsampleLpf, gain);
|
||||
|
||||
baroReady = true;
|
||||
}
|
||||
|
||||
static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
|
||||
{
|
||||
extDevice_t *dev = &baroDev->dev;
|
||||
|
|
@ -311,6 +294,12 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
|
|||
return true;
|
||||
}
|
||||
|
||||
void baroInit(void)
|
||||
{
|
||||
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
|
||||
baroReady = true;
|
||||
}
|
||||
|
||||
bool baroIsCalibrated(void)
|
||||
{
|
||||
return baroCalibrated;
|
||||
|
|
@ -432,6 +421,12 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
|
|||
|
||||
performBaroCalibrationCycle();
|
||||
|
||||
if (baroIsCalibrated()) {
|
||||
baro.BaroAlt = baroAltitudeRaw - baroGroundAltitude;
|
||||
} else {
|
||||
baro.BaroAlt = 0.0f;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature);
|
||||
DEBUG_SET(DEBUG_BARO, 2, baroAltitudeRaw - baroGroundAltitude);
|
||||
|
||||
|
|
@ -461,15 +456,9 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
|
|||
return sleepTime;
|
||||
}
|
||||
|
||||
// baroAltitudeRaw will get updated in the BARO task while baroUpsampleAltitude() will run in the ALTITUDE task.
|
||||
float baroUpsampleAltitude(void)
|
||||
// baroAltitude samples baro.BaroAlt the ALTITUDE task rate
|
||||
float getBaroAltitude(void)
|
||||
{
|
||||
const float baroAltitudeRawFiltered = pt2FilterApply(&baroUpsampleLpf, baroAltitudeRaw);
|
||||
if (baroIsCalibrated()) {
|
||||
baro.BaroAlt = baroAltitudeRawFiltered - baroGroundAltitude;
|
||||
} else {
|
||||
baro.BaroAlt = 0.0f;
|
||||
}
|
||||
DEBUG_SET(DEBUG_BARO, 3, baro.BaroAlt); // cm
|
||||
return baro.BaroAlt;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,8 +43,6 @@ typedef struct barometerConfig_s {
|
|||
uint8_t baro_i2c_device;
|
||||
uint8_t baro_i2c_address;
|
||||
uint8_t baro_hardware; // Barometer hardware to use
|
||||
uint16_t baro_noise_lpf; // lowpass cutoff (value / 100) Hz for baro smoothing
|
||||
uint16_t baro_vario_lpf; // lowpass for (value / 100) Hz baro derivative smoothing
|
||||
ioTag_t baro_eoc_tag;
|
||||
ioTag_t baro_xclr_tag;
|
||||
} barometerConfig_t;
|
||||
|
|
@ -72,5 +70,5 @@ void baroSetGroundLevel(void);
|
|||
uint32_t baroUpdate(timeUs_t currentTimeUs);
|
||||
bool isBaroReady(void);
|
||||
bool isBaroSampleReady(void);
|
||||
float baroUpsampleAltitude(void);
|
||||
float getBaroAltitude(void);
|
||||
void performBaroCalibrationCycle(void);
|
||||
|
|
|
|||
|
|
@ -32,6 +32,8 @@
|
|||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/pid.h"
|
||||
#include "flight/position.h"
|
||||
#include "flight/gps_rescue.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
|
@ -46,6 +48,7 @@
|
|||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
||||
|
||||
// requestedSensors is not actually used
|
||||
uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
|
||||
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
|
||||
|
|
@ -84,6 +87,9 @@ bool sensorsAutodetect(void)
|
|||
baroInit();
|
||||
#endif
|
||||
|
||||
positionInit();
|
||||
gpsRescueInit();
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
rangefinderInit();
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ uint16_t getVbat(void)
|
|||
|
||||
extern "C" {
|
||||
static int32_t amperage = 100;
|
||||
static int32_t estimatedVario = 0;
|
||||
static int16_t estimatedVario = 0;
|
||||
static uint8_t batteryRemaining = 0;
|
||||
static throttleStatus_e throttleStatus = THROTTLE_HIGH;
|
||||
static uint32_t definedFeatures = 0;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue