diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4a2e74a109e..b85c3ff6595 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -531,7 +531,7 @@ void tryArm(void) if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) { sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); ENABLE_ARMING_FLAG(ARMED); - enableFlightMode(TURTLE_MODE); + ENABLE_FLIGHT_MODE(TURTLE_MODE); return; } #endif @@ -667,18 +667,14 @@ void processRx(timeUs_t currentTimeUs) /* Flaperon mode */ if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) { - if (!FLIGHT_MODE(FLAPERON)) { - ENABLE_FLIGHT_MODE(FLAPERON); - } + ENABLE_FLIGHT_MODE(FLAPERON); } else { DISABLE_FLIGHT_MODE(FLAPERON); } /* Turn assistant mode */ if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) { - if (!FLIGHT_MODE(TURN_ASSISTANT)) { - ENABLE_FLIGHT_MODE(TURN_ASSISTANT); - } + ENABLE_FLIGHT_MODE(TURN_ASSISTANT); } else { DISABLE_FLIGHT_MODE(TURN_ASSISTANT); } @@ -697,9 +693,7 @@ void processRx(timeUs_t currentTimeUs) #if defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { - if (!FLIGHT_MODE(HEADFREE_MODE)) { - ENABLE_FLIGHT_MODE(HEADFREE_MODE); - } + ENABLE_FLIGHT_MODE(HEADFREE_MODE); } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index a4fb116b9e6..76c80b31404 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -90,24 +90,6 @@ armingFlag_e isArmingDisabledReason(void) return 0; } -/** - * Enables the given flight mode. Returns the new 'flightModeFlags' value. - */ -uint32_t enableFlightMode(flightModeFlags_e mask) -{ - flightModeFlags |= (mask); - return flightModeFlags; -} - -/** - * Disables the given flight mode. Returns the new 'flightModeFlags' value. - */ -uint32_t disableFlightMode(flightModeFlags_e mask) -{ - flightModeFlags &= ~(mask); - return flightModeFlags; -} - /** * Called at Rx update rate. Beeper sounded if flight mode state has changed. */ diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 86f3e5d5a14..984232233cb 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -109,8 +109,8 @@ typedef enum { extern uint32_t flightModeFlags; -#define DISABLE_FLIGHT_MODE(mask) disableFlightMode(mask) -#define ENABLE_FLIGHT_MODE(mask) enableFlightMode(mask) +#define DISABLE_FLIGHT_MODE(mask) (flightModeFlags &= ~(mask)) +#define ENABLE_FLIGHT_MODE(mask) (flightModeFlags |= (mask)) #define FLIGHT_MODE(mask) (flightModeFlags & (mask)) typedef enum { @@ -201,8 +201,6 @@ extern simulatorData_t simulatorData; #endif -uint32_t enableFlightMode(flightModeFlags_e mask); -uint32_t disableFlightMode(flightModeFlags_e mask); void updateFlightModeChangeBeeper(void); bool sensors(uint32_t mask); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2602868c4db..38eb0ea1400 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -192,7 +192,7 @@ tables: enum: gpsBaudRate_e - name: nav_mc_althold_throttle values: ["STICK", "MID_STICK", "HOVER"] - enum: navMcAltHoldThrottle_e + enum: navMcAltHoldThrottle_e constants: RPYL_PID_MIN: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f5b861080f4..cd0189f04ff 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1111,9 +1111,12 @@ void FAST_CODE pidController(float dT) const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; levelingEnabled = false; + // Pitch hold enabled during Course Hold mode if AttiHold mode selected (but not active since Angle has priority) + bool navPitchHoldActive = IS_RC_MODE_ACTIVE(BOXATTIHOLD) && FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE); + static bool restartAttiMode = true; if (!restartAttiMode) { - restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE); // set restart flag if attihold_mode not active + restartAttiMode = !FLIGHT_MODE(ATTIHOLD_MODE) && !navPitchHoldActive; // set restart flag if attitude hold not active } for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { @@ -1121,16 +1124,19 @@ void FAST_CODE pidController(float dT) // If axis angle override, get the correct angle from Logic Conditions float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis)); - if (FLIGHT_MODE(ATTIHOLD_MODE) && !isFlightAxisAngleOverrideActive(axis)) { + if (STATE(AIRPLANE) && (FLIGHT_MODE(ATTIHOLD_MODE) || (navPitchHoldActive && axis == FD_PITCH)) && !isFlightAxisAngleOverrideActive(axis)) { static int16_t attiHoldTarget[2]; - if (restartAttiMode) { // set target attitude to current attitude when initialised + if (restartAttiMode) { // set target attitude to current attitude when initialised attiHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL]; attiHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH]; restartAttiMode = false; } uint16_t bankLimit = pidProfile()->max_angle_inclination[axis]; + if (navPitchHoldActive) { // use Nav bank limits if Nav active, limited to pitch only + bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); + } if (calculateRollPitchCenterStatus() == CENTERED) { angleTarget = constrain(attiHoldTarget[axis], -bankLimit, bankLimit); } else { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index de0e3126b7c..2276bf8ff94 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -106,8 +106,8 @@ typedef struct pidProfile_s { pidBank_t bank_mc; uint8_t dterm_lpf_type; // Dterm LPF type: PT1, BIQUAD - uint16_t dterm_lpf_hz; - + uint16_t dterm_lpf_hz; + uint8_t yaw_lpf_hz; uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller @@ -129,7 +129,7 @@ typedef struct pidProfile_s { float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees - + float navVelXyDTermLpfHz; uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 12076127458..4dc3f5a2185 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3727,9 +3727,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) /* Soaring mode, disables altitude control in Position hold and Course hold modes. * Pitch allowed to freefloat within defined Angle mode deadband */ if (IS_RC_MODE_ACTIVE(BOXSOARING) && (FLIGHT_MODE(NAV_POSHOLD_MODE) || FLIGHT_MODE(NAV_COURSE_HOLD_MODE))) { - if (!FLIGHT_MODE(SOARING_MODE)) { - ENABLE_FLIGHT_MODE(SOARING_MODE); - } + ENABLE_FLIGHT_MODE(SOARING_MODE); } else { DISABLE_FLIGHT_MODE(SOARING_MODE); }