Skip to content

Commit bb771f9

Browse files
breadovenbreadoven
committed
CR140
1 parent 456298e commit bb771f9

File tree

6 files changed

+71
-75
lines changed

6 files changed

+71
-75
lines changed

‎docs/Settings.md‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2238,7 +2238,7 @@ Weight of barometer climb rate measurements in estimated climb rate. Setting is
22382238

22392239
| Default | Min | Max |
22402240
| --- | --- | --- |
2241-
| 0.1 | 0 | 10 |
2241+
| 0.35 | 0 | 10 |
22422242

22432243
---
22442244

@@ -2248,7 +2248,7 @@ Weight of GPS altitude measurements in estimated altitude. Setting is used on bo
22482248

22492249
| Default | Min | Max |
22502250
| --- | --- | --- |
2251-
| 0.2 | 0 | 10 |
2251+
| 0.35 | 0 | 10 |
22522252

22532253
---
22542254

@@ -2258,7 +2258,7 @@ Weight of GPS climb rate measurements in estimated climb rate. Setting is used o
22582258

22592259
| Default | Min | Max |
22602260
| --- | --- | --- |
2261-
| 0.1 | 0 | 10 |
2261+
| 0.35 | 0 | 10 |
22622262

22632263
---
22642264

‎src/main/fc/settings.yaml‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2457,19 +2457,19 @@ groups:
24572457
field: w_z_baro_v
24582458
min: 0
24592459
max: 10
2460-
default_value: 0.1
2460+
default_value: 0.35 # CR140
24612461
- name: inav_w_z_gps_p
24622462
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
24632463
field: w_z_gps_p
24642464
min: 0
24652465
max: 10
2466-
default_value: 0.2
2466+
default_value: 0.35 # CR140
24672467
- name: inav_w_z_gps_v
24682468
description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
24692469
field: w_z_gps_v
24702470
min: 0
24712471
max: 10
2472-
default_value: 0.1
2472+
default_value: 0.35 # CR140
24732473
- name: inav_w_xy_gps_p
24742474
description: "Weight of GPS coordinates in estimated UAV position and speed."
24752475
default_value: 1.0

‎src/main/navigation/navigation_fixedwing.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
151151
// ****************** TEST FILTER VERT VEL
152152
// static pt1Filter_t velz2FilterState;
153153
// currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, 1.0f, US2S(deltaMicros));
154-
DEBUG_SET(DEBUG_ALWAYS, 3, currentClimbRate);
154+
// DEBUG_SET(DEBUG_ALWAYS, 3, currentClimbRate);
155155
// ******************
156156

157157
// // ****************** TEST USING POSITION AGAIN
@@ -170,7 +170,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
170170

171171
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
172172
// DEBUG_SET(DEBUG_ALWAYS, 3, posControl.pids.fw_alt.feedForward);
173-
DEBUG_SET(DEBUG_ALWAYS, 6, targetPitchAngle); // CR133
173+
// DEBUG_SET(DEBUG_ALWAYS, 6, targetPitchAngle); // CR133
174174

175175
// Apply low-pass filter to prevent rapid correction
176176
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));

‎src/main/navigation/navigation_pos_estimator.c‎

Lines changed: 58 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -374,15 +374,8 @@ static void updateIMUEstimationWeight(const float dt)
374374
// DEBUG_VIBE[0-3] are used in IMU
375375
DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
376376
}
377-
// CR137
377+
// CR140
378378
// float navGetAccelerometerWeight(void) // pointless
379-
// {
380-
// const float accWeightScaled = posEstimator.imu.accWeightFactor;
381-
// DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
382-
383-
// return accWeightScaled;
384-
// }
385-
// CR137
386379
static void updateIMUTopic(timeUs_t currentTimeUs)
387380
{
388381
const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime);
@@ -398,28 +391,22 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
398391
else {
399392
/* Update acceleration weight based on vibration levels and clipping */
400393
updateIMUEstimationWeight(dt);
401-
402-
fpVector3_t accelBF;
394+
// CR140
395+
fpVector3_t accelReading;
403396

404397
/* Read acceleration data in body frame */
405-
accelBF.x = imuMeasuredAccelBF.x;
406-
accelBF.y = imuMeasuredAccelBF.y;
407-
accelBF.z = imuMeasuredAccelBF.z;
408-
409-
/* Correct accelerometer bias */
410-
accelBF.x -= posEstimator.imu.accelBias.x;
411-
accelBF.y -= posEstimator.imu.accelBias.y;
412-
accelBF.z -= posEstimator.imu.accelBias.z;
398+
accelReading.x = imuMeasuredAccelBF.x;
399+
accelReading.y = imuMeasuredAccelBF.y;
400+
accelReading.z = imuMeasuredAccelBF.z;
413401

414402
/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
415-
imuTransformVectorBodyToEarth(&accelBF);
403+
imuTransformVectorBodyToEarth(&accelReading);
416404

417-
/* Read acceleration data in NEU frame from IMU */
418-
posEstimator.imu.accelNEU.x = accelBF.x;
419-
posEstimator.imu.accelNEU.y = accelBF.y;
420-
posEstimator.imu.accelNEU.z = accelBF.z;
405+
posEstimator.imu.accelNEU.x = accelReading.x + posEstimator.imu.accelBias.x;
406+
posEstimator.imu.accelNEU.y = accelReading.y + posEstimator.imu.accelBias.y;
407+
posEstimator.imu.accelNEU.z = accelReading.z + posEstimator.imu.accelBias.z;
408+
// CR140
421409

422-
// DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.imu.calibratedGravityCMSS * 1000);
423410
/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
424411
if (gyroConfig()->init_gyro_cal_enabled) {
425412
if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
@@ -615,27 +602,31 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
615602
// We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
616603
bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
617604
(((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
618-
((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
605+
(posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt)); // CR140
619606

620607
// Altitude
621608
const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
622609
const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
623-
const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;
610+
const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; // CR140
611+
const float w_z_baro_v = positionEstimationConfig()->w_z_baro_v;
624612

625613
ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
626-
// ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt; // CR137
627-
ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt;
614+
ctx->estVelCorr.z += baroVelZResidual * w_z_baro_v * ctx->dt; // CR140 use same w_z_baro for p and v
628615

629616
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, w_z_baro_p);
630617

631-
// Accelerometer bias
618+
// Accelerometer bias // CR140 use baroVelZResidual instead or both
619+
DEBUG_SET(DEBUG_ALWAYS, 3, baroAltResidual);
620+
DEBUG_SET(DEBUG_ALWAYS, 4, baroVelZResidual);
621+
DEBUG_SET(DEBUG_ALWAYS, 2, 0);
632622
if (!isAirCushionEffectDetected) {
633-
ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
623+
ctx->accBiasCorr.z += (baroAltResidual * sq(w_z_baro_p) + baroVelZResidual * sq(w_z_baro_v)); // CR140
634624
}
625+
DEBUG_SET(DEBUG_ALWAYS, 2, posEstimator.baro.alt);
635626

636627
correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
637628
}
638-
DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.baro.baroAltRate);
629+
639630
if (ctx->newFlags & EST_GPS_Z_VALID && wGps) {
640631
// Reset current estimate to GPS altitude if estimate not valid
641632
if (!(ctx->newFlags & EST_Z_VALID)) {
@@ -648,21 +639,27 @@ DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.baro.baroAltRate);
648639
const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
649640
const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
650641
const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p;
642+
const float w_z_gps_v = positionEstimationConfig()->w_z_gps_v;
651643

652644
ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
653-
// ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt; // CR137
654-
ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;
645+
ctx->estVelCorr.z += gpsVelZResidual * w_z_gps_v * ctx->dt; // CR140 use same w_z_gps for p and v
655646

656647
ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, fabsf(gpsAltResidual)), w_z_gps_p);
657648

658-
// Accelerometer bias
659-
ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
649+
// Accelerometer bias // CR140 use gpsVelZResidual instead or both
650+
ctx->accBiasCorr.z += (gpsAltResidual * sq(w_z_gps_p) + gpsVelZResidual * sq(w_z_gps_v)); // CR140
660651
}
661652

662653
correctOK = ARMING_FLAG(WAS_EVER_ARMED); // No correction until first armed
663654
}
664-
// DEBUG_SET(DEBUG_ALWAYS, 0, wBaro * 100);
665-
// DEBUG_SET(DEBUG_ALWAYS, 1, wGps * 100);
655+
// CR140
656+
// Double corrections if only a single sensor used
657+
if (wGps == 0 || wBaro == 0) {
658+
ctx->estPosCorr.z *= 2.0f;
659+
ctx->estVelCorr.z *= 2.0f;
660+
ctx->accBiasCorr.z *= 2.0f;
661+
}
662+
// CR140
666663
return correctOK;
667664
}
668665

@@ -694,17 +691,13 @@ static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
694691
ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
695692
ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;
696693

697-
// Velocity from coordinates - CR137
698-
// ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
699-
// ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;
700-
701694
// Velocity from direct measurement
702695
ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
703696
ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;
704697

705-
// Accelerometer bias
706-
ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
707-
ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);
698+
// Accelerometer bias // CR140
699+
ctx->accBiasCorr.x += (gpsPosXResidual * sq(w_xy_gps_p) + gpsVelXResidual * sq(w_xy_gps_v));
700+
ctx->accBiasCorr.y += (gpsPosYResidual * sq(w_xy_gps_p) + gpsVelYResidual * sq(w_xy_gps_v));
708701

709702
/* Adjust EPH */
710703
ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
@@ -751,7 +744,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
751744
return;
752745
}
753746

754-
/* Calculate new EPH and EPV for the case we didn't update postion */
747+
/* Calculate new EPH and EPV for the case we didn't update position */
755748
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
756749
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
757750
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
@@ -783,10 +776,8 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
783776
}
784777

785778
// Boost the corrections based on accWeight
786-
// const float accWeight = navGetAccelerometerWeight(); // CR137
787-
const float accWeight = posEstimator.imu.accWeightFactor;
788-
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f/accWeight);
789-
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f/accWeight);
779+
vectorScale(&ctx.estPosCorr, &ctx.estPosCorr, 1.0f / posEstimator.imu.accWeightFactor); // CR140
780+
vectorScale(&ctx.estVelCorr, &ctx.estVelCorr, 1.0f / posEstimator.imu.accWeightFactor); // CR140
790781

791782
// Apply corrections
792783
vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
@@ -795,18 +786,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
795786
/* Correct accelerometer bias */
796787
const float w_acc_bias = positionEstimationConfig()->w_acc_bias;
797788
if (w_acc_bias > 0.0f) {
798-
const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
799-
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
800-
/* transform error vector from NEU frame to body frame */
801-
imuTransformVectorEarthToBody(&ctx.accBiasCorr);
802-
803-
/* Correct accel bias */
804-
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
805-
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
806-
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
807-
}
808-
}
789+
// CR140
790+
ctx.accBiasCorr.x = constrainf(ctx.accBiasCorr.x, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
791+
ctx.accBiasCorr.y = constrainf(ctx.accBiasCorr.y, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
792+
ctx.accBiasCorr.z = constrainf(ctx.accBiasCorr.z, -INAV_ACC_BIAS_ACCEPTANCE_VALUE, INAV_ACC_BIAS_ACCEPTANCE_VALUE);
809793

794+
/* Correct accel bias */
795+
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
796+
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
797+
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
798+
}
799+
// CR140
810800
/* Update ground course */
811801
estimationCalculateGroundCourse(currentTimeUs);
812802

@@ -816,6 +806,14 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
816806

817807
// Keep flags for further usage
818808
posEstimator.flags = ctx.newFlags;
809+
810+
811+
DEBUG_SET(DEBUG_ALWAYS, 0, posEstimator.imu.accelBias.z * 1000);
812+
DEBUG_SET(DEBUG_ALWAYS, 1, ctx.accBiasCorr.z);
813+
// DEBUG_SET(DEBUG_ALWAYS, 4, ctx.estPosCorr.z * 1000);
814+
// DEBUG_SET(DEBUG_ALWAYS, 3, ctx.estVelCorr.z * 1000);
815+
// DEBUG_SET(DEBUG_ALWAYS, 4, posEstimator.imu.accWeightFactor * 100);
816+
DEBUG_SET(DEBUG_ALWAYS, 5, posEstimator.baro.baroAltRate);
819817
}
820818

821819
/**

‎src/main/navigation/navigation_pos_estimator_agl.c‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -147,11 +147,9 @@ void estimationCalculateAGL(estimationContext_t * ctx)
147147
}
148148

149149
// Update estimate
150-
// const float accWeight = navGetAccelerometerWeight(); // CR137
151-
const float accWeight = posEstimator.imu.accWeightFactor;
152150
posEstimator.est.aglAlt += posEstimator.est.aglVel * ctx->dt;
153-
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
154-
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
151+
posEstimator.est.aglAlt += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * posEstimator.imu.accWeightFactor; // CR140
152+
posEstimator.est.aglVel += posEstimator.imu.accelNEU.z * ctx->dt * sq(posEstimator.imu.accWeightFactor); // CR140
155153

156154
// Apply correction
157155
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {

‎src/main/sensors/sensors.c‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,12 +50,12 @@ float applySensorTempCompensation(int16_t sensorTemp, float sensorMeasurement, s
5050
if (!setting) {
5151
return 0.0f;
5252
}
53-
DEBUG_SET(DEBUG_ALWAYS, 1, sensorTemp);
54-
DEBUG_SET(DEBUG_ALWAYS, 0, sensor_comp_data[sensorType].correctionFactor * 100);
53+
// DEBUG_SET(DEBUG_ALWAYS, 1, sensorTemp);
54+
// DEBUG_SET(DEBUG_ALWAYS, 0, sensor_comp_data[sensorType].correctionFactor * 100);
5555
// DEBUG_SET(DEBUG_ALWAYS, 5, sensorMeasurement);
5656
if (sensor_comp_data[sensorType].calibrationState == SENSOR_TEMP_CAL_COMPLETE) {
5757
float tempCal = sensor_comp_data[sensorType].correctionFactor * CENTIDEGREES_TO_DEGREES(sensor_comp_data[sensorType].referenceTemp - sensorTemp);
58-
DEBUG_SET(DEBUG_ALWAYS, 2, tempCal);
58+
// DEBUG_SET(DEBUG_ALWAYS, 2, tempCal);
5959
return tempCal;
6060
// return sensor_comp_data[sensorType].correctionFactor * CENTIDEGREES_TO_DEGREES(sensor_comp_data[sensorType].referenceTemp - sensorTemp);
6161
}

0 commit comments

Comments
 (0)