mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 08:45:31 +03:00
Merge pull request #9387 from shota3527/sh_ahrs
Yaw/Altitude estimation sensor fusion
This commit is contained in:
Paweł Spychalski
GitHub
commit
28d728c483
19 changed files with 197 additions and 148 deletions
|
|
@ -242,6 +242,16 @@ Inertial Measurement Unit KP Gain for compass measurements
|
|||
|
||||
---
|
||||
|
||||
### ahrs_gps_yaw_weight
|
||||
|
||||
Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 100 | 0 | 500 |
|
||||
|
||||
---
|
||||
|
||||
### ahrs_gps_yaw_windcomp
|
||||
|
||||
Wind compensation in heading estimation from gps groundcourse(fixed wing only)
|
||||
|
|
@ -1474,11 +1484,11 @@ Enable automatic configuration of UBlox GPS receivers.
|
|||
|
||||
### gps_dyn_model
|
||||
|
||||
GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying.
|
||||
GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| AIR_1G | | |
|
||||
| AIR_2G | | |
|
||||
|
||||
---
|
||||
|
||||
|
|
@ -1912,33 +1922,23 @@ Decay coefficient for estimated velocity when GPS reference for position is lost
|
|||
|
||||
---
|
||||
|
||||
### inav_w_xyz_acc_p
|
||||
|
||||
_// TODO_
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 1.0 | 0 | 1 |
|
||||
|
||||
---
|
||||
|
||||
### inav_w_z_baro_p
|
||||
|
||||
Weight of barometer measurements in estimated altitude and climb rate
|
||||
Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0.35 | 0 | 10 |
|
||||
| 0.4 | 0 | 10 |
|
||||
|
||||
---
|
||||
|
||||
### inav_w_z_gps_p
|
||||
|
||||
Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes
|
||||
Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
|
||||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0.2 | 0 | 10 |
|
||||
| 0.4 | 0 | 10 |
|
||||
|
||||
---
|
||||
|
||||
|
|
@ -1948,7 +1948,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
|
|||
|
||||
| Default | Min | Max |
|
||||
| --- | --- | --- |
|
||||
| 0.1 | 0 | 10 |
|
||||
| 0.8 | 0 | 10 |
|
||||
|
||||
---
|
||||
|
||||
|
|
|
|||
|
|
@ -310,6 +310,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
|
|||
{"accSmooth", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"accSmooth", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"accSmooth", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"accVib", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ACC},
|
||||
{"attitude", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
|
||||
{"attitude", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
|
||||
{"attitude", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ATTITUDE},
|
||||
|
|
@ -487,6 +488,7 @@ typedef struct blackboxMainState_s {
|
|||
int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
|
||||
|
||||
int16_t accADC[XYZ_AXIS_COUNT];
|
||||
int16_t accVib;
|
||||
int16_t attitude[XYZ_AXIS_COUNT];
|
||||
int32_t debug[DEBUG32_VALUE_COUNT];
|
||||
int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
|
|
@ -917,6 +919,7 @@ static void writeIntraframe(void)
|
|||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
|
||||
blackboxWriteSigned16VBArray(blackboxCurrent->accADC, XYZ_AXIS_COUNT);
|
||||
blackboxWriteUnsignedVB(blackboxCurrent->accVib);
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
|
||||
|
|
@ -1182,6 +1185,7 @@ static void writeInterframe(void)
|
|||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ACC)) {
|
||||
blackboxWriteArrayUsingAveragePredictor16(offsetof(blackboxMainState_t, accADC), XYZ_AXIS_COUNT);
|
||||
blackboxWriteSignedVB(blackboxCurrent->accVib - blackboxLast->accVib);
|
||||
}
|
||||
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_ATTITUDE)) {
|
||||
|
|
@ -1601,6 +1605,7 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
|
||||
}
|
||||
}
|
||||
blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
|
||||
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ static bool fakeMagInit(magDev_t *magDev)
|
|||
{
|
||||
UNUSED(magDev);
|
||||
// initially point north
|
||||
fakeMagData[X] = 4096;
|
||||
fakeMagData[X] = 1024;
|
||||
fakeMagData[Y] = 0;
|
||||
fakeMagData[Z] = 0;
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -478,7 +478,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
|
|||
}
|
||||
for (int i = 0; i < 3; i++) {
|
||||
#ifdef USE_MAG
|
||||
sbufWriteU16(dst, mag.magADC[i]);
|
||||
sbufWriteU16(dst, lrintf(mag.magADC[i]));
|
||||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -50,7 +50,7 @@ tables:
|
|||
values: ["AUTO", "EGNOS", "WAAS", "MSAS", "GAGAN", "SPAN", "NONE"]
|
||||
enum: sbasMode_e
|
||||
- name: gps_dyn_model
|
||||
values: ["PEDESTRIAN", "AIR_1G", "AIR_4G"]
|
||||
values: ["PEDESTRIAN","AUTOMOTIVE", "AIR_1G", "AIR_2G", "AIR_4G"]
|
||||
enum: gpsDynModel_e
|
||||
- name: reset_type
|
||||
values: ["NEVER", "FIRST_ARM", "EACH_ARM"]
|
||||
|
|
@ -1463,6 +1463,12 @@ groups:
|
|||
default_value: ADAPTIVE
|
||||
field: inertia_comp_method
|
||||
table: imu_inertia_comp_method
|
||||
- name: ahrs_gps_yaw_weight
|
||||
description: "Arhs gps yaw weight when mag is avaliable, 0 means no gps yaw, 100 means equal weight as compass"
|
||||
default_value: 100
|
||||
field: gps_yaw_weight
|
||||
min: 0
|
||||
max: 500
|
||||
|
||||
- name: PG_ARMING_CONFIG
|
||||
type: armingConfig_t
|
||||
|
|
@ -1608,8 +1614,8 @@ groups:
|
|||
table: gps_sbas_mode
|
||||
type: uint8_t
|
||||
- name: gps_dyn_model
|
||||
description: "GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying."
|
||||
default_value: "AIR_1G"
|
||||
description: "GPS navigation model: Pedestrian, Automotive, Air<1g, Air<2g, Air<4g. Default is AIR_2G. Use pedestrian/Automotive with caution, can cause flyaways with fast flying."
|
||||
default_value: "AIR_2G"
|
||||
field: dynModel
|
||||
table: gps_dyn_model
|
||||
type: uint8_t
|
||||
|
|
@ -2322,23 +2328,23 @@ groups:
|
|||
max: 100
|
||||
default_value: 2.0
|
||||
- name: inav_w_z_baro_p
|
||||
description: "Weight of barometer measurements in estimated altitude and climb rate"
|
||||
description: "Weight of barometer measurements in estimated altitude and climb rate. Setting is used on both airplanes and multirotors."
|
||||
field: w_z_baro_p
|
||||
min: 0
|
||||
max: 10
|
||||
default_value: 0.35
|
||||
default_value: 0.4
|
||||
- name: inav_w_z_gps_p
|
||||
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes"
|
||||
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
|
||||
field: w_z_gps_p
|
||||
min: 0
|
||||
max: 10
|
||||
default_value: 0.2
|
||||
default_value: 0.4
|
||||
- name: inav_w_z_gps_v
|
||||
description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
|
||||
field: w_z_gps_v
|
||||
min: 0
|
||||
max: 10
|
||||
default_value: 0.1
|
||||
default_value: 0.8
|
||||
- name: inav_w_xy_gps_p
|
||||
description: "Weight of GPS coordinates in estimated UAV position and speed."
|
||||
default_value: 1.0
|
||||
|
|
@ -2363,11 +2369,6 @@ groups:
|
|||
field: w_xy_res_v
|
||||
min: 0
|
||||
max: 10
|
||||
- name: inav_w_xyz_acc_p
|
||||
field: w_xyz_acc_p
|
||||
min: 0
|
||||
max: 1
|
||||
default_value: 1.0
|
||||
- name: inav_w_acc_bias
|
||||
description: "Weight for accelerometer drift estimation"
|
||||
default_value: 0.01
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@
|
|||
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/mixer_profile.h"
|
||||
#include "flight/pid.h"
|
||||
#if defined(USE_WIND_ESTIMATOR)
|
||||
#include "flight/wind_estimator.h"
|
||||
|
|
@ -77,6 +78,9 @@
|
|||
|
||||
#define SPIN_RATE_LIMIT 20
|
||||
#define MAX_ACC_NEARNESS 0.2 // 20% or G error soft-accepted (0.8-1.2G)
|
||||
#define MAX_MAG_NEARNESS 0.25 // 25% or magnetic field error soft-accepted (0.75-1.25)
|
||||
#define COS10DEG 0.985f
|
||||
#define COS20DEG 0.940f
|
||||
#define IMU_ROTATION_LPF 3 // Hz
|
||||
FASTRAM fpVector3_t imuMeasuredAccelBF;
|
||||
FASTRAM fpVector3_t imuMeasuredRotationBF;
|
||||
|
|
@ -111,6 +115,8 @@ FASTRAM bool gpsHeadingInitialized;
|
|||
|
||||
FASTRAM bool imuUpdated = false;
|
||||
|
||||
static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF);
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
|
||||
|
||||
PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
|
||||
|
|
@ -122,7 +128,8 @@ PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
|
|||
.acc_ignore_rate = SETTING_AHRS_ACC_IGNORE_RATE_DEFAULT,
|
||||
.acc_ignore_slope = SETTING_AHRS_ACC_IGNORE_SLOPE_DEFAULT,
|
||||
.gps_yaw_windcomp = SETTING_AHRS_GPS_YAW_WINDCOMP_DEFAULT,
|
||||
.inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT
|
||||
.inertia_comp_method = SETTING_AHRS_INERTIA_COMP_METHOD_DEFAULT,
|
||||
.gps_yaw_weight = SETTING_AHRS_GPS_YAW_WEIGHT_DEFAULT
|
||||
);
|
||||
|
||||
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
||||
|
|
@ -326,6 +333,15 @@ bool isGPSTrustworthy(void)
|
|||
return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6;
|
||||
}
|
||||
|
||||
static float imuCalculateMcCogWeight(void)
|
||||
{
|
||||
float wCoG = imuCalculateAccelerometerWeightNearness(&imuMeasuredAccelBF);
|
||||
float rotRateMagnitude = fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBFFiltered));
|
||||
const float rateSlopeMax = DEGREES_TO_RADIANS((imuConfig()->acc_ignore_rate)) * 4.0f;
|
||||
wCoG *= scaleRangef(constrainf(rotRateMagnitude, 0.0f, rateSlopeMax), 0.0f, rateSlopeMax, 1.0f, 0.0f);
|
||||
return wCoG;
|
||||
}
|
||||
|
||||
static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVector3_t * accBF, const fpVector3_t * magBF, bool useCOG, float courseOverGround, float accWScaler, float magWScaler)
|
||||
{
|
||||
STATIC_FASTRAM fpVector3_t vGyroDriftEstimate = { 0 };
|
||||
|
|
@ -339,11 +355,15 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
|
|||
/* Step 1: Yaw correction */
|
||||
// Use measured magnetic field vector
|
||||
if (magBF || useCOG) {
|
||||
static const fpVector3_t vForward = { .v = { 1.0f, 0.0f, 0.0f } };
|
||||
float wMag = 1.0f;
|
||||
float wCoG = 1.0f;
|
||||
if(magBF){wCoG *= imuConfig()->gps_yaw_weight / 100.0f;}
|
||||
|
||||
fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
|
||||
fpVector3_t vMagErr = { .v = { 0.0f, 0.0f, 0.0f } };
|
||||
fpVector3_t vCoGErr = { .v = { 0.0f, 0.0f, 0.0f } };
|
||||
|
||||
if (magBF && vectorNormSquared(magBF) > 0.01f) {
|
||||
wMag *= bellCurve((fast_fsqrtf(vectorNormSquared(magBF)) - 1024.0f) / 1024.0f, MAX_MAG_NEARNESS);
|
||||
fpVector3_t vMag;
|
||||
|
||||
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
|
||||
|
|
@ -369,13 +389,20 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
|
|||
|
||||
// Reference mag field vector heading is Magnetic North in EF. We compute that by rotating True North vector by declination and assuming Z-component is zero
|
||||
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
|
||||
vectorCrossProduct(&vErr, &vMag, &vCorrectedMagNorth);
|
||||
vectorCrossProduct(&vMagErr, &vMag, &vCorrectedMagNorth);
|
||||
|
||||
// Rotate error back into body frame
|
||||
quaternionRotateVector(&vErr, &vErr, &orientation);
|
||||
quaternionRotateVector(&vMagErr, &vMagErr, &orientation);
|
||||
}
|
||||
}
|
||||
else if (useCOG) {
|
||||
if (useCOG) {
|
||||
fpVector3_t vForward = { .v = { 0.0f, 0.0f, 0.0f } };
|
||||
//vForward as trust vector
|
||||
if (STATE(MULTIROTOR) && (!isMixerTransitionMixing)){
|
||||
vForward.z = 1.0f;
|
||||
}else{
|
||||
vForward.x = 1.0f;
|
||||
}
|
||||
fpVector3_t vHeadingEF;
|
||||
|
||||
// Use raw heading error (from GPS or whatever else)
|
||||
|
|
@ -386,6 +413,7 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
|
|||
// (Rxx; Ryx) - measured (estimated) heading vector (EF)
|
||||
// (-cos(COG), sin(COG)) - reference heading vector (EF)
|
||||
|
||||
float airSpeed = gpsSol.groundSpeed;
|
||||
// Compute heading vector in EF from scalar CoG,x axis of accelerometer is pointing backwards.
|
||||
fpVector3_t vCoG = { .v = { -cos_approx(courseOverGround), sin_approx(courseOverGround), 0.0f } };
|
||||
#if defined(USE_WIND_ESTIMATOR)
|
||||
|
|
@ -395,12 +423,21 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
|
|||
vectorScale(&vCoG, &vCoG, gpsSol.groundSpeed);
|
||||
vCoG.x += getEstimatedWindSpeed(X);
|
||||
vCoG.y -= getEstimatedWindSpeed(Y);
|
||||
airSpeed = fast_fsqrtf(vectorNormSquared(&vCoG));
|
||||
vectorNormalize(&vCoG, &vCoG);
|
||||
}
|
||||
#endif
|
||||
|
||||
wCoG *= scaleRangef(constrainf((airSpeed+gpsSol.groundSpeed)/2, 400, 1000), 400, 1000, 0.0f, 1.0f);
|
||||
// Rotate Forward vector from BF to EF - will yield Heading vector in Earth frame
|
||||
quaternionRotateVectorInv(&vHeadingEF, &vForward, &orientation);
|
||||
|
||||
if (STATE(MULTIROTOR)){
|
||||
//when multicopter`s orientation or speed is changing rapidly. less weight on gps heading
|
||||
wCoG *= imuCalculateMcCogWeight();
|
||||
//scale accroading to multirotor`s tilt angle
|
||||
wCoG *= scaleRangef(constrainf(vHeadingEF.z, COS20DEG, COS10DEG), COS20DEG, COS10DEG, 1.0f, 0.0f);
|
||||
//for inverted flying, wCoG is lowered by imuCalculateMcCogWeight no additional processing needed
|
||||
}
|
||||
vHeadingEF.z = 0.0f;
|
||||
|
||||
// We zeroed out vHeadingEF.z - make sure the whole vector didn't go to zero
|
||||
|
|
@ -409,13 +446,16 @@ static void imuMahonyAHRSupdate(float dt, const fpVector3_t * gyroBF, const fpVe
|
|||
vectorNormalize(&vHeadingEF, &vHeadingEF);
|
||||
|
||||
// error is cross product between reference heading and estimated heading (calculated in EF)
|
||||
vectorCrossProduct(&vErr, &vCoG, &vHeadingEF);
|
||||
vectorCrossProduct(&vCoGErr, &vCoG, &vHeadingEF);
|
||||
|
||||
// Rotate error back into body frame
|
||||
quaternionRotateVector(&vErr, &vErr, &orientation);
|
||||
quaternionRotateVector(&vCoGErr, &vCoGErr, &orientation);
|
||||
}
|
||||
}
|
||||
|
||||
fpVector3_t vErr = { .v = { 0.0f, 0.0f, 0.0f } };
|
||||
vectorScale(&vMagErr, &vMagErr, wMag);
|
||||
vectorScale(&vCoGErr, &vCoGErr, wCoG);
|
||||
vectorAdd(&vErr, &vMagErr, &vCoGErr);
|
||||
// Compute and apply integral feedback if enabled
|
||||
if (imuRuntimeConfig.dcm_ki_mag > 0.0f) {
|
||||
// Stop integrating if spinning beyond the certain limit
|
||||
|
|
@ -535,10 +575,10 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
|
|||
}
|
||||
}
|
||||
|
||||
static float imuCalculateAccelerometerWeightNearness(void)
|
||||
static float imuCalculateAccelerometerWeightNearness(fpVector3_t* accBF)
|
||||
{
|
||||
fpVector3_t accBFNorm;
|
||||
vectorScale(&accBFNorm, &compansatedGravityBF, 1.0f / GRAVITY_CMSS);
|
||||
vectorScale(&accBFNorm, accBF, 1.0f / GRAVITY_CMSS);
|
||||
const float accMagnitudeSq = vectorNormSquared(&accBFNorm);
|
||||
const float accWeight_Nearness = bellCurve(fast_fsqrtf(accMagnitudeSq) - 1.0f, MAX_ACC_NEARNESS);
|
||||
return accWeight_Nearness;
|
||||
|
|
@ -672,36 +712,29 @@ static void imuCalculateEstimatedAttitude(float dT)
|
|||
bool useMag = false;
|
||||
bool useCOG = false;
|
||||
#if defined(USE_GPS)
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
bool canUseCOG = isGPSHeadingValid();
|
||||
bool canUseCOG = isGPSHeadingValid();
|
||||
|
||||
// Prefer compass (if available)
|
||||
if (canUseMAG) {
|
||||
useMag = true;
|
||||
gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails
|
||||
}
|
||||
else if (canUseCOG) {
|
||||
if (gpsHeadingInitialized) {
|
||||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||
useCOG = true;
|
||||
}
|
||||
else {
|
||||
// Re-initialize quaternion from known Roll, Pitch and GPS heading
|
||||
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
|
||||
gpsHeadingInitialized = true;
|
||||
|
||||
// Force reset of heading hold target
|
||||
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
||||
}
|
||||
} else if (!ARMING_FLAG(ARMED)) {
|
||||
gpsHeadingInitialized = false;
|
||||
}
|
||||
// Use compass (if available)
|
||||
if (canUseMAG) {
|
||||
useMag = true;
|
||||
gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails
|
||||
}
|
||||
else {
|
||||
// Multicopters don't use GPS heading
|
||||
if (canUseMAG) {
|
||||
useMag = true;
|
||||
// Use GPS (if available)
|
||||
if (canUseCOG) {
|
||||
if (gpsHeadingInitialized) {
|
||||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||
useCOG = true;
|
||||
}
|
||||
else if (!canUseMAG) {
|
||||
// Re-initialize quaternion from known Roll, Pitch and GPS heading
|
||||
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
|
||||
gpsHeadingInitialized = true;
|
||||
|
||||
// Force reset of heading hold target
|
||||
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
||||
}
|
||||
} else if (!ARMING_FLAG(ARMED)) {
|
||||
gpsHeadingInitialized = false;
|
||||
}
|
||||
|
||||
imuCalculateFilters(dT);
|
||||
|
|
@ -751,7 +784,7 @@ static void imuCalculateEstimatedAttitude(float dT)
|
|||
}
|
||||
compansatedGravityBF = imuMeasuredAccelBF
|
||||
#endif
|
||||
float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness();
|
||||
float accWeight = imuGetPGainScaleFactor() * imuCalculateAccelerometerWeightNearness(&compansatedGravityBF);
|
||||
accWeight = accWeight * imuCalculateAccelerometerWeightRateIgnore(acc_ignore_slope_multipiler);
|
||||
const bool useAcc = (accWeight > 0.001f);
|
||||
|
||||
|
|
@ -807,6 +840,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
bool isImuReady(void)
|
||||
{
|
||||
return sensors(SENSOR_ACC) && STATE(ACCELEROMETER_CALIBRATED) && gyroIsCalibrationComplete();
|
||||
|
|
@ -814,7 +848,7 @@ bool isImuReady(void)
|
|||
|
||||
bool isImuHeadingValid(void)
|
||||
{
|
||||
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || (STATE(FIXED_WING_LEGACY) && gpsHeadingInitialized);
|
||||
return (sensors(SENSOR_MAG) && STATE(COMPASS_CALIBRATED)) || gpsHeadingInitialized;
|
||||
}
|
||||
|
||||
float calculateCosTiltAngle(void)
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@ typedef struct imuConfig_s {
|
|||
uint8_t acc_ignore_slope;
|
||||
uint8_t gps_yaw_windcomp;
|
||||
uint8_t inertia_comp_method;
|
||||
uint16_t gps_yaw_weight;
|
||||
} imuConfig_t;
|
||||
|
||||
PG_DECLARE(imuConfig_t, imuConfig);
|
||||
|
|
|
|||
|
|
@ -453,7 +453,7 @@ bool isGPSHealthy(void)
|
|||
|
||||
bool isGPSHeadingValid(void)
|
||||
{
|
||||
return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
|
||||
return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 400;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -76,7 +76,9 @@ typedef enum {
|
|||
|
||||
typedef enum {
|
||||
GPS_DYNMODEL_PEDESTRIAN = 0,
|
||||
GPS_DYNMODEL_AUTOMOTIVE,
|
||||
GPS_DYNMODEL_AIR_1G,
|
||||
GPS_DYNMODEL_AIR_2G,
|
||||
GPS_DYNMODEL_AIR_4G,
|
||||
} gpsDynModel_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@
|
|||
|
||||
#include "platform.h"
|
||||
#include "build/build_config.h"
|
||||
#include "common/log.h"
|
||||
|
||||
#if defined(USE_GPS_FAKE)
|
||||
|
||||
|
|
|
|||
|
|
@ -800,10 +800,16 @@ STATIC_PROTOTHREAD(gpsConfigure)
|
|||
case GPS_DYNMODEL_PEDESTRIAN:
|
||||
configureNAV5(UBX_DYNMODEL_PEDESTRIAN, UBX_FIXMODE_AUTO);
|
||||
break;
|
||||
case GPS_DYNMODEL_AIR_1G: // Default to this
|
||||
default:
|
||||
case GPS_DYNMODEL_AUTOMOTIVE:
|
||||
configureNAV5(UBX_DYNMODEL_AUTOMOVITE, UBX_FIXMODE_AUTO);
|
||||
break;
|
||||
case GPS_DYNMODEL_AIR_1G:
|
||||
configureNAV5(UBX_DYNMODEL_AIR_1G, UBX_FIXMODE_AUTO);
|
||||
break;
|
||||
case GPS_DYNMODEL_AIR_2G: // Default to this
|
||||
default:
|
||||
configureNAV5(UBX_DYNMODEL_AIR_2G, UBX_FIXMODE_AUTO);
|
||||
break;
|
||||
case GPS_DYNMODEL_AIR_4G:
|
||||
configureNAV5(UBX_DYNMODEL_AIR_4G, UBX_FIXMODE_AUTO);
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,9 @@ extern "C" {
|
|||
#define GPS_CAPA_INTERVAL 5000
|
||||
|
||||
#define UBX_DYNMODEL_PEDESTRIAN 3
|
||||
#define UBX_DYNMODEL_AUTOMOVITE 4
|
||||
#define UBX_DYNMODEL_AIR_1G 6
|
||||
#define UBX_DYNMODEL_AIR_2G 7
|
||||
#define UBX_DYNMODEL_AIR_4G 8
|
||||
|
||||
#define UBX_FIXMODE_2D_ONLY 1
|
||||
|
|
|
|||
|
|
@ -203,7 +203,6 @@ typedef struct positionEstimationConfig_s {
|
|||
float w_xy_res_v;
|
||||
|
||||
float w_acc_bias; // Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
|
||||
float w_xyz_acc_p;
|
||||
|
||||
float max_eph_epv; // Max estimated position error acceptable for estimation (cm)
|
||||
float baro_epv; // Baro position error
|
||||
|
|
|
|||
|
|
@ -70,8 +70,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
|
|||
|
||||
.max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
|
||||
|
||||
.w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
|
||||
|
||||
.w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
|
||||
|
||||
.w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
|
||||
|
|
@ -389,28 +387,30 @@ static bool gravityCalibrationComplete(void)
|
|||
|
||||
return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
|
||||
}
|
||||
|
||||
#define ACC_VIB_FACTOR_S 1.0f
|
||||
#define ACC_VIB_FACTOR_E 3.0f
|
||||
static void updateIMUEstimationWeight(const float dt)
|
||||
{
|
||||
bool isAccClipped = accIsClipped();
|
||||
|
||||
// If accelerometer measurement is clipped - drop the acc weight to zero
|
||||
static float acc_clip_factor = 1.0f;
|
||||
// If accelerometer measurement is clipped - drop the acc weight to 0.3
|
||||
// and gradually restore weight back to 1.0 over time
|
||||
if (isAccClipped) {
|
||||
posEstimator.imu.accWeightFactor = 0.0f;
|
||||
if (accIsClipped()) {
|
||||
acc_clip_factor = 0.5f;
|
||||
}
|
||||
else {
|
||||
const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
|
||||
posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
|
||||
acc_clip_factor = acc_clip_factor * (1.0f - relAlpha) + 1.0f * relAlpha;
|
||||
}
|
||||
|
||||
// Update accelerometer weight based on vibration levels and clipping
|
||||
float acc_vibration_factor = scaleRangef(constrainf(accGetVibrationLevel(),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E),ACC_VIB_FACTOR_S,ACC_VIB_FACTOR_E,1.0f,0.3f); // g/s
|
||||
posEstimator.imu.accWeightFactor = acc_vibration_factor * acc_clip_factor;
|
||||
// DEBUG_VIBE[0-3] are used in IMU
|
||||
DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
|
||||
}
|
||||
|
||||
float navGetAccelerometerWeight(void)
|
||||
{
|
||||
const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
|
||||
const float accWeightScaled = posEstimator.imu.accWeightFactor;
|
||||
DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
|
||||
|
||||
return accWeightScaled;
|
||||
|
|
@ -553,13 +553,12 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
|
|||
|
||||
static void estimationPredict(estimationContext_t * ctx)
|
||||
{
|
||||
const float accWeight = navGetAccelerometerWeight();
|
||||
|
||||
/* Prediction step: Z-axis */
|
||||
if ((ctx->newFlags & EST_Z_VALID)) {
|
||||
posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
|
||||
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
|
||||
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
|
||||
posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
|
||||
posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
|
||||
}
|
||||
|
||||
/* Prediction step: XY-axis */
|
||||
|
|
@ -570,10 +569,10 @@ static void estimationPredict(estimationContext_t * ctx)
|
|||
|
||||
// If heading is valid, accelNEU is valid as well. Account for acceleration
|
||||
if (navIsHeadingUsable() && navIsAccelerationUsable()) {
|
||||
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
|
||||
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
|
||||
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
|
||||
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
|
||||
posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
|
||||
posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
|
||||
posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt;
|
||||
posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -589,7 +588,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
|
|||
DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed
|
||||
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
|
||||
|
||||
if (ctx->newFlags & EST_BARO_VALID) {
|
||||
bool correctOK = false;
|
||||
|
||||
//ignore baro if difference is too big, baro is probably wrong
|
||||
const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f;
|
||||
//fade out the baro to prevent sudden jump
|
||||
const float start_epv = positionEstimationConfig()->max_eph_epv;
|
||||
const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f;
|
||||
const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f);
|
||||
//use both baro and gps
|
||||
if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) {
|
||||
timeUs_t currentTimeUs = micros();
|
||||
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
|
|
@ -599,44 +607,33 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
|
|||
}
|
||||
else {
|
||||
if (posEstimator.est.vel.z > 15) {
|
||||
if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
|
||||
posEstimator.state.isBaroGroundValid = false;
|
||||
}
|
||||
posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
|
||||
}
|
||||
else {
|
||||
posEstimator.state.baroGroundTimeout = currentTimeUs + 250000; // 0.25 sec
|
||||
}
|
||||
}
|
||||
|
||||
// We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
|
||||
// We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
|
||||
bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
|
||||
(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
|
||||
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
|
||||
|
||||
// Altitude
|
||||
const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
|
||||
ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
|
||||
ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
|
||||
|
||||
// If GPS is available - also use GPS climb rate
|
||||
if (ctx->newFlags & EST_GPS_Z_VALID) {
|
||||
// Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
|
||||
const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
|
||||
const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
|
||||
ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
|
||||
}
|
||||
ctx->estPosCorr.z += wBaro * baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
|
||||
ctx->estVelCorr.z += wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
|
||||
|
||||
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
|
||||
|
||||
// Accelerometer bias
|
||||
if (!isAirCushionEffectDetected) {
|
||||
ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
ctx->accBiasCorr.z -= wBaro * baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
|
||||
}
|
||||
|
||||
return true;
|
||||
correctOK = true;
|
||||
}
|
||||
else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
|
||||
// If baro is not available - use GPS Z for correction on a plane
|
||||
if (ctx->newFlags & EST_GPS_Z_VALID) {
|
||||
// Reset current estimate to GPS altitude if estimate not valid
|
||||
if (!(ctx->newFlags & EST_Z_VALID)) {
|
||||
ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
|
||||
|
|
@ -646,20 +643,21 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
|
|||
else {
|
||||
// Altitude
|
||||
const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
|
||||
const float gpsVelZResudual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
|
||||
|
||||
ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
|
||||
ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
|
||||
ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
|
||||
ctx->estVelCorr.z += gpsVelZResudual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
|
||||
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
|
||||
|
||||
// Accelerometer bias
|
||||
ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
|
||||
}
|
||||
|
||||
return true;
|
||||
correctOK = true;
|
||||
}
|
||||
|
||||
return false;
|
||||
return correctOK;
|
||||
}
|
||||
|
||||
static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
|
||||
|
|
@ -714,15 +712,10 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
|
|||
|
||||
static void estimationCalculateGroundCourse(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
if (STATE(GPS_FIX) && navIsHeadingUsable()) {
|
||||
static timeUs_t lastUpdateTimeUs = 0;
|
||||
|
||||
if (currentTimeUs - lastUpdateTimeUs >= HZ2US(INAV_COG_UPDATE_RATE_HZ)) { // limit update rate
|
||||
const float dt = US2S(currentTimeUs - lastUpdateTimeUs);
|
||||
uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y * dt, posEstimator.est.vel.x * dt)));
|
||||
posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
|
||||
lastUpdateTimeUs = currentTimeUs;
|
||||
}
|
||||
uint32_t groundCourse = wrap_36000(RADIANS_TO_CENTIDEGREES(atan2_approx(posEstimator.est.vel.y, posEstimator.est.vel.x)));
|
||||
posEstimator.est.cog = CENTIDEGREES_TO_DECIDEGREES(groundCourse);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -779,7 +772,10 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
|
||||
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
|
||||
}
|
||||
|
||||
// Boost the corrections based on accWeight
|
||||
const float accWeight = navGetAccelerometerWeight();
|
||||
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
|
||||
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
|
||||
// Apply corrections
|
||||
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
|
||||
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
|
||||
|
|
@ -817,13 +813,14 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
{
|
||||
static navigationTimer_t posPublishTimer;
|
||||
|
||||
/* IMU operates in decidegrees while INAV operates in deg*100
|
||||
* Use course over ground when GPS heading valid */
|
||||
int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
|
||||
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
|
||||
|
||||
/* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
|
||||
if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
|
||||
/* Publish heading update */
|
||||
/* IMU operates in decidegrees while INAV operates in deg*100
|
||||
* Use course over ground when GPS heading valid */
|
||||
int16_t cogValue = isGPSHeadingValid() ? posEstimator.est.cog : attitude.values.yaw;
|
||||
updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw), DECIDEGREES_TO_CENTIDEGREES(cogValue));
|
||||
|
||||
/* Publish position update */
|
||||
if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
|
||||
// FIXME!!!!!
|
||||
|
|
|
|||
|
|
@ -39,7 +39,6 @@
|
|||
|
||||
#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate
|
||||
#define INAV_PITOT_UPDATE_RATE 10
|
||||
#define INAV_COG_UPDATE_RATE_HZ 20 // ground course update rate
|
||||
|
||||
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
|
||||
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
|
||||
|
|
|
|||
|
|
@ -576,6 +576,7 @@ void accUpdate(void)
|
|||
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
|
||||
const float accDiff = acc.accADCf[axis] - accFloorFilt;
|
||||
acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
|
||||
acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
|
||||
}
|
||||
|
||||
// Filter acceleration
|
||||
|
|
@ -612,7 +613,7 @@ void accGetVibrationLevels(fpVector3_t *accVibeLevels)
|
|||
|
||||
float accGetVibrationLevel(void)
|
||||
{
|
||||
return fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
|
||||
return acc.accVibe;
|
||||
}
|
||||
|
||||
uint32_t accGetClipCount(void)
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@
|
|||
#define GRAVITY_CMSS 980.665f
|
||||
#define GRAVITY_MSS 9.80665f
|
||||
|
||||
#define ACC_CLIPPING_THRESHOLD_G 7.9f
|
||||
#define ACC_CLIPPING_THRESHOLD_G 15.9f
|
||||
#define ACC_VIBE_FLOOR_FILT_HZ 5.0f
|
||||
#define ACC_VIBE_FILT_HZ 2.0f
|
||||
|
||||
|
|
@ -58,6 +58,7 @@ typedef struct acc_s {
|
|||
uint32_t accTargetLooptime;
|
||||
float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
|
||||
float accVibeSq[XYZ_AXIS_COUNT];
|
||||
float accVibe;
|
||||
uint32_t accClipCount;
|
||||
bool isClipped;
|
||||
acc_extremes_t extremes[XYZ_AXIS_COUNT];
|
||||
|
|
|
|||
|
|
@ -296,9 +296,9 @@ static void exchangeData(void)
|
|||
//rfValues.m_orientationQuaternion_W = getDoubleFromResponse(response, "m-orientationQuaternion-W");
|
||||
rfValues.m_aircraftPositionX_MTR = getDoubleFromResponse(response, "m-aircraftPositionX-MTR");
|
||||
rfValues.m_aircraftPositionY_MTR = getDoubleFromResponse(response, "m-aircraftPositionY-MTR");
|
||||
//rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
|
||||
//rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
|
||||
//rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
|
||||
rfValues.m_velocityWorldU_MPS = getDoubleFromResponse(response, "m-velocityWorldU-MPS");
|
||||
rfValues.m_velocityWorldV_MPS = getDoubleFromResponse(response, "m-velocityWorldV-MPS");
|
||||
rfValues.m_velocityWorldW_MPS = getDoubleFromResponse(response, "m-velocityWorldW-MPS");
|
||||
//rfValues.m_velocityBodyU_MPS = getDoubleFromResponse(response, "m-velocityBodyU-MPS");
|
||||
//rfValues.m_velocityBodyV_MPS = getDoubleFromResponse(response, "mm-velocityBodyV-MPS");
|
||||
//rfValues.m_velocityBodyW_MPS = getDoubleFromResponse(response, "m-velocityBodyW-MPS");
|
||||
|
|
@ -332,7 +332,7 @@ static void exchangeData(void)
|
|||
float lat, lon;
|
||||
fakeCoords(FAKE_LAT, FAKE_LON, rfValues.m_aircraftPositionX_MTR, -rfValues.m_aircraftPositionY_MTR, &lat, &lon);
|
||||
|
||||
int16_t course = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
|
||||
int16_t course = (int16_t)roundf(RADIANS_TO_DECIDEGREES(atan2_approx(-rfValues.m_velocityWorldU_MPS,rfValues.m_velocityWorldV_MPS)));
|
||||
int32_t altitude = (int32_t)roundf(rfValues.m_altitudeASL_MTR * 100);
|
||||
gpsFakeSet(
|
||||
GPS_FIX_3D,
|
||||
|
|
@ -342,9 +342,9 @@ static void exchangeData(void)
|
|||
altitude,
|
||||
(int16_t)roundf(rfValues.m_groundspeed_MPS * 100),
|
||||
course,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,//(int16_t)roundf(rfValues.m_velocityWorldV_MPS * 100), //not sure about the direction
|
||||
0,//(int16_t)roundf(-rfValues.m_velocityWorldU_MPS * 100),
|
||||
0,//(int16_t)roundf(rfValues.m_velocityWorldW_MPS * 100),
|
||||
0
|
||||
);
|
||||
|
||||
|
|
@ -357,7 +357,7 @@ static void exchangeData(void)
|
|||
|
||||
const int16_t roll_inav = (int16_t)roundf(rfValues.m_roll_DEG * 10);
|
||||
const int16_t pitch_inav = (int16_t)roundf(-rfValues.m_inclination_DEG * 10);
|
||||
const int16_t yaw_inav = course;
|
||||
const int16_t yaw_inav = (int16_t)roundf(convertAzimuth(rfValues.m_azimuth_DEG) * 10);
|
||||
if (!useImu) {
|
||||
imuSetAttitudeRPY(roll_inav, pitch_inav, yaw_inav);
|
||||
imuUpdateAttitude(micros());
|
||||
|
|
@ -399,9 +399,9 @@ static void exchangeData(void)
|
|||
computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
|
||||
transformVectorEarthToBody(&north, &quat);
|
||||
fakeMagSet(
|
||||
constrainToInt16(north.x * 16000.0f),
|
||||
constrainToInt16(north.y * 16000.0f),
|
||||
constrainToInt16(north.z * 16000.0f)
|
||||
constrainToInt16(north.x * 1024.0f),
|
||||
constrainToInt16(north.y * 1024.0f),
|
||||
constrainToInt16(north.z * 1024.0f)
|
||||
);
|
||||
|
||||
free(rfValues.m_currentAircraftStatus);
|
||||
|
|
|
|||
|
|
@ -421,9 +421,9 @@ static void* listenWorker(void* arg)
|
|||
computeQuaternionFromRPY(&quat, roll_inav, pitch_inav, yaw_inav);
|
||||
transformVectorEarthToBody(&north, &quat);
|
||||
fakeMagSet(
|
||||
constrainToInt16(north.x * 16000.0f),
|
||||
constrainToInt16(north.y * 16000.0f),
|
||||
constrainToInt16(north.z * 16000.0f)
|
||||
constrainToInt16(north.x * 1024.0f),
|
||||
constrainToInt16(north.y * 1024.0f),
|
||||
constrainToInt16(north.z * 1024.0f)
|
||||
);
|
||||
|
||||
if (!initalized) {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue