From 7f9c6978fe41c99f728417b4efaeff37b077caed Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 7 Nov 2023 22:22:07 +0000 Subject: [PATCH] CR97 + CR47 --- src/main/blackbox/blackbox.c | 2 +- src/main/cms/cms_menu_navigation.c | 2 +- src/main/fc/settings.yaml | 8 +++++ src/main/navigation/navigation.c | 37 +++++++++++++------- src/main/navigation/navigation_fixedwing.c | 3 +- src/main/navigation/navigation_multicopter.c | 37 +++++++++++++------- src/main/navigation/navigation_private.h | 5 ++- 7 files changed, 63 insertions(+), 31 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index cb12a7a8968..ceed6b5542b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -472,7 +472,7 @@ typedef struct blackboxMainState_s { int32_t mcSurfacePID[3]; int32_t mcSurfacePIDOutput; - int32_t fwAltPID[3]; + int32_t fwAltPID[3]; // CR97 int32_t fwAltPIDOutput; int32_t fwPosPID[3]; int32_t fwPosPIDOutput; diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 75fe8e498ce..eed626bd54c 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -53,6 +53,7 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] = OSD_SETTING_ENTRY("USE FW YAW CONTROL", SETTING_NAV_USE_FW_YAW_CONTROL), OSD_SETTING_ENTRY("LAND SENSITIVITY", SETTING_NAV_LAND_DETECT_SENSITIVITY), OSD_SETTING_ENTRY("LANDING DISARM", SETTING_NAV_DISARM_ON_LANDING), + OSD_SETTING_ENTRY("MC ACCEL MAX LIMIT", SETTING_NAV_MC_XY_ACCEL_MAX_LIMIT), // CR47 OSD_BACK_AND_END_ENTRY, }; @@ -121,7 +122,6 @@ static const OSD_Entry cmsx_menuFWCruiseEntries[] = OSD_SETTING_ENTRY("PITCH TO THR SMOOTHING", SETTING_NAV_FW_PITCH2THR_SMOOTHING), OSD_SETTING_ENTRY("PITCH TO THR THRESHOLD", SETTING_NAV_FW_PITCH2THR_THRESHOLD), OSD_SETTING_ENTRY("MANUAL THR INCREASE", SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE), - OSD_SETTING_ENTRY("MC ACCEL MAX LIMIT", SETTING_NAV_MC_XY_ACCEL_MAX_LIMIT), // CR47 OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 85a9257b99c..c646f498fcb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2003,6 +2003,14 @@ groups: field: bank_fw.pid[PID_POS_Z].D min: 0 max: 255 +# CR97 + # - name: nav_fw_pos_z_ff + # description: "Feed forward gain of altitude PID controller (Fixedwing)" + # default_value: 10 + # field: bank_fw.pid[PID_POS_Z].FF + # min: 0 + # max: 255 +# CR97 - name: nav_fw_pos_xy_p description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" default_value: 75 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 952c927ff7c..261ff91934b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3049,30 +3049,42 @@ bool isProbablyStillFlying(void) // lastUpdateTimeUs = currentTimeUs; // } // CR97 -bool isAltitudeLimitExceeded(void) +static bool isMaxAltitudeLimitExceeded(void) { return navGetCurrentActualPositionAndVelocity()->pos.z >= navConfig()->general.max_altitude; } -int32_t getClimbRate(float targetAltitude) +int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { - if (navConfig()->general.max_altitude && isAltitudeLimitExceeded()) { + if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded()) { targetAltitude = MIN(targetAltitude, navConfig()->general.max_altitude); } - const int32_t targetAltitudeError = navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude; - const float maxClimbRate = posControl.flags.isAdjustingAltitude ? navConfig()->general.max_manual_climb_rate : navConfig()->general.max_auto_climb_rate; + float targetVel = 0.0f; const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); - const float minClimbRate = emergLandingIsActive ? 100.0f : 0.0f; - const uint16_t maxRateCutoffAlt = STATE(AIRPLANE) ? maxClimbRate * 5 : maxClimbRate; - const int8_t direction = targetAltitudeError < 0 ? 1 : -1; - const float verticalVelScaled = scaleRangef(targetAltitudeError, 0.0f, -maxRateCutoffAlt * direction, minClimbRate, maxClimbRate); + float maxClimbRate = navConfig()->general.max_auto_climb_rate; + if (emergLandingIsActive) { + maxClimbRate = navConfig()->general.emerg_descent_rate; + } else if (posControl.flags.isAdjustingAltitude) { + maxClimbRate = navConfig()->general.max_manual_climb_rate; + } + const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z; + + if (STATE(MULTIROTOR)) { + targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros); + } else { + // const uint16_t maxRateCutoffAlt = maxClimbRate * 5; + // const int8_t direction = targetAltitudeError < 0 ? 1 : -1; + + // targetVel = direction * scaleRangef(targetAltitudeError, 0.0f, -maxRateCutoffAlt * direction, 0.0f, maxClimbRate); + targetVel = scaleRangef(targetAltitudeError, 0.0f, maxClimbRate * 5, 0.0f, maxClimbRate); + } - if (emergLandingIsActive && targetAltitudeError < 50) { + if (emergLandingIsActive && targetAltitudeError > -50) { return -100; } else { - return direction * constrainf(verticalVelScaled, minClimbRate, maxClimbRate); + return constrainf(targetVel, -maxClimbRate, maxClimbRate); } } @@ -3098,7 +3110,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 * In other words, when altitude is reached, allow it only to shrink */ - if (navConfig()->general.max_altitude && isAltitudeLimitExceeded()) { + if (navConfig()->general.max_altitude && isMaxAltitudeLimitExceeded() && desiredClimbRate >= 0.0f) { posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude); } @@ -4341,6 +4353,7 @@ void navigationUsePIDs(void) navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, // CR97 uses 100, original was 10 (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, // CR97 uses 100, original was 10 (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, // CR97 uses 100, original was 10 + // (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f, // CR97 new FF, original was 0.0f 0.0f, NAV_DTERM_CUT_HZ, 0.0f diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 82e5f8a2084..f733951ca4b 100644 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -139,7 +139,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) float desiredClimbRate = posControl.desiredState.vel.z; if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) { - desiredClimbRate = getClimbRate(posControl.desiredState.pos.z); + desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); } // CR96 @@ -165,6 +165,7 @@ DEBUG_SET(DEBUG_ALWAYS, 2, desiredClimbRate - currentClimbRate); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); DEBUG_SET(DEBUG_ALWAYS, 3, targetPitchAngle); +DEBUG_SET(DEBUG_ALWAYS, 4, posControl.desiredState.pos.z); // Reconstrain pitch angle ( >0 - climb, <0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); posControl.rcAdjustment[PITCH] = targetPitchAngle; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 8ea30977e7b..fa92d2f50a5 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -62,13 +62,24 @@ static int16_t altHoldThrottleRCZero = 1500; static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; - +//CR97 +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) +{ + return sqrtControllerApply( + &alt_hold_sqrt_controller, + targetAltitude, + navGetCurrentActualPositionAndVelocity()->pos.z, + US2S(deltaMicros) + ); +} +// CR97 // Position to velocity controller for Z axis static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) -{ - float targetVel = posControl.desiredState.vel.z; // CR97 +{// CR97 + float targetVel = posControl.desiredState.vel.z; + if (posControl.desiredState.pos.z != NAV_IMPOSSIBLE_ALTITUDE_TARGET) { - targetVel = getClimbRate(posControl.desiredState.pos.z); + targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); } // float targetVel = sqrtControllerApply( @@ -78,17 +89,17 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) // US2S(deltaMicros) // ); - // hard limit desired target velocity to max_climb_rate - float vel_max_z = 0.0f; + // // hard limit desired target velocity to max_climb_rate + // float vel_max_z = 0.0f; - if (posControl.flags.isAdjustingAltitude) { - vel_max_z = navConfig()->general.max_manual_climb_rate; - } else { - vel_max_z = navConfig()->general.max_auto_climb_rate; - } - - targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); + // if (posControl.flags.isAdjustingAltitude) { + // vel_max_z = navConfig()->general.max_manual_climb_rate; + // } else { + // vel_max_z = navConfig()->general.max_auto_climb_rate; + // } + // targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); + // CR97 posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info CR97 // Limit max up/down acceleration target diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index d0a5296233e..01c35beafc7 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -474,7 +474,7 @@ bool isWaypointNavTrackingActive(void); void updateActualHeading(bool headingValid, int32_t newHeading, int32_t newGroundCourse); void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY); void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus, float gpsCfEstimatedAltitudeError); -int32_t getClimbRate(float targetAltitude); // CR97 +int32_t getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros); // CR97 bool checkForPositionSensorTimeout(void); @@ -493,10 +493,9 @@ bool adjustMulticopterHeadingFromRCInput(void); bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment); void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs); - bool isMulticopterLandingDetected(void); - void calculateMulticopterInitialHoldPosition(fpVector3_t * pos); +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros); // CR97 /* Fixed-wing specific functions */ void setupFixedWingAltitudeController(void);