Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: sync for 4.1.0beta

This commit is contained in:
Andrew Tridgell 2021-07-22 13:41:25 +10:00 committed by Randy Mackay
parent f60c592af3
commit faec3e1a8a
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF_core_common.h
View File

@ -50,7 +50,7 @@ protected:
// fill all the common scratch variables with NaN on SITL // fill all the common scratch variables with NaN on SITL
void fill_scratch_variables(void); void fill_scratch_variables(void);
// zero part of an array // zero part of an array for index range [n1,n2]
static void zero_range(ftype *v, uint8_t n1, uint8_t n2) { static void zero_range(ftype *v, uint8_t n1, uint8_t n2) {
memset(&v[n1], 0, sizeof(ftype)*(1+(n2-n1))); memset(&v[n1], 0, sizeof(ftype)*(1+(n2-n1)));
} }
@ -58,6 +58,10 @@ protected:
#if HAL_WITH_EKF_DOUBLE #if HAL_WITH_EKF_DOUBLE
// stack frames are larger with double EKF // stack frames are larger with double EKF
#pragma GCC diagnostic error "-Wframe-larger-than=2100" #if MATH_CHECK_INDEXES
#pragma GCC diagnostic error "-Wframe-larger-than=4000"
#else
#pragma GCC diagnostic error "-Wframe-larger-than=2500"
#endif
#endif #endif

8
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -231,7 +231,7 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt); AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
AHRS[mdl_idx].gyro_bias[i] = constrain_float(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit); AHRS[mdl_idx].gyro_bias[i] = constrain_ftype(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit);
} }
} }
@ -543,13 +543,13 @@ void EKFGSF_yaw::resetEKFGSF()
run_ekf_gsf = false; run_ekf_gsf = false;
memset(&EKF, 0, sizeof(EKF)); memset(&EKF, 0, sizeof(EKF));
const ftype yaw_increment = M_2PI / (float)N_MODELS_EKFGSF; const ftype yaw_increment = M_2PI / (ftype)N_MODELS_EKFGSF;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
// evenly space initial yaw estimates in the region between +-Pi // evenly space initial yaw estimates in the region between +-Pi
EKF[mdl_idx].X[2] = -M_PI + (0.5f * yaw_increment) + ((float)mdl_idx * yaw_increment); EKF[mdl_idx].X[2] = -M_PI + (0.5f * yaw_increment) + ((ftype)mdl_idx * yaw_increment);
// All filter models start with the same weight // All filter models start with the same weight
GSF.weights[mdl_idx] = 1.0f / (float)N_MODELS_EKFGSF; GSF.weights[mdl_idx] = 1.0f / (ftype)N_MODELS_EKFGSF;
// Use half yaw interval for yaw uncertainty as that is the maximum that the best model can be away from truth // Use half yaw interval for yaw uncertainty as that is the maximum that the best model can be away from truth
GSF.yaw_variance = sq(0.5f * yaw_increment); GSF.yaw_variance = sq(0.5f * yaw_increment);