Skip to content

Commit

Permalink
CR131 & CR133
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Sep 9, 2024
1 parent f3a4894 commit c8b1276
Show file tree
Hide file tree
Showing 7 changed files with 118 additions and 45 deletions.
12 changes: 11 additions & 1 deletion docs/Settings.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2102,6 +2102,16 @@ Weight of barometer measurements in estimated altitude and climb rate. Setting i

---

### inav_w_z_baro_v

Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors.

| Default | Min | Max |
| --- | --- | --- |
| 0.1 | 0 | 10 |

---

### inav_w_z_gps_p

Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors.
Expand Down Expand Up @@ -2988,7 +2998,7 @@ Adjusts the deceleration response of fixed wing altitude control as the target a

| Default | Min | Max |
| --- | --- | --- |
| 30 | 5 | 100 |
| 40 | 5 | 100 |

---

Expand Down
14 changes: 11 additions & 3 deletions src/main/fc/settings.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2125,7 +2125,7 @@ groups:
# CR47
- name: nav_fw_pos_z_p
description: "P gain of altitude PID controller (Fixedwing)"
default_value: 40
default_value: 40 # CR133
field: bank_fw.pid[PID_POS_Z].P
min: 0
max: 255
Expand All @@ -2140,10 +2140,10 @@ groups:
default_value: 10
field: bank_fw.pid[PID_POS_Z].D
min: 0
max: 255
max: 255 # CR133 add FF
- name: nav_fw_alt_control_response
description: "Adjusts the deceleration response of fixed wing altitude control as the target altitude is approached. Decrease value to help avoid overshooting the target altitude."
default_value: 30 # CR133
default_value: 40 # CR133
field: fwAltControlResponseFactor
min: 5
max: 100
Expand Down Expand Up @@ -2412,6 +2412,14 @@ groups:
min: 0
max: 10
default_value: 0.35
# CR131
- name: inav_w_z_baro_v
description: "Weight of barometer climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors."
field: w_z_baro_v
min: 0
max: 10
default_value: 0.1
# CR131
- name: inav_w_z_gps_p
description: "Weight of GPS altitude measurements in estimated altitude. Setting is used on both airplanes and multirotors."
field: w_z_gps_p
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,7 +243,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 100
.I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 100
.D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 100
.FF = 0,
.FF = 0, // CR133
},
[PID_POS_XY] = {
.P = SETTING_NAV_FW_POS_XY_P_DEFAULT, // FW_POS_XY_P * 100
Expand Down
38 changes: 29 additions & 9 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1934,8 +1934,31 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance),
posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);

// DEBUG_SET(DEBUG_ALWAYS, 0, tmpWaypoint.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);


// targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;


// Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
// Update climb rate until within 100cm of total climb xy distance to WP
// static bool latched = false;
// if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f && posControl.wpDistance < 0.9f * posControl.wpInitialDistance) {
// float climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
// (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);

// if (latched || fabsf(climbRate) > 50.0f) {
// latched = true;
// climbRate = posControl.desiredState.vel.z + 0.1f * (climbRate - posControl.desiredState.vel.z);
// updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
// DEBUG_SET(DEBUG_ALWAYS, 1, climbRate);
// }
// } else if (latched) {
// updateClimbRateToAltitudeController(0.0f, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
// latched = false;
// }
// DEBUG_SET(DEBUG_ALWAYS, 2, latched);
// CR133
if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) {
Expand Down Expand Up @@ -4143,11 +4166,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
}

/* Consume position data */
if (posControl.flags.horizontalPositionDataConsumed)
posControl.flags.horizontalPositionDataNew = false;

if (posControl.flags.verticalPositionDataConsumed)
posControl.flags.verticalPositionDataNew = false;
if (posControl.flags.horizontalPositionDataConsumed) posControl.flags.horizontalPositionDataNew = false;
if (posControl.flags.verticalPositionDataConsumed) posControl.flags.verticalPositionDataNew = false;

//Update blackbox data
if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
Expand Down Expand Up @@ -4763,9 +4783,9 @@ void navigationUsePIDs(void)
0.0f
);
// CR133
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 50.0f, // 20 @ 30 so 35 @ 50 All for response factor of 50
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 30.0f, // 10 @ 30
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 66.7f, // 20 @ 100 so 10 @ 67
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, // 0.6 All for response factor of 50
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 30.0f, // 0.333
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 50.0f, // 0.15 to 0.3
0.0f, //(float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // poss new FF, original was 0.0f
NAV_DTERM_CUT_HZ,
0.0f
Expand Down
1 change: 1 addition & 0 deletions src/main/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -239,6 +239,7 @@ typedef struct positionEstimationConfig_s {
uint16_t max_surface_altitude;

float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_baro_v; // Weight (cutoff frequency) for barometer climb rate measurements CR131_X

float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements
float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements
Expand Down
42 changes: 34 additions & 8 deletions src/main/navigation/navigation_fixedwing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,11 +148,14 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;

// CR133
// DEBUG_SET(DEBUG_ALWAYS, 0, currentClimbRate);
// static pt1Filter_t velz2FilterState;
// currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, navConfig()->fw.wp_tracking_accuracy, US2S(deltaMicros));
// ****************** TEST FILTER VERT VEL
DEBUG_SET(DEBUG_ALWAYS, 0, currentClimbRate);
static pt1Filter_t velz2FilterState;
currentClimbRate = pt1FilterApply4(&velz2FilterState, currentClimbRate, 0.1 * navConfig()->fw.wp_tracking_accuracy, US2S(deltaMicros));
// DEBUG_SET(DEBUG_ALWAYS, 1, currentClimbRate);
// ******************

// ****************** TEST PID INTEGRATOR
// const float climbRateError = currentClimbRate - desiredClimbRate;

// static timeUs_t previousTimeMonitoringUpdate;
Expand All @@ -171,11 +174,34 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)

// // Only allow PID integrator to shrink if error is decreasing over time
// const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);

// float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, 0, climbRateError, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, pidFlags);
// ******************

// ****************** TEST USING POSITION AGAIN
// PIDS = P 10 @ 100 PID factor, I 1 @ 30, D 10 @ 50
// float desiredAltitude = 0; //posControl.desiredState.pos.z;
// static float desiredRateLimitedAltitude = 0;
// float currentAltitude = navGetCurrentActualPositionAndVelocity()->pos.z;

// if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
// posControl.desiredState.pos.z += desiredClimbRate * US2S(deltaMicros);
// desiredAltitude = 2 * posControl.desiredState.pos.z - currentAltitude;
// } else {
// static float lastPosZ = -10000000;
// if (posControl.desiredState.pos.z != lastPosZ) {
// lastPosZ = posControl.desiredState.pos.z;
// desiredRateLimitedAltitude = currentAltitude;
// }
// desiredRateLimitedAltitude += desiredClimbRate * US2S(deltaMicros);
// desiredAltitude = 2 * desiredRateLimitedAltitude - currentAltitude;
// }
// float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredAltitude, currentAltitude, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0);
// DEBUG_SET(DEBUG_ALWAYS, 1, desiredAltitude);
// **********************8

float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
// float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, 0, climbRateError, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, pidFlags);
// DEBUG_SET(DEBUG_ALWAYS, 6, targetPitchAngle); // CR133

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

Expand Down Expand Up @@ -460,9 +486,9 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
previousCrossTrackError = navCrossTrackError;
}

DEBUG_SET(DEBUG_ALWAYS, 0, navCrossTrackError);
// DEBUG_SET(DEBUG_ALWAYS, 0, navCrossTrackError);
DEBUG_SET(DEBUG_ALWAYS, 3, virtualTargetBearing);
DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate);
// DEBUG_SET(DEBUG_ALWAYS, 2, crossTrackErrorRate);

uint16_t trackingDeadband = METERS_TO_CENTIMETERS(navConfig()->fw.wp_tracking_accuracy);

Expand All @@ -474,7 +500,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
float desiredApproachSpeed = constrainf(navCrossTrackError / 3.0f, 50.0f, maxApproachSpeed);
adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed);
// DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed);
DEBUG_SET(DEBUG_ALWAYS, 5, adjustmentFactor);

/* Calculate final adjusted virtualTargetBearing */
Expand Down
54 changes: 31 additions & 23 deletions src/main/navigation/navigation_pos_estimator.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,

.w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
.w_z_baro_v = SETTING_INAV_W_Z_BARO_V_DEFAULT, // CR131_X

.w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
.w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
Expand Down Expand Up @@ -491,13 +492,15 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
/* Figure out if we have valid position data from our data sources */
uint32_t newFlags = 0;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv; // CR131_X

if ((sensors(SENSOR_GPS)
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && posControl.gpsOrigin.valid && ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
(posEstimator.gps.eph < max_eph_epv)) {
if (posEstimator.gps.epv < max_eph_epv) {
newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
}
else {
Expand All @@ -517,11 +520,11 @@ static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
newFlags |= EST_FLOW_VALID;
}

if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
if (posEstimator.est.eph < max_eph_epv) {
newFlags |= EST_XY_VALID;
}

if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
if (posEstimator.est.epv < max_eph_epv) {
newFlags |= EST_Z_VALID;
}

Expand Down Expand Up @@ -619,16 +622,17 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
// CR131
const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p; // CR131_X
// DEBUG_SET(DEBUG_ALWAYS, 2, baroVelZResidual);
ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
ctx->estPosCorr.z += baroAltResidual * w_z_baro_p * ctx->dt;
ctx->estVelCorr.z += baroAltResidual * sq(w_z_baro_p) * ctx->dt;
ctx->estVelCorr.z += baroVelZResidual * positionEstimationConfig()->w_z_baro_v * ctx->dt; // CR131_X

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

// Accelerometer bias
if (!isAirCushionEffectDetected) {
ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
ctx->accBiasCorr.z -= baroAltResidual * sq(w_z_baro_p);
}

correctOK = ARMING_FLAG(WAS_EVER_ARMED);
Expand All @@ -647,15 +651,16 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
// CR131
const float gpsAltResidual = wGps * (posEstimator.gps.pos.z - posEstimator.est.pos.z);
const float gpsVelZResidual = wGps * (posEstimator.gps.vel.z - posEstimator.est.vel.z);
const float w_z_gps_p = positionEstimationConfig()->w_z_gps_p; // CR131_X

ctx->estPosCorr.z += gpsAltResidual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
ctx->estPosCorr.z += gpsAltResidual * w_z_gps_p * ctx->dt;
ctx->estVelCorr.z += gpsAltResidual * sq(w_z_gps_p) * ctx->dt;
ctx->estVelCorr.z += gpsVelZResidual * positionEstimationConfig()->w_z_gps_v * ctx->dt;

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

// Accelerometer bias
ctx->accBiasCorr.z -= gpsAltResidual * sq(positionEstimationConfig()->w_z_gps_p);
ctx->accBiasCorr.z -= gpsAltResidual * sq(w_z_gps_p);
}

correctOK = ARMING_FLAG(WAS_EVER_ARMED);
Expand Down Expand Up @@ -740,17 +745,19 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
posEstimator.est.lastUpdateTime = currentTimeUs;

const float max_eph_epv = positionEstimationConfig()->max_eph_epv; // CR131_X

/* If IMU is not ready we can't estimate anything */
if (!isImuReady()) {
posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
posEstimator.est.eph = max_eph_epv + 0.001f;
posEstimator.est.epv = max_eph_epv + 0.001f;
posEstimator.flags = 0;
return;
}

/* Calculate new EPH and EPV for the case we didn't update postion */
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
vectorZero(&ctx.estPosCorr);
vectorZero(&ctx.estVelCorr);
Expand All @@ -770,12 +777,12 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
const bool estXYCorrectOk = estimationCalculateCorrection_XY_GPS(&ctx) || estimationCalculateCorrection_XY_FLOW(&ctx);

// If we can't apply correction or accuracy is off the charts - decay velocity to zero
if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
if (!estXYCorrectOk || ctx.newEPH > max_eph_epv) {
ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
}

if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
if (!estZCorrectOk || ctx.newEPV > max_eph_epv) {
ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
}

Expand All @@ -789,16 +796,17 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);

/* Correct accelerometer bias */
if (positionEstimationConfig()->w_acc_bias > 0.0f) {
const float w_acc_bias = positionEstimationConfig()->w_acc_bias; // CR131_X
if (w_acc_bias > 0.0f) {
const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
/* transform error vector from NEU frame to body frame */
imuTransformVectorEarthToBody(&ctx.accBiasCorr);

/* Correct accel bias */
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * w_acc_bias * ctx.dt;
posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * w_acc_bias * ctx.dt;
}
}

Expand Down

0 comments on commit c8b1276

Please sign in to comment.