diff --git a/docs/Settings.md b/docs/Settings.md index c919327194..b8e47c57f1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -258,7 +258,7 @@ Inertia force compensation method when gps is avaliable, VELNED use the acclerat | Default | Min | Max | | --- | --- | --- | -| VELNED | | | +| ADAPTIVE | | | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 261bf14a56..37fb701af3 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1452,7 +1452,7 @@ groups: type: bool - name: ahrs_inertia_comp_method description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop" - default_value: VELNED + default_value: ADAPTIVE field: inertia_comp_method table: imu_inertia_comp_method diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index c9d0f0aacb..43673e790d 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -719,6 +719,7 @@ static void imuCalculateEstimatedAttitude(float dT) } if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) { + //pick the best centrifugal acceleration between velned and turnrate fpVector3_t compansatedGravityBF_velned; vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); @@ -726,21 +727,17 @@ static void imuCalculateEstimatedAttitude(float dT) fpVector3_t compansatedGravityBF_turnrate; vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); - if (velned_magnitude > turnrate_magnitude) - { - compansatedGravityBF = compansatedGravityBF_turnrate; - } - else - { - compansatedGravityBF = compansatedGravityBF_velned; - } + + compansatedGravityBF = velned_magnitude > turnrate_magnitude? compansatedGravityBF_turnrate:compansatedGravityBF_velned; } - else if (imuConfig()->inertia_comp_method == COMPMETHOD_VELNED && isGPSTrustworthy()) + else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy()) { + //velned centrifugal force compensation, quad will use this method vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); } else if (STATE(AIRPLANE)) { + //turnrate centrifugal force compensation vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); } else