From b84e9f4676c69313dcf41d39da946b7519782e29 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Tue, 8 Dec 2015 00:22:49 +0100 Subject: [PATCH] AIR MODE Safety Enhancements --- src/main/flight/pid.c | 11 ----------- src/main/flight/pid.h | 1 - src/main/io/rc_controls.c | 11 +++++++++++ src/main/io/rc_controls.h | 2 +- src/main/mw.c | 39 ++++++++++++++++++++++++++++++++++----- 5 files changed, 46 insertions(+), 18 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 45e3619c45..fe827357fd 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -61,8 +61,6 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; static int32_t errorGyroI[3] = { 0, 0, 0 }; static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; -static int32_t errorAngleI[2] = { 0, 0 }; -static float errorAngleIf[2] = { 0.0f, 0.0f }; static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); @@ -72,15 +70,6 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we using, defaultMultiWii -void pidResetErrorAngle(void) -{ - errorAngleI[ROLL] = 0; - errorAngleI[PITCH] = 0; - - errorAngleIf[ROLL] = 0.0f; - errorAngleIf[PITCH] = 0.0f; -} - void pidResetErrorGyro(void) { errorGyroI[ROLL] = 0; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 189093b397..b7b7374082 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -75,6 +75,5 @@ extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; void pidSetController(pidControllerType_e type); -void pidResetErrorAngle(void); void pidResetErrorGyro(void); diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 725721612a..4cd3188464 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -65,6 +65,7 @@ static pidProfile_t *pidProfile; // true if arming is done via the sticks (as opposed to a switch) static bool isUsingSticksToArm = true; +static bool rollPitchCentered = true; // Roll and pitch are centered, AIR Mode condition int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW @@ -106,6 +107,10 @@ bool isUsingSticksForArming(void) return isUsingSticksToArm; } +bool isRollPitchCentered(void) +{ + return rollPitchCentered; +} bool areSticksInApModePosition(uint16_t ap_mode) { @@ -146,6 +151,12 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat rcDelayCommand = 0; rcSticks = stTmp; + if (rcSticks == PIT_CE + ROL_CE) { + rollPitchCentered = true; + } else { + rollPitchCentered = false; + } + // perform actions if (!isUsingSticksToArm) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 2b5e0305e5..b9b5af23e7 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -126,7 +126,6 @@ typedef struct modeActivationCondition_s { #define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep) -#define SHOULD_RESET_ERRORS ((throttleStatus == THROTTLE_LOW && !(IS_RC_MODE_ACTIVE(BOXAIRMODE))) || !(ARMING_FLAG(ARMED)) || ((throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)))) typedef struct controlRateConfig_s { uint8_t rcRate8; uint8_t rcExpo8; @@ -244,3 +243,4 @@ bool isUsingSticksForArming(void); int32_t getRcStickDeflection(int32_t axis, uint16_t midrc); bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId); +bool isRollPitchCentered(void); diff --git a/src/main/mw.c b/src/main/mw.c index 5907b378d7..c27cadd44b 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -101,6 +101,10 @@ enum { #define PREVENT_BARO_READ_PRE_LOOP_TRIGGER 150 // Prevent BARO processing before expected loop trigger #define GYRO_RATE 0.001f // Gyro refresh rate 1khz +// AIR MODE Reset timers +#define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting +#define ERROR_RESET_ACTIVATE_DELAY (5 * 1000) // 5 sec delay to enable AIR MODE Iterm resetting + uint32_t currentTime = 0; uint32_t previousTime = 0; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop @@ -565,9 +569,36 @@ void processRx(void) throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); - if (SHOULD_RESET_ERRORS) { - pidResetErrorAngle(); - pidResetErrorGyro(); + static bool airModeErrorResetIsEnabled = true; // Should always initialize with reset enabled + static uint32_t airModeErrorResetToggleTimeout = 0; // Timeout for both activate and deactivate mode + + if (throttleStatus == THROTTLE_LOW) { + + // Evaluate if the error needs resetting again. Should be entered when in air mode during landings etc... + if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled) { + if ((millis() > airModeErrorResetToggleTimeout) && isRollPitchCentered()) { + airModeErrorResetIsEnabled = true; + } + } + + // Conditions to reset Error + if (!IS_RC_MODE_ACTIVE(BOXARM) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled)) { + pidResetErrorGyro(); + airModeErrorResetToggleTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer + } + } else { + if (!(feature(FEATURE_MOTOR_STOP)) && IS_RC_MODE_ACTIVE(BOXARM) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) { + if (airModeErrorResetIsEnabled) { + if ((millis() > airModeErrorResetToggleTimeout) && !isRollPitchCentered()) { // Only disable error reset when roll and pitch not centered + airModeErrorResetIsEnabled = false; + airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Start reset activate timer + } + } else if (!airModeErrorResetIsEnabled) { + airModeErrorResetToggleTimeout = millis() + ERROR_RESET_ACTIVATE_DELAY; // Reset activate timer + } + } else { + airModeErrorResetIsEnabled = true; // This is needed to reset procedure when switch is toggled in air + } } // When armed and motors aren't spinning, do beeps and then disarm @@ -634,7 +665,6 @@ void processRx(void) canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { - pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { @@ -646,7 +676,6 @@ void processRx(void) DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { - pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else {