Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

development of posestimator fusion

Browse source
This commit is contained in:
shota 2023-10-23 02:12:30 +09:00
parent 00b46bc0d6
commit a4c959943d
2 changed files with 4 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/fc/settings.yaml
View file

@ -2330,7 +2330,7 @@ groups:
field: w_z_gps_v
min: 0
max: 10
default_value: 0.5
default_value: 1.0
- name: inav_w_xy_gps_p
description: "Weight of GPS coordinates in estimated UAV position and speed."
default_value: 1.0

8
src/main/navigation/navigation_pos_estimator.c
View file

@ -572,9 +572,9 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
bool correctOK = false;
const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro is deference is too big
const float gpsBaroResidual = fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt); //ignore baro if difference is too big, baro is probably wrong
//use both baro and gps
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv)) {
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (gpsBaroResidual < positionEstimationConfig()->max_eph_epv * 2)) {
timeUs_t currentTimeUs = micros();
if (!ARMING_FLAG(ARMED)) {
@ -584,9 +584,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
}
else {
if (posEstimator.est.vel.z > 15) {
if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
posEstimator.state.isBaroGroundValid = false;
}
posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
}
else {
posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec