Skip to content

Commit

Permalink
CR132 + 133
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Sep 2, 2024
1 parent 04a5d35 commit f3a4894
Show file tree
Hide file tree
Showing 4 changed files with 92 additions and 65 deletions.
2 changes: 1 addition & 1 deletion docs/Settings.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2988,7 +2988,7 @@ Adjusts the deceleration response of fixed wing altitude control as the target a

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

---

Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/settings.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2143,7 +2143,7 @@ groups:
max: 255
- 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: 20
default_value: 30 # CR133
field: fwAltControlResponseFactor
min: 5
max: 100
Expand Down
28 changes: 12 additions & 16 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1866,6 +1866,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
case NAV_WP_ACTION_LAND:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z; // CR133
posControl.wpAltitudeReached = false;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS

Expand Down Expand Up @@ -1926,20 +1927,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
// CR133
fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);

// 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
float climbRate = 0.0f;
if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
(posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
}
updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, 0.1f * posControl.wpInitialDistance, posControl.wpInitialDistance),
posControl.wpInitialDistance, 0.1f * posControl.wpInitialDistance,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);

setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
// CR133
if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) {
case NAV_WP_HEAD_MODE_NONE:
Expand Down Expand Up @@ -4765,12 +4762,11 @@ void navigationUsePIDs(void)
NAV_DTERM_CUT_HZ,
0.0f
);

navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 50.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 50.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 50.0f,
// (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // poss new FF, original was 0.0f
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
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
125 changes: 78 additions & 47 deletions src/main/navigation/navigation_fixedwing.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,8 +146,36 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)

// PID controller to translate desired climb rate error into pitch angle [decideg]
float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);

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

// const float climbRateError = currentClimbRate - desiredClimbRate;

// static timeUs_t previousTimeMonitoringUpdate;
// static int32_t previousClimbRateError;
// static bool errorIsDecreasing;

// // Slow error monitoring (2Hz rate)
// if ((micros() - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
// // Check if error is decreasing over time
// errorIsDecreasing = (ABS(previousClimbRateError) > ABS(climbRateError));

// // Save values for next iteration
// previousClimbRateError = climbRateError;
// previousTimeMonitoringUpdate = micros();
// }

// // 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, 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 @@ -404,57 +432,61 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
}
// CR132
// Calculate cross track error
fpVector3_t virtualCoursePoint;
virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);

/* If waypoint tracking enabled force craft toward waypoint course line and hold on course line */
if (navConfig()->fw.wp_tracking_accuracy && isWaypointNavTrackingActive() && !needToCalculateCircularLoiter) {
static float crossTrackErrorRate;
static timeUs_t previousCrossTrackErrorUpdateTime;
static float previousCrossTrackError = 0.0f;
static pt1Filter_t fwCrossTrackErrorRateFilterState;

if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
if (isWaypointNavTrackingActive()) {
// Calculate cross track error
posControl.wpDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);

fpVector3_t virtualCoursePoint;
virtualCoursePoint.x = posControl.activeWaypoint.pos.x -
posControl.wpDistance * cos_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
virtualCoursePoint.y = posControl.activeWaypoint.pos.y -
posControl.wpDistance * sin_approx(CENTIDEGREES_TO_RADIANS(posControl.activeWaypoint.bearing));
navCrossTrackError = calculateDistanceToDestination(&virtualCoursePoint);

/* If waypoint tracking enabled force craft toward waypoint course line and hold on course line */
if (navConfig()->fw.wp_tracking_accuracy && !needToCalculateCircularLoiter) {
static float crossTrackErrorRate;
static timeUs_t previousCrossTrackErrorUpdateTime;
static float previousCrossTrackError = 0.0f;
static pt1Filter_t fwCrossTrackErrorRateFilterState;

if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
const float crossTrackErrorDtSec = US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
}
crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
previousCrossTrackErrorUpdateTime = currentTimeUs;
previousCrossTrackError = navCrossTrackError;
}
crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
previousCrossTrackErrorUpdateTime = currentTimeUs;
previousCrossTrackError = navCrossTrackError;
}

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

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

if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);
if ((ABS(wrap_18000(virtualTargetBearing - posControl.actualState.cog)) < 9000 || posControl.wpDistance < 1000.0f) && navCrossTrackError > trackingDeadband) {
float adjustmentFactor = wrap_18000(posControl.activeWaypoint.bearing - virtualTargetBearing);
uint16_t angleLimit = DEGREES_TO_CENTIDEGREES(navConfig()->fw.wp_tracking_max_angle);

/* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
float maxApproachSpeed = posControl.actualState.velXY * sin_approx(CENTIDEGREES_TO_RADIANS(angleLimit));
float desiredApproachSpeed = constrainf(navCrossTrackError, 50.0f, maxApproachSpeed);
adjustmentFactor = SIGN(adjustmentFactor) * navCrossTrackError * ((desiredApproachSpeed - crossTrackErrorRate) / desiredApproachSpeed);
DEBUG_SET(DEBUG_ALWAYS, 1, desiredApproachSpeed);
DEBUG_SET(DEBUG_ALWAYS, 5, adjustmentFactor);
/* Apply heading adjustment to match crossTrackErrorRate with fixed convergence speed profile */
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, 5, adjustmentFactor);

/* Calculate final adjusted virtualTargetBearing */
uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);
/* Calculate final adjusted virtualTargetBearing */
uint16_t limit = constrainf(navCrossTrackError, 1000.0f, angleLimit);
adjustmentFactor = constrainf(adjustmentFactor, -limit, limit);
virtualTargetBearing = wrap_36000(posControl.activeWaypoint.bearing - adjustmentFactor);

DEBUG_SET(DEBUG_ALWAYS, 7, limit);
DEBUG_SET(DEBUG_ALWAYS, 6, adjustmentFactor);
DEBUG_SET(DEBUG_ALWAYS, 7, limit);
DEBUG_SET(DEBUG_ALWAYS, 6, adjustmentFactor);
}
DEBUG_SET(DEBUG_ALWAYS, 4, virtualTargetBearing);
}
DEBUG_SET(DEBUG_ALWAYS, 4, virtualTargetBearing);
}
// CR132
/*
Expand Down Expand Up @@ -490,10 +522,9 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// 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);

// DEBUG_SET(DEBUG_ALWAYS, 5, navHeadingError);

// Input error in (deg*100), output roll angle (deg*100)
float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog, US2S(deltaMicros),
float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.cog + navHeadingError, posControl.actualState.cog,
US2S(deltaMicros),
-DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
pidFlags);
Expand Down

0 comments on commit f3a4894

Please sign in to comment.