diff --git a/docs/Settings.md b/docs/Settings.md index adb371d17a0..30533330adf 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2772,6 +2772,26 @@ Enable the possibility to manually increase the throttle in auto throttle contro --- +### nav_fw_alt_control_response + +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 | Min | Max | +| --- | --- | --- | +| 20 | 5 | 100 | + +--- + +### nav_fw_auto_climb_rate + +Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 10 | 2000 | + +--- + ### nav_fw_bank_angle Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index f8c75ab4801..5fd9df7d973 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -222,6 +222,8 @@ static const OSD_Entry cmsx_menuPidAltMagEntries[] = { OSD_LABEL_DATA_ENTRY("-- ALT&MAG --", profileIndexString), + OSD_SETTING_ENTRY("FW ALT RESPONSE", SETTING_NAV_FW_ALT_CONTROL_RESPONSE), + OTHER_PIDFF_ENTRY("ALT P", &cmsx_pidPosZ.P), OTHER_PIDFF_ENTRY("ALT I", &cmsx_pidPosZ.I), OTHER_PIDFF_ENTRY("ALT D", &cmsx_pidPosZ.D), diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d44a6e1fcad..49f11e6d787 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1109,7 +1109,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF legacyLedConfig |= ledConfig->led_function << shiftCount; shiftCount += 4; legacyLedConfig |= (ledConfig->led_overlay & 0x3F) << (shiftCount); - shiftCount += 6; + shiftCount += 6; legacyLedConfig |= (ledConfig->led_color) << (shiftCount); shiftCount += 4; legacyLedConfig |= (ledConfig->led_direction) << (shiftCount); @@ -1336,9 +1336,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_NAV_POSHOLD: sbufWriteU8(dst, navConfig()->general.flags.user_control_mode); sbufWriteU16(dst, navConfig()->general.max_auto_speed); - sbufWriteU16(dst, navConfig()->mc.max_auto_climb_rate); + sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate); sbufWriteU16(dst, navConfig()->general.max_manual_speed); - sbufWriteU16(dst, mixerConfig()->platformType != PLATFORM_AIRPLANE ? navConfig()->mc.max_manual_climb_rate:navConfig()->fw.max_manual_climb_rate); + sbufWriteU16(dst, mixerConfig()->platformType == PLATFORM_AIRPLANE ? navConfig()->fw.max_manual_climb_rate : navConfig()->mc.max_manual_climb_rate); sbufWriteU8(dst, navConfig()->mc.max_bank_angle); sbufWriteU8(dst, navConfig()->mc.althold_throttle_type); sbufWriteU16(dst, currentBatteryProfile->nav.mc.hover_throttle); @@ -1586,7 +1586,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timerHardware[i].usageFlags); } break; - + case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE sbufWriteU16(dst, navConfig()->mc.braking_speed_threshold); @@ -2399,12 +2399,16 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) if (dataSize == 13) { navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src); navConfigMutable()->general.max_auto_speed = sbufReadU16(src); - navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { + navConfigMutable()->fw.max_auto_climb_rate = sbufReadU16(src); + } else { + navConfigMutable()->mc.max_auto_climb_rate = sbufReadU16(src); + } navConfigMutable()->general.max_manual_speed = sbufReadU16(src); - if (mixerConfig()->platformType != PLATFORM_AIRPLANE) { - navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); - }else{ + if (mixerConfig()->platformType == PLATFORM_AIRPLANE) { navConfigMutable()->fw.max_manual_climb_rate = sbufReadU16(src); + } else { + navConfigMutable()->mc.max_manual_climb_rate = sbufReadU16(src); } navConfigMutable()->mc.max_bank_angle = sbufReadU8(src); navConfigMutable()->mc.althold_throttle_type = sbufReadU8(src); @@ -2734,7 +2738,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_WP: if (dataSize == 21) { - + const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number navWaypoint_t msp_wp; msp_wp.action = sbufReadU8(src); // action @@ -2948,7 +2952,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ledConfig->led_position = legacyConfig & 0xFF; ledConfig->led_function = (legacyConfig >> 8) & 0xF; ledConfig->led_overlay = (legacyConfig >> 12) & 0x3F; - ledConfig->led_color = (legacyConfig >> 18) & 0xF; + ledConfig->led_color = (legacyConfig >> 18) & 0xF; ledConfig->led_direction = (legacyConfig >> 22) & 0x3F; ledConfig->led_params = (legacyConfig >> 28) & 0xF; @@ -3238,7 +3242,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) fwAutolandApproachConfigMutable(i)->approachAlt = sbufReadU32(src); fwAutolandApproachConfigMutable(i)->landAlt = sbufReadU32(src); fwAutolandApproachConfigMutable(i)->approachDirection = sbufReadU8(src); - + int16_t head1 = 0, head2 = 0; if (sbufReadI16Safe(&head1, src)) { fwAutolandApproachConfigMutable(i)->landApproachHeading1 = head1; @@ -3292,12 +3296,12 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.correctionEnd = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightCenter = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->rateDynamics.weightEnd = sbufReadU8(src); - + } else { return MSP_RESULT_ERROR; } - break; + break; #endif #ifdef USE_PROGRAMMING_FRAMEWORK @@ -3591,7 +3595,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) static uint8_t osdPos_x = 0; //indicate new format hitl 1.4.0 - sbufWriteU8(dst, 255); + sbufWriteU8(dst, 255); if (isOSDTypeSupportedBySimulator()) { @@ -3797,7 +3801,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu #ifdef USE_SIMULATOR case MSP_SIMULATOR: tmp_u8 = sbufReadU8(src); // Get the Simulator MSP version - + // Check the MSP version of simulator if (tmp_u8 != SIMULATOR_MSP_VERSION) { break; @@ -3892,7 +3896,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } else { sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); } - + // Get the acceleration in 1G units acc.accADCf[X] = ((int16_t)sbufReadU16(src)) / 1000.0f; acc.accADCf[Y] = ((int16_t)sbufReadU16(src)) / 1000.0f; @@ -3900,7 +3904,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu acc.accVibeSq[X] = 0.0f; acc.accVibeSq[Y] = 0.0f; acc.accVibeSq[Z] = 0.0f; - + // Get the angular velocity in DPS gyro.gyroADCf[X] = ((int16_t)sbufReadU16(src)) / 16.0f; gyro.gyroADCf[Y] = ((int16_t)sbufReadU16(src)) / 16.0f; @@ -3931,7 +3935,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu simulatorData.airSpeed = sbufReadU16(src); } else { if (SIMULATOR_HAS_OPTION(HITL_EXTENDED_FLAGS)) { - sbufReadU16(src); + sbufReadU16(src); } } @@ -4006,8 +4010,8 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu *ret = MSP_RESULT_ERROR; } break; -#endif - +#endif + default: // Not handled return false; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 334b128741f..dfa7bb97d35 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2075,6 +2075,12 @@ groups: field: bank_fw.pid[PID_POS_Z].D min: 0 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 + field: fwAltControlResponseFactor + min: 5 + max: 100 - 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 @@ -2302,7 +2308,7 @@ groups: field: w_z_surface_p min: 0 max: 100 - default_value: 3.5 + default_value: 3.5 - name: inav_w_z_surface_v description: "Weight of rangefinder measurements in estimated climb rate. Setting is used on both airplanes and multirotors when rangefinder is present and Surface mode enabled" field: w_z_surface_v @@ -2772,6 +2778,12 @@ groups: field: fw.max_bank_angle min: 5 max: 80 + - name: nav_fw_auto_climb_rate + description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" + default_value: 500 + field: fw.max_auto_climb_rate + min: 10 + max: 2000 - name: nav_fw_manual_climb_rate description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" default_value: 300 @@ -4115,7 +4127,7 @@ groups: type: navFwAutolandConfig_t headers: ["navigation/navigation.h"] condition: USE_FW_AUTOLAND - members: + members: - name: nav_fw_land_approach_length description: "Length of the final approach" default_value: 35000 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2a9a3407842..e647fe475e3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -169,7 +169,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -230,9 +230,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, }, [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 }, [PID_POS_Z] = { - .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10 - .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10 - .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10 + .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, }, [PID_POS_XY] = { @@ -298,6 +298,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, + .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT, #ifdef USE_SMITH_PREDICTOR .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e088ccffadf..b42465a9dd7 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -149,6 +149,8 @@ typedef struct pidProfile_s { float fixedWingLevelTrim; float fixedWingLevelTrimGain; + + uint8_t fwAltControlResponseFactor; #ifdef USE_SMITH_PREDICTOR float smithPredictorStrength; float smithPredictorDelay; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 711ee05f37a..19178ddb1bd 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -205,6 +205,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, // Fixed wing .fw = { .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s .max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees @@ -1662,7 +1663,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LOITER_PRIOR_TO_LAN // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) { resetLandingDetector(); // force reset landing detector just in case - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_LOITER_ABOVE_HOME; // success = land } else { fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); @@ -1853,7 +1854,6 @@ 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; posControl.wpAltitudeReached = false; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS @@ -1917,10 +1917,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na fpVector3_t tmpWaypoint; tmpWaypoint.x = posControl.activeWaypoint.pos.x; tmpWaypoint.y = posControl.activeWaypoint.pos.y; - tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance), - posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f, - posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z); - setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); + 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); + if(STATE(MULTIROTOR)) { switch (wpHeadingControl.mode) { case NAV_WP_HEAD_MODE_NONE: @@ -2236,7 +2243,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI } if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER; return NAV_FSM_EVENT_SUCCESS; } @@ -3200,7 +3207,7 @@ bool rthAltControlStickOverrideCheck(uint8_t axis) (axis == ROLL && STATE(MULTIROTOR) && !posControl.flags.rthTrackbackActive)) { return false; } - + static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { @@ -3288,8 +3295,7 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag // Z-position if ((useMask & NAV_POS_UPDATE_Z) != 0) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); // Reset RoC/RoD -> altitude controller - posControl.desiredState.pos.z = pos->z; + updateClimbRateToAltitudeController(0, pos->z, ROC_TO_ALT_TARGET); } // Heading @@ -3399,66 +3405,77 @@ bool isProbablyStillFlying(void) /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ -void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) +float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros) { -#define MIN_TARGET_CLIMB_RATE 100.0f // cm/s + const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding(); + float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate; - static timeUs_t lastUpdateTimeUs; - timeUs_t currentTimeUs = micros(); + if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) { + if (posControl.flags.isAdjustingAltitude) { + maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate; + } - // Terrain following uses different altitude measurement - const float altitudeToUse = navGetCurrentActualPositionAndVelocity()->pos.z; + return constrainf(posControl.desiredState.climbRateDemand, -maxClimbRate, maxClimbRate); + } - if (mode != ROC_TO_ALT_RESET && desiredClimbRate) { - /* ROC_TO_ALT_CONSTANT - constant climb rate - * ROC_TO_ALT_TARGET - constant climb rate until close to target altitude reducing to min rate when altitude reached - * Rate reduction starts at distance from target altitude of 5 x climb rate */ + if (posControl.desiredState.climbRateDemand) { + maxClimbRate = constrainf(ABS(posControl.desiredState.climbRateDemand), 0.0f, maxClimbRate); + } else if (emergLandingIsActive) { + maxClimbRate = navConfig()->general.emerg_descent_rate; + } - if (mode == ROC_TO_ALT_TARGET && fabsf(desiredClimbRate) > MIN_TARGET_CLIMB_RATE) { - const int8_t direction = desiredClimbRate > 0 ? 1 : -1; - const float absClimbRate = fabsf(desiredClimbRate); - const uint16_t maxRateCutoffAlt = absClimbRate * 5; - const float verticalVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z - targetAltitude, - 0.0f, -maxRateCutoffAlt * direction, MIN_TARGET_CLIMB_RATE, absClimbRate); + const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z; + float targetVel = 0.0f; - desiredClimbRate = direction * constrainf(verticalVelScaled, MIN_TARGET_CLIMB_RATE, absClimbRate); - } + if (STATE(MULTIROTOR)) { + targetVel = getSqrtControllerVelocity(targetAltitude, deltaMicros); + } else { + targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f; + } - /* - * 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 > 0 && altitudeToUse >= navConfig()->general.max_altitude && desiredClimbRate > 0) { - desiredClimbRate = 0; - } + if (emergLandingIsActive && targetAltitudeError > -50.0f) { + return -100.0f; // maintain 1 m/s descent during emerg landing when within 50cm of min speed landing altitude target + } else { + return constrainf(targetVel, -maxClimbRate, maxClimbRate); + } +} - if (STATE(FIXED_WING_LEGACY)) { - // Fixed wing climb rate controller is open-loop. We simply move the known altitude target - float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); - static bool targetHoldActive = false; - - if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ) && desiredClimbRate) { - // Update target altitude only if actual altitude moving in same direction and lagging by < 5 m, otherwise hold target - if (navGetCurrentActualPositionAndVelocity()->vel.z * desiredClimbRate >= 0 && fabsf(posControl.desiredState.pos.z - altitudeToUse) < 500) { - posControl.desiredState.pos.z += desiredClimbRate * timeDelta; - targetHoldActive = false; - } else if (!targetHoldActive) { // Reset and hold target to actual + climb rate boost until actual catches up - posControl.desiredState.pos.z = altitudeToUse + desiredClimbRate; - targetHoldActive = true; - } - } else { - targetHoldActive = false; - } +void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAltitude, climbRateToAltitudeControllerMode_e mode) +{ + /* ROC_TO_ALT_TARGET - constant climb rate until close to target altitude then reducing down as altitude is reached. + * Any non zero climb rate sets the max allowed climb rate. Default max climb rate limits are used when set to 0. + * + * ROC_TO_ALT_CURRENT - similar to ROC_TO_ALT_TARGET except target altitude set to current altitude. + * No climb rate or altitude target required. + * + * ROC_TO_ALT_CONSTANT - constant climb rate. Climb rate and direction required. Target alt not required. */ + + if (mode == ROC_TO_ALT_CURRENT) { + posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z; + desiredClimbRate = 0.0f; + } else if (mode == ROC_TO_ALT_TARGET) { + posControl.desiredState.pos.z = targetAltitude; + } + + posControl.desiredState.climbRateDemand = desiredClimbRate; + posControl.flags.rocToAltMode = mode; + + /* + * If max altitude is set limit desired altitude and impose altitude limit for constant climbs unless climb rate is -ve. + * Inhibit during RTH mode and also WP mode with altitude enforce active to avoid climbs getting stuck at max alt limit. + */ + if (navConfig()->general.max_altitude && !FLIGHT_MODE(NAV_RTH_MODE) && !(FLIGHT_MODE(NAV_WP_MODE) && navConfig()->general.waypoint_enforce_altitude)) { + posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude); + + if (mode != ROC_TO_ALT_CONSTANT || (mode == ROC_TO_ALT_CONSTANT && desiredClimbRate < 0.0f)) { + return; } - else { - // Multicopter climb-rate control is closed-loop, it's possible to directly calculate desired altitude setpoint to yield the required RoC/RoD - posControl.desiredState.pos.z = altitudeToUse + (desiredClimbRate / posControl.pids.pos[Z].param.kP); + + if (posControl.flags.isAdjustingAltitude) { + posControl.desiredState.pos.z = navConfig()->general.max_altitude; + posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET; } - } else { // ROC_TO_ALT_RESET or zero desired climbrate - posControl.desiredState.pos.z = altitudeToUse; } - - lastUpdateTimeUs = currentTimeUs; } static void resetAltitudeController(bool useTerrainFollowing) @@ -4701,9 +4718,9 @@ void navigationUsePIDs(void) 0.0f ); - navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f, + navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, + (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f, 0.0f, NAV_DTERM_CUT_HZ, 0.0f diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 042c20de8ea..0613a19c24e 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -264,7 +264,7 @@ typedef struct positionEstimationConfig_s { #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; -#endif +#endif } positionEstimationConfig_t; PG_DECLARE(positionEstimationConfig_t, positionEstimationConfig); @@ -328,7 +328,7 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // multicopter max banking angle (deg) - uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec + uint16_t max_auto_climb_rate; // max vertical speed limitation nav modes cm/sec uint16_t max_manual_climb_rate; // manual velocity control max vertical speed #ifdef USE_MR_BRAKING_MODE @@ -350,6 +350,7 @@ typedef struct navConfig_s { struct { uint8_t max_bank_angle; // Fixed wing max banking angle (deg) + uint16_t max_auto_climb_rate; // max vertical speed limitation nav modes cm/sec uint16_t max_manual_climb_rate; // manual velocity control max vertical speed uint8_t max_climb_angle; // Fixed wing max banking angle (deg) uint8_t max_dive_angle; // Fixed wing max banking angle (deg) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 46cc6c95822..71a7f99fc34 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -122,7 +122,7 @@ bool adjustFixedWingAltitudeFromRCInput(void) else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); } return false; } @@ -133,46 +133,30 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) { static pt1Filter_t velzFilterState; - // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to - // velocity error. We use PID controller on altitude error and calculate desired pitch angle + float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); - // Update energies - const float demSPE = (posControl.desiredState.pos.z * 0.01f) * GRAVITY_MSS; - const float demSKE = 0.0f; - - const float estSPE = (navGetCurrentActualPositionAndVelocity()->pos.z * 0.01f) * GRAVITY_MSS; - const float estSKE = 0.0f; - - // speedWeight controls balance between potential and kinetic energy used for pitch controller - // speedWeight = 1.0 : pitch will only control airspeed and won't control altitude - // speedWeight = 0.5 : pitch will be used to control both airspeed and altitude - // speedWeight = 0.0 : pitch will only control altitude - const float speedWeight = 0.0f; // no speed sensing for now - - const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight; - const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight; - - // SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed - const float pitchGainInv = 1.0f / 1.0f; + // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention) + if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) { + desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate; + } // Here we use negative values for dive for better clarity - float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); - // Reduce max allowed climb pitch if performing loiter (stall prevention) - if (needToCalculateCircularLoiter) { - maxClimbDeciDeg = maxClimbDeciDeg * 0.67f; - } - - // PID controller to translate energy balance error [J] into pitch angle [decideg] - float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f); + // 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); // Apply low-pass filter to prevent rapid correction targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros)); - // Reconstrain pitch angle ( >0 - climb, <0 - dive) + // Reconstrain pitch angle (> 0 - climb, < 0 - dive) targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg); posControl.rcAdjustment[PITCH] = targetPitchAngle; + + posControl.desiredState.vel.z = desiredClimbRate; + navDesiredVelocity[Z] = constrain(lrintf(posControl.desiredState.vel.z), -32678, 32767); } void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) @@ -785,8 +769,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true); if (posControl.flags.estAltStatus >= EST_USABLE) { - // target min descent rate 10m above takeoff altitude - updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 1000.0f, ROC_TO_ALT_TARGET); + // target min descent rate at distance 2 x emerg descent rate above takeoff altitude + updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET); applyFixedWingAltitudeAndThrottleController(currentTimeUs); int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle)); diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 54caccb52b5..b108ef6aa31 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -63,28 +63,22 @@ static pt1Filter_t altholdThrottleFilterState; static bool prepareForTakeoffOnReset = false; static sqrt_controller_t alt_hold_sqrt_controller; -// Position to velocity controller for Z axis -static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) +float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros) { - float targetVel = sqrtControllerApply( - &alt_hold_sqrt_controller, - posControl.desiredState.pos.z, - navGetCurrentActualPositionAndVelocity()->pos.z, - US2S(deltaMicros) + return sqrtControllerApply( + &alt_hold_sqrt_controller, + targetAltitude, + navGetCurrentActualPositionAndVelocity()->pos.z, + US2S(deltaMicros) ); +} - // hard limit desired target velocity to max_climb_rate - float vel_max_z = 0.0f; - - if (posControl.flags.isAdjustingAltitude) { - vel_max_z = navConfig()->mc.max_manual_climb_rate; - } else { - vel_max_z = navConfig()->mc.max_auto_climb_rate; - } - - targetVel = constrainf(targetVel, -vel_max_z, vel_max_z); +// Position to velocity controller for Z axis +static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros) +{ + float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros); - posControl.pids.pos[Z].output_constrained = targetVel; + posControl.pids.pos[Z].output_constrained = targetVel; // only used for Blackbox and OSD info // Limit max up/down acceleration target const float smallVelChange = US2S(deltaMicros) * (GRAVITY_CMSS * 0.1f); @@ -132,8 +126,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) // In terrain follow mode we apply different logic for terrain control if (posControl.flags.estAglStatus == EST_TRUSTED && altTarget > 10.0f) { // We have solid terrain sensor signal - directly map throttle to altitude - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); - posControl.desiredState.pos.z = altTarget; + updateClimbRateToAltitudeController(0, altTarget, ROC_TO_ALT_TARGET); } else { updateClimbRateToAltitudeController(-50.0f, 0, ROC_TO_ALT_CONSTANT); @@ -144,6 +137,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) } else { const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); + if (rcThrottleAdjustment) { // set velocity proportional to stick movement float rcClimbRate; @@ -165,7 +159,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) else { // Adjusting finished - reset desired position to stay exactly where pilot released the stick if (posControl.flags.isAdjustingAltitude) { - updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET); + updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_CURRENT); } return false; @@ -931,8 +925,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Check if last correction was not too long ago if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { - // target min descent rate 5m above takeoff altitude - updateClimbRateToAltitudeController(-navConfig()->general.emerg_descent_rate, 500.0f, ROC_TO_ALT_TARGET); + // target min descent rate at distance 2 x emerg descent rate above takeoff altitude + updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index da44e6e76b2..6d7b386cbde 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -58,7 +58,7 @@ typedef enum { } navSetWaypointFlags_t; typedef enum { - ROC_TO_ALT_RESET, + ROC_TO_ALT_CURRENT, ROC_TO_ALT_CONSTANT, ROC_TO_ALT_TARGET } climbRateToAltitudeControllerMode_e; @@ -91,6 +91,8 @@ typedef struct navigationFlags_s { navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane bool gpsCfEstimatedAltitudeMismatch; // Indicates a mismatch between GPS altitude and estimated altitude + climbRateToAltitudeControllerMode_e rocToAltMode; + bool isAdjustingPosition; bool isAdjustingAltitude; bool isAdjustingHeading; @@ -136,6 +138,7 @@ typedef struct { fpVector3_t pos; fpVector3_t vel; int32_t yaw; + int16_t climbRateDemand; } navigationDesiredState_t; typedef enum { @@ -524,6 +527,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); +float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros); bool checkForPositionSensorTimeout(void); @@ -542,10 +546,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); /* Fixed-wing specific functions */ void setupFixedWingAltitudeController(void);