diff --git a/docs/Settings.md b/docs/Settings.md index 79278af68f..db8180ed43 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1872,16 +1872,6 @@ _// TODO_ --- -### inav_use_gps_velned - -Defined if INAV should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF INAV will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. - -| Default | Min | Max | -| --- | --- | --- | -| ON | OFF | ON | - ---- - ### inav_w_acc_bias Weight for accelerometer drift estimation diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2c3f40dfe4..c076fa88c4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1420,7 +1420,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_p * 100); // 2 inav_w_xy_gps_p float as value * 100 sbufWriteU16(dst, positionEstimationConfig()->w_xy_gps_v * 100); // 2 inav_w_xy_gps_v float as value * 100 sbufWriteU8(dst, gpsConfigMutable()->gpsMinSats); // 1 - sbufWriteU8(dst, positionEstimationConfig()->use_gps_velned); // 1 inav_use_gps_velned ON/OFF + sbufWriteU8(dst, 1); // 1 inav_use_gps_velned ON/OFF break; @@ -2489,7 +2489,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) positionEstimationConfigMutable()->w_xy_gps_p = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f); positionEstimationConfigMutable()->w_xy_gps_v = constrainf(sbufReadU16(src) / 100.0f, 0.0f, 10.0f); gpsConfigMutable()->gpsMinSats = constrain(sbufReadU8(src), 5, 10); - positionEstimationConfigMutable()->use_gps_velned = constrain(sbufReadU8(src), 0, 1); + sbufReadU8(src); // was positionEstimationConfigMutable()->use_gps_velned } else return MSP_RESULT_ERROR; break; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0afd4c8bae..cf37f208a2 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2288,11 +2288,6 @@ groups: field: gravity_calibration_tolerance min: 0 max: 255 - - name: inav_use_gps_velned - description: "Defined if INAV should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF INAV will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance." - default_value: ON - field: use_gps_velned - type: bool - name: inav_use_gps_no_baro field: use_gps_no_baro type: bool diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 84e5390d43..0e63998f88 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -234,7 +234,6 @@ typedef struct positionEstimationConfig_s { uint8_t reset_altitude_type; // from nav_reset_type_e uint8_t reset_home_type; // nav_reset_type_e uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) - uint8_t use_gps_velned; uint8_t allow_dead_reckoning; uint16_t max_surface_altitude; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 3453c75c92..19018983cc 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -56,7 +56,7 @@ navigationPosEstimator_t posEstimator; static float initialBaroAltitudeOffset = 0.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 6); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -64,7 +64,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT, .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT, .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT, // 5 cm/s/s calibration error accepted (0.5% of gravity) - .use_gps_velned = SETTING_INAV_USE_GPS_VELNED_DEFAULT, // "Disabled" is mandatory with gps_dyn_model = Pedestrian .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT, // Use GPS altitude if no baro is available on all aircrafts .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT, @@ -229,7 +228,7 @@ void onNewGPSData(void) /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */ float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f); - if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) { + if (!ARMING_FLAG(SIMULATOR_MODE_SITL) && gpsSol.flags.validVelNE) { posEstimator.gps.vel.x = gpsSol.velNED[X]; posEstimator.gps.vel.y = gpsSol.velNED[Y]; } @@ -238,7 +237,7 @@ void onNewGPSData(void) posEstimator.gps.vel.y = (posEstimator.gps.vel.y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f; } - if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelD) { + if (gpsSol.flags.validVelD) { posEstimator.gps.vel.z = -gpsSol.velNED[Z]; // NEU } else {