mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
Refactor position estimation code
Remove position estimate history code Remove rangefinder median filtering Change VEL_XY PID to allow more authority
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
f3422ce423
commit
a60e9f63d4
11 changed files with 128 additions and 182 deletions
|
|
@ -175,6 +175,7 @@ void initActiveBoxIds(void)
|
|||
activeBoxIds[activeBoxIdCount++] = BOXNAVALTHOLD;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
|
||||
}
|
||||
|
||||
if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
|
||||
activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
|
||||
|
|
|
|||
|
|
@ -201,8 +201,6 @@ void taskUpdateRangefinder(timeUs_t currentTimeUs)
|
|||
#ifdef USE_OPTICAL_FLOW
|
||||
void taskUpdateOpticalFlow(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
if (!sensors(SENSOR_OPFLOW))
|
||||
return;
|
||||
|
||||
|
|
|
|||
|
|
@ -206,6 +206,9 @@ groups:
|
|||
members:
|
||||
- name: rangefinder_hardware
|
||||
table: rangefinder_hardware
|
||||
- name: rangefinder_median_filter
|
||||
field: use_median_filtering
|
||||
type: bool
|
||||
|
||||
- name: PG_OPFLOW_CONFIG
|
||||
type: opticalFlowConfig_t
|
||||
|
|
@ -260,7 +263,7 @@ groups:
|
|||
members:
|
||||
- name: baro_hardware
|
||||
table: baro_hardware
|
||||
- name: baro_use_median_filter
|
||||
- name: baro_median_filter
|
||||
field: use_median_filtering
|
||||
type: bool
|
||||
|
||||
|
|
@ -976,10 +979,6 @@ groups:
|
|||
- name: inav_use_gps_velned
|
||||
field: use_gps_velned
|
||||
type: bool
|
||||
- name: inav_gps_delay
|
||||
field: gps_delay_ms
|
||||
min: 0
|
||||
max: 500
|
||||
- name: inav_reset_altitude
|
||||
field: reset_altitude_type
|
||||
table: reset_altitude
|
||||
|
|
|
|||
|
|
@ -126,7 +126,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
|
|||
.D = 10, // posResponseExpo * 100
|
||||
},
|
||||
[PID_VEL_XY] = {
|
||||
.P = 180, // NAV_VEL_XY_P * 100
|
||||
.P = 40, // NAV_VEL_XY_P * 20
|
||||
.I = 15, // NAV_VEL_XY_I * 100
|
||||
.D = 100, // NAV_VEL_XY_D * 100
|
||||
},
|
||||
|
|
|
|||
|
|
@ -48,13 +48,13 @@ static uint8_t buffer[10];
|
|||
static int bufferPtr;
|
||||
|
||||
typedef struct __attribute__((packed)) {
|
||||
uint8_t header;
|
||||
uint8_t res0;
|
||||
uint8_t header; // 0xFE
|
||||
uint8_t res0; // Seems to be 0x04 all the time
|
||||
int16_t motionX;
|
||||
int16_t motionY;
|
||||
int8_t motionT;
|
||||
uint8_t squal;
|
||||
uint8_t footer;
|
||||
int8_t motionT; // ???
|
||||
uint8_t squal; // Not sure about this
|
||||
uint8_t footer; // 0xAA
|
||||
} cxofPacket_t;
|
||||
|
||||
static bool cxofOpflowDetect(void)
|
||||
|
|
|
|||
|
|
@ -171,6 +171,7 @@ static void setupAltitudeController(void);
|
|||
void resetNavigation(void);
|
||||
void resetGCSFlags(void);
|
||||
|
||||
static bool posEstimationHasGlobalReference(void);
|
||||
static void calcualteAndSetActiveWaypoint(const navWaypoint_t * waypoint);
|
||||
static void calcualteAndSetActiveWaypointToLocalPosition(const t_fp_vector * pos);
|
||||
void calculateInitialHoldPosition(t_fp_vector * pos);
|
||||
|
|
@ -790,8 +791,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
{
|
||||
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
|
||||
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
|
||||
// If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
|
|
@ -1477,8 +1479,6 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
|
|||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
posControl.flags.estHeadingStatus = isImuHeadingValid() ? EST_TRUSTED : EST_NONE;
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[X] = newX;
|
||||
navLatestActualPosition[Y] = newY;
|
||||
|
|
@ -1534,18 +1534,19 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
|
|||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[Z] = constrain(newAltitude, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(newVelocity, -32678, 32767);
|
||||
navLatestActualPosition[Z] = constrain(posControl.actualState.pos.V.Z, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(posControl.actualState.vel.V.Z, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Processes an update to estimated heading
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualHeading(int32_t newHeading)
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading)
|
||||
{
|
||||
/* Update heading */
|
||||
posControl.actualState.yaw = newHeading;
|
||||
posControl.flags.estHeadingStatus = headingValid ? EST_TRUSTED : EST_NONE;
|
||||
|
||||
/* Precompute sin/cos of yaw angle */
|
||||
posControl.actualState.sinYaw = sin_approx(CENTIDEGREES_TO_RADIANS(newHeading));
|
||||
|
|
@ -2319,6 +2320,11 @@ static bool canActivatePosHoldMode(void)
|
|||
return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static bool posEstimationHasGlobalReference(void)
|
||||
{
|
||||
return true; // For now assume that we always have global coordinates available
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||
{
|
||||
static bool canActivateWaypoint = false;
|
||||
|
|
@ -2370,7 +2376,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
}
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
|
||||
if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
}
|
||||
else {
|
||||
|
|
@ -2571,7 +2577,7 @@ void navigationUsePIDs(void)
|
|||
for (int axis = 0; axis < 2; axis++) {
|
||||
navPInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f);
|
||||
|
||||
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 100.0f,
|
||||
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
|
||||
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -76,7 +76,6 @@ typedef struct positionEstimationConfig_s {
|
|||
uint8_t reset_altitude_type;
|
||||
uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s)
|
||||
uint8_t use_gps_velned;
|
||||
uint16_t gps_delay_ms;
|
||||
|
||||
uint16_t max_surface_altitude;
|
||||
|
||||
|
|
|
|||
|
|
@ -151,12 +151,6 @@ typedef struct {
|
|||
bool isBaroGroundValid;
|
||||
} navPositionEstimatorSTATE_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t index;
|
||||
t_fp_vector pos[INAV_HISTORY_BUF_SIZE];
|
||||
t_fp_vector vel[INAV_HISTORY_BUF_SIZE];
|
||||
} navPosisitonEstimatorHistory_t;
|
||||
|
||||
typedef struct {
|
||||
t_fp_vector accelNEU;
|
||||
t_fp_vector accelBias;
|
||||
|
|
@ -177,22 +171,18 @@ typedef struct {
|
|||
// Estimate
|
||||
navPositionEstimatorESTIMATE_t est;
|
||||
|
||||
// Estimation history
|
||||
navPosisitonEstimatorHistory_t history;
|
||||
|
||||
// Extra state variables
|
||||
navPositionEstimatorSTATE_t state;
|
||||
} navigationPosEstimator_t;
|
||||
|
||||
static navigationPosEstimator_t posEstimator;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 1);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
||||
// Inertial position estimator parameters
|
||||
.automatic_mag_declination = 1,
|
||||
.reset_altitude_type = NAV_RESET_ALTITUDE_ON_FIRST_ARM,
|
||||
.gps_delay_ms = 200,
|
||||
.gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity)
|
||||
.use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
|
||||
|
||||
|
|
@ -625,13 +615,19 @@ static float updateEPE(const float oldEPE, const float dt, const float newEPE, c
|
|||
return oldEPE + (newEPE - oldEPE) * w * dt;
|
||||
}
|
||||
|
||||
static bool navIsHeadingUsable(void)
|
||||
{
|
||||
// If we have GPS - we need true IMU north (valid heading)
|
||||
return isImuHeadingValid();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
|
||||
* Function is called at main loop rate
|
||||
*/
|
||||
static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
||||
{
|
||||
t_fp_vector accelBiasCorr;
|
||||
/* Calculate dT */
|
||||
float dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
|
||||
posEstimator.est.lastUpdateTime = currentTimeUs;
|
||||
|
||||
|
|
@ -654,14 +650,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
newEPV *= 1.0f + dt;
|
||||
}
|
||||
|
||||
|
||||
/* Figure out if we have valid position data from our data sources */
|
||||
bool isGPSValid = sensors(SENSOR_GPS) &&
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
//isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
|
||||
#endif
|
||||
const bool isGPSValid = sensors(SENSOR_GPS) &&
|
||||
posControl.gpsOrigin.valid &&
|
||||
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
|
||||
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv); // EPV is checked later
|
||||
bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
|
||||
bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS));
|
||||
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv);
|
||||
const bool isGPSZValid = isGPSValid && (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv);
|
||||
const bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
|
||||
const bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS));
|
||||
|
||||
/* Do some preparations to data */
|
||||
if (isBaroValid) {
|
||||
|
|
@ -690,12 +689,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
((isSurfaceValid && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
|
||||
(isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
|
||||
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
//isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
|
||||
#endif
|
||||
|
||||
/* Validate EPV for GPS and calculate altitude/climb rate correction flags */
|
||||
const bool isGPSZValid = isGPSValid && (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv);
|
||||
const bool useGpsZPos = STATE(FIXED_WING) && !sensors(SENSOR_BARO) && isGPSValid && isGPSZValid;
|
||||
const bool useGpsZVel = isGPSValid && isGPSZValid;
|
||||
|
||||
|
|
@ -703,44 +697,6 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
const bool isEstXYValid = (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv);
|
||||
const bool isEstZValid = (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv);
|
||||
|
||||
/* Handle GPS loss and recovery */
|
||||
if (isGPSValid) {
|
||||
bool positionWasReset = false;
|
||||
|
||||
/* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
|
||||
if (!isEstXYValid) {
|
||||
posEstimator.est.pos.V.X = posEstimator.gps.pos.V.X;
|
||||
posEstimator.est.pos.V.Y = posEstimator.gps.pos.V.Y;
|
||||
posEstimator.est.vel.V.X = posEstimator.gps.vel.V.X;
|
||||
posEstimator.est.vel.V.Y = posEstimator.gps.vel.V.Y;
|
||||
newEPH = posEstimator.gps.eph;
|
||||
positionWasReset = true;
|
||||
}
|
||||
|
||||
if (!isEstZValid && useGpsZPos) {
|
||||
posEstimator.est.pos.V.Z = posEstimator.gps.pos.V.Z;
|
||||
posEstimator.est.vel.V.Z = posEstimator.gps.vel.V.Z;
|
||||
newEPV = posEstimator.gps.epv;
|
||||
positionWasReset = true;
|
||||
}
|
||||
|
||||
/* If position was reset we need to reset history as well */
|
||||
if (positionWasReset) {
|
||||
for (int i = 0; i < INAV_HISTORY_BUF_SIZE; i++) {
|
||||
posEstimator.history.pos[i] = posEstimator.est.pos;
|
||||
posEstimator.history.vel[i] = posEstimator.est.vel;
|
||||
}
|
||||
|
||||
posEstimator.history.index = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Pre-calculate history index for GPS delay compensation */
|
||||
int gpsHistoryIndex = (posEstimator.history.index - 1) - constrain(((int)positionEstimationConfig()->gps_delay_ms / (1000 / INAV_POSITION_PUBLISH_RATE_HZ)), 0, INAV_HISTORY_BUF_SIZE - 1);
|
||||
if (gpsHistoryIndex < 0) {
|
||||
gpsHistoryIndex += INAV_HISTORY_BUF_SIZE;
|
||||
}
|
||||
|
||||
/* Prediction step: Z-axis */
|
||||
if (isEstZValid) {
|
||||
inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.V.Z);
|
||||
|
|
@ -748,7 +704,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Prediction step: XY-axis */
|
||||
if (isEstXYValid) {
|
||||
if (isImuHeadingValid()) {
|
||||
if (navIsHeadingUsable()) {
|
||||
inavFilterPredict(X, dt, posEstimator.imu.accelNEU.V.X);
|
||||
inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.V.Y);
|
||||
}
|
||||
|
|
@ -758,105 +714,98 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
|
||||
/* Calculate residual */
|
||||
float gpsResidual[3][2];
|
||||
float baroResidual;
|
||||
float gpsResidualXYMagnitude;
|
||||
|
||||
if (isGPSValid) {
|
||||
gpsResidual[X][0] = posEstimator.gps.pos.V.X - posEstimator.history.pos[gpsHistoryIndex].V.X;
|
||||
gpsResidual[Y][0] = posEstimator.gps.pos.V.Y - posEstimator.history.pos[gpsHistoryIndex].V.Y;
|
||||
gpsResidual[Z][0] = posEstimator.gps.pos.V.Z - posEstimator.history.pos[gpsHistoryIndex].V.Z;
|
||||
gpsResidual[X][1] = posEstimator.gps.vel.V.X - posEstimator.history.vel[gpsHistoryIndex].V.X;
|
||||
gpsResidual[Y][1] = posEstimator.gps.vel.V.Y - posEstimator.history.vel[gpsHistoryIndex].V.Y;
|
||||
gpsResidual[Z][1] = posEstimator.gps.vel.V.Z - posEstimator.history.vel[gpsHistoryIndex].V.Z;
|
||||
|
||||
gpsResidualXYMagnitude = sqrtf(sq(gpsResidual[X][0]) + sq(gpsResidual[Y][0]));
|
||||
}
|
||||
|
||||
if (isBaroValid) {
|
||||
/* If we are going to use GPS Z-position - calculate and apply barometer offset */
|
||||
baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.V.Z;
|
||||
}
|
||||
/* Accelerometer bias correction */
|
||||
const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0);
|
||||
t_fp_vector accelBiasCorr = { { 0, 0, 0} };
|
||||
|
||||
/* Correction step: Z-axis */
|
||||
if (useGpsZPos || isBaroValid) {
|
||||
float gpsWeightScaler = 1.0f;
|
||||
|
||||
/* Handle Z-axis reset */
|
||||
if (!isEstZValid && useGpsZPos) {
|
||||
posEstimator.est.pos.V.Z = posEstimator.gps.pos.V.Z;
|
||||
posEstimator.est.vel.V.Z = posEstimator.gps.vel.V.Z;
|
||||
newEPV = posEstimator.gps.epv;
|
||||
}
|
||||
else {
|
||||
#if defined(USE_BARO)
|
||||
/* Apply BARO correction to altitude */
|
||||
if (isBaroValid) {
|
||||
/* Apply only baro correction, no sonar */
|
||||
const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.V.Z;
|
||||
inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p);
|
||||
|
||||
/* Adjust EPV */
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
|
||||
|
||||
/* accelerometer bias correction for baro */
|
||||
if (updateAccBias && !isAirCushionEffectDetected) {
|
||||
accelBiasCorr.V.Z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Apply GPS correction to altitude */
|
||||
if (useGpsZPos) {
|
||||
/*
|
||||
gpsWeightScaler = scaleRangef(bellCurve(gpsResidual[Z][0], INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
inavFilterCorrectPos(Z, dt, gpsResidual[Z][0], positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler);
|
||||
*/
|
||||
inavFilterCorrectPos(Z, dt, gpsResidual[Z][0], positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler);
|
||||
const float gpsResidualZ = posEstimator.gps.pos.V.Z - posEstimator.est.pos.V.Z;
|
||||
inavFilterCorrectPos(Z, dt, gpsResidualZ, positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler);
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p);
|
||||
|
||||
/* Adjust EPV */
|
||||
newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidual[Z][0]), positionEstimationConfig()->w_z_gps_p);
|
||||
if (updateAccBias) {
|
||||
accelBiasCorr.V.Z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
}
|
||||
}
|
||||
|
||||
/* Apply GPS correction to climb rate */
|
||||
if (useGpsZVel) {
|
||||
inavFilterCorrectVel(Z, dt, gpsResidual[Z][1], positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
|
||||
const float gpsResidualZVel = posEstimator.gps.vel.V.Z - posEstimator.est.vel.V.Z;
|
||||
inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.V.Z, positionEstimationConfig()->w_z_res_v);
|
||||
}
|
||||
|
||||
/* Correct position from GPS - always if GPS is valid */
|
||||
/* Correction step: XY-axis */
|
||||
/* GPS */
|
||||
if (isGPSValid) {
|
||||
/* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
|
||||
if (!isEstXYValid) {
|
||||
posEstimator.est.pos.V.X = posEstimator.gps.pos.V.X;
|
||||
posEstimator.est.pos.V.Y = posEstimator.gps.pos.V.Y;
|
||||
posEstimator.est.vel.V.X = posEstimator.gps.vel.V.X;
|
||||
posEstimator.est.vel.V.Y = posEstimator.gps.vel.V.Y;
|
||||
newEPH = posEstimator.gps.eph;
|
||||
}
|
||||
else {
|
||||
const float gpsResidualX = posEstimator.gps.pos.V.X - posEstimator.est.pos.V.X;
|
||||
const float gpsResidualY = posEstimator.gps.pos.V.Y - posEstimator.est.pos.V.Y;
|
||||
const float gpsResidualXVel = posEstimator.gps.vel.V.X - posEstimator.est.vel.V.X;
|
||||
const float gpsResidualYVel = posEstimator.gps.vel.V.Y - posEstimator.est.vel.V.Y;
|
||||
const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY));
|
||||
|
||||
//const float gpsWeightScaler = scaleRangef(bellCurve(gpsResidualXYMagnitude, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
const float gpsWeightScaler = 1.0f;
|
||||
|
||||
const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler;
|
||||
const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler);
|
||||
|
||||
inavFilterCorrectPos(X, dt, gpsResidual[X][0], w_xy_gps_p);
|
||||
inavFilterCorrectPos(Y, dt, gpsResidual[Y][0], w_xy_gps_p);
|
||||
inavFilterCorrectPos(X, dt, gpsResidualX, w_xy_gps_p);
|
||||
inavFilterCorrectPos(Y, dt, gpsResidualY, w_xy_gps_p);
|
||||
|
||||
inavFilterCorrectVel(X, dt, gpsResidual[X][1], w_xy_gps_v);
|
||||
inavFilterCorrectVel(Y, dt, gpsResidual[Y][1], w_xy_gps_v);
|
||||
inavFilterCorrectVel(X, dt, gpsResidualXVel, w_xy_gps_v);
|
||||
inavFilterCorrectVel(Y, dt, gpsResidualYVel, w_xy_gps_v);
|
||||
|
||||
/* Adjust EPH */
|
||||
newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsResidualXYMagnitude), positionEstimationConfig()->w_xy_gps_p);
|
||||
}
|
||||
}
|
||||
else {
|
||||
inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.V.X, positionEstimationConfig()->w_xy_res_v);
|
||||
inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.V.Y, positionEstimationConfig()->w_xy_res_v);
|
||||
}
|
||||
|
||||
/* Correct accelerometer bias */
|
||||
if (positionEstimationConfig()->w_acc_bias > 0) {
|
||||
accelBiasCorr.V.X = 0;
|
||||
accelBiasCorr.V.Y = 0;
|
||||
accelBiasCorr.V.Z = 0;
|
||||
|
||||
/* accelerometer bias correction for GPS */
|
||||
if (isGPSValid) {
|
||||
accelBiasCorr.V.X -= gpsResidual[X][0] * sq(positionEstimationConfig()->w_xy_gps_p);
|
||||
accelBiasCorr.V.Y -= gpsResidual[Y][0] * sq(positionEstimationConfig()->w_xy_gps_p);
|
||||
|
||||
if (useGpsZPos) {
|
||||
accelBiasCorr.V.Z -= gpsResidual[Z][0] * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
}
|
||||
}
|
||||
|
||||
/* accelerometer bias correction for baro */
|
||||
if (isBaroValid && !isAirCushionEffectDetected) {
|
||||
accelBiasCorr.V.Z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
}
|
||||
|
||||
if (updateAccBias) {
|
||||
const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.V.X) + sq(accelBiasCorr.V.Y) + sq(accelBiasCorr.V.Z);
|
||||
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
|
||||
/* transform error vector from NEU frame to body frame */
|
||||
|
|
@ -873,8 +822,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
posEstimator.est.eph = newEPH;
|
||||
posEstimator.est.epv = newEPV;
|
||||
|
||||
/* AGL estimation */
|
||||
#ifdef USE_RANGEFINDER
|
||||
/* Surface offset */
|
||||
if (isSurfaceValid) { // If surface topic is updated in timely manner - do something smart
|
||||
navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
|
||||
bool resetSurfaceEstimate = false;
|
||||
|
|
@ -970,6 +919,14 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
|
||||
DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
|
||||
DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
|
||||
DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
|
||||
#endif
|
||||
|
||||
#else
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
|
|
@ -986,7 +943,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
static navigationTimer_t posPublishTimer;
|
||||
|
||||
/* IMU operates in decidegrees while INAV operates in deg*100 */
|
||||
updateActualHeading(DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw));
|
||||
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw));
|
||||
|
||||
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
|
||||
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
|
||||
|
|
@ -1007,21 +964,6 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.V.Z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
|
||||
DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
|
||||
DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
|
||||
DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
|
||||
#endif
|
||||
|
||||
/* Store history data */
|
||||
posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos;
|
||||
posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel;
|
||||
posEstimator.history.index++;
|
||||
if (posEstimator.history.index >= INAV_HISTORY_BUF_SIZE) {
|
||||
posEstimator.history.index = 0;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navEPH = posEstimator.est.eph;
|
||||
navEPV = posEstimator.est.epv;
|
||||
|
|
@ -1054,8 +996,6 @@ void initializePositionEstimator(void)
|
|||
posEstimator.est.aglAlt = 0;
|
||||
posEstimator.est.aglVel = 0;
|
||||
|
||||
posEstimator.history.index = 0;
|
||||
|
||||
posEstimator.imu.gravityCalibrationComplete = false;
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
|
|
@ -1063,9 +1003,6 @@ void initializePositionEstimator(void)
|
|||
posEstimator.est.pos.A[axis] = 0;
|
||||
posEstimator.est.vel.A[axis] = 0;
|
||||
}
|
||||
|
||||
memset(&posEstimator.history.pos[0], 0, sizeof(posEstimator.history.pos));
|
||||
memset(&posEstimator.history.vel[0], 0, sizeof(posEstimator.history.vel));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -325,7 +325,7 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint);
|
|||
bool isApproachingLastWaypoint(void);
|
||||
float getActiveWaypointSpeed(void);
|
||||
|
||||
void updateActualHeading(int32_t newHeading);
|
||||
void updateActualHeading(bool headingValid, int32_t newHeading);
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY);
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);
|
||||
|
||||
|
|
|
|||
|
|
@ -61,10 +61,11 @@ rangefinder_t rangefinder;
|
|||
#define RANGEFINDER_DYNAMIC_FACTOR 75
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 1);
|
||||
|
||||
PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
|
||||
.rangefinder_hardware = RANGEFINDER_NONE,
|
||||
.use_median_filtering = 0,
|
||||
);
|
||||
|
||||
const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void)
|
||||
|
|
@ -293,7 +294,11 @@ bool rangefinderProcess(float cosTiltAngle)
|
|||
|
||||
if (distance >= 0) {
|
||||
rangefinder.lastValidResponseTimeMs = millis();
|
||||
rangefinder.rawAltitude = applyMedianFilter(distance);
|
||||
rangefinder.rawAltitude = distance;
|
||||
|
||||
if (rangefinderConfig()->use_median_filtering) {
|
||||
rangefinder.rawAltitude = applyMedianFilter(rangefinder.rawAltitude);
|
||||
}
|
||||
}
|
||||
else if (distance == RANGEFINDER_OUT_OF_RANGE) {
|
||||
rangefinder.lastValidResponseTimeMs = millis();
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@ typedef enum {
|
|||
|
||||
typedef struct rangefinderConfig_s {
|
||||
uint8_t rangefinder_hardware;
|
||||
uint8_t use_median_filtering;
|
||||
} rangefinderConfig_t;
|
||||
|
||||
PG_DECLARE(rangefinderConfig_t, rangefinderConfig);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue