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

minor changes on ahrs

Browse source
This commit is contained in:
shota 2023-10-09 17:02:10 +09:00
parent aff0034b43
commit e63fa473a0
3 changed files with 8 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

2
docs/Settings.md
View file

@ -258,7 +258,7 @@ Inertia force compensation method when gps is avaliable, VELNED use the acclerat
| Default | Min | Max | | Default | Min | Max |
| --- | --- | --- | | --- | --- | --- |
| VELNED | | | | ADAPTIVE | | |
--- ---

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

@ -1452,7 +1452,7 @@ groups:
type: bool type: bool
- name: ahrs_inertia_comp_method - 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" 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 field: inertia_comp_method
table: imu_inertia_comp_method table: imu_inertia_comp_method

15
src/main/flight/imu.c
View file

@ -719,6 +719,7 @@ static void imuCalculateEstimatedAttitude(float dT)
} }
if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE)) if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE))
{ {
//pick the best centrifugal acceleration between velned and turnrate
fpVector3_t compansatedGravityBF_velned; fpVector3_t compansatedGravityBF_velned;
vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS); float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
@ -726,21 +727,17 @@ static void imuCalculateEstimatedAttitude(float dT)
fpVector3_t compansatedGravityBF_turnrate; fpVector3_t compansatedGravityBF_turnrate;
vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS); float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
if (velned_magnitude > turnrate_magnitude)
{ compansatedGravityBF = velned_magnitude > turnrate_magnitude? compansatedGravityBF_turnrate:compansatedGravityBF_velned;
compansatedGravityBF = compansatedGravityBF_turnrate;
} }
else else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy())
{
compansatedGravityBF = compansatedGravityBF_velned;
}
}
else if (imuConfig()->inertia_comp_method == COMPMETHOD_VELNED && isGPSTrustworthy())
{ {
//velned centrifugal force compensation, quad will use this method
vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned); vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
} }
else if (STATE(AIRPLANE)) else if (STATE(AIRPLANE))
{ {
//turnrate centrifugal force compensation
vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate); vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
} }
else else