Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 14:25:16 +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 |
| --- | --- | --- |
| VELNED | | |
| ADAPTIVE | | |
---

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

@ -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

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))
{
//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;
compansatedGravityBF = velned_magnitude > turnrate_magnitude? compansatedGravityBF_turnrate:compansatedGravityBF_velned;
}
else
{
compansatedGravityBF = 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