diff --git a/.testThrottle.c.swp b/.testThrottle.c.swp new file mode 100644 index 0000000000..d53eb7fc58 Binary files /dev/null and b/.testThrottle.c.swp differ diff --git a/src/main/config/config.c b/src/main/config/config.c index 04a611c597..f73948de49 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -418,8 +418,8 @@ static void resetConf(void) masterConfig.current_profile_index = 0; // default profile masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000 - masterConfig.gyro_lpf = 1; // 188HZ - masterConfig.gyro_sync_denom = 4; + masterConfig.gyro_lpf = 0; // 256HZ default + masterConfig.gyro_sync_denom = 8; masterConfig.gyro_soft_lpf_hz = 60; masterConfig.pid_process_denom = 1; @@ -452,7 +452,7 @@ static void resetConf(void) masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.spektrum_sat_bind_autoreset = 1; masterConfig.rxConfig.midrc = 1500; - masterConfig.rxConfig.mincheck = 1100; + masterConfig.rxConfig.mincheck = 1040; masterConfig.rxConfig.maxcheck = 1900; masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1a069dba0e..17ec005857 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -521,6 +521,8 @@ static const uint16_t airPWM[] = { static const uint16_t multiPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), @@ -529,35 +531,49 @@ static const uint16_t multiPPM[] = { PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM13 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; static const uint16_t multiPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), + PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), 0xFFFF }; static const uint16_t airPPM[] = { PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 - PWM6 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), PWM7 | (MAP_TO_SERVO_OUTPUT << 8), PWM8 | (MAP_TO_SERVO_OUTPUT << 8), PWM9 | (MAP_TO_SERVO_OUTPUT << 8), PWM10 | (MAP_TO_SERVO_OUTPUT << 8), - PWM11 | (MAP_TO_SERVO_OUTPUT << 8), - PWM12 | (MAP_TO_SERVO_OUTPUT << 8), - PWM13 | (MAP_TO_SERVO_OUTPUT << 8), - PWM2 | (MAP_TO_SERVO_OUTPUT << 8), - PWM3 | (MAP_TO_SERVO_OUTPUT << 8), // servo #10 + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 0xFFFF }; static const uint16_t airPWM[] = { + PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 + PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 + PWM4 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1 + PWM5 | (MAP_TO_SERVO_OUTPUT << 8), + PWM6 | (MAP_TO_SERVO_OUTPUT << 8), + PWM7 | (MAP_TO_SERVO_OUTPUT << 8), + PWM8 | (MAP_TO_SERVO_OUTPUT << 8), + PWM9 | (MAP_TO_SERVO_OUTPUT << 8), + PWM10 | (MAP_TO_SERVO_OUTPUT << 8), + PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #8 0xFFFF }; #endif @@ -754,8 +770,8 @@ if (init->useBuzzerP6) { #endif #if defined(SPRACINGF3MINI) - // remap PWM8+9 as servos - if ((timerIndex == PWM8 || timerIndex == PWM9) && timerHardwarePtr->tim == TIM15) + // remap PWM6+7 as servos + if ((timerIndex == PWM6 || timerIndex == PWM7) && timerHardwarePtr->tim == TIM15) type = MAP_TO_SERVO_OUTPUT; #endif @@ -814,11 +830,6 @@ if (init->useBuzzerP6) { if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2); } -#endif -#ifdef SPRACINGF3MINI - if (init->useOneshot || isMotorBrushed(init->motorPwmRate)) { - ppmAvoidPWMTimerClash(timerHardwarePtr, TIM3); - } #endif ppmInConfig(timerHardwarePtr); } else if (type == MAP_TO_PWM_INPUT) { diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 32cfea6b60..5d17c6a491 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -325,30 +325,25 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { // PPM Pad #ifdef SPRACINGF3MINI_MKII_REVA - { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // RC_CH5 - PB5 - *TIM3_CH1 + { TIM3, GPIOB, Pin_5, TIM_Channel_2, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource5, GPIO_AF_2}, // PPM - PB5 // PB4 / TIM3 CH1 is connected to USBPresent #else - { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // RC_CH5 - PB4 - *TIM3_CH1 + { TIM3, GPIOB, Pin_4, TIM_Channel_1, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource4, GPIO_AF_2}, // PPM - PB4 // PB5 / TIM3 CH2 is connected to USBPresent #endif - // PAD6/7 Sonar Pads - { TIM3, GPIOB, Pin_0, TIM_Channel_3, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_2}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - { TIM3, GPIOB, Pin_1, TIM_Channel_4, TIM3_IRQn, 0, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_2}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N - // UART3 RX/TX pads - { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM5 - PB8 - { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM6 - PB9 - { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // PWM7 - PA2 - { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM8 - PA3 + { TIM16, GPIOA, Pin_6, TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, Mode_AF_PP, GPIO_PinSource6, GPIO_AF_1}, // PWM1 - PA6 + { TIM17, GPIOA, Pin_7, TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, Mode_AF_PP, GPIO_PinSource7, GPIO_AF_1}, // PWM2 - PA7 + { TIM4, GPIOB, Pin_8, TIM_Channel_3, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_2}, // PWM3 - PB8 + { TIM4, GPIOB, Pin_9, TIM_Channel_4, TIM4_IRQn, 1, Mode_AF_PP, GPIO_PinSource9, GPIO_AF_2}, // PWM4 - PB9 + { TIM15, GPIOA, Pin_2, TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource2, GPIO_AF_9}, // PWM5 - PA2 + { TIM15, GPIOA, Pin_3, TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, Mode_AF_PP, GPIO_PinSource3, GPIO_AF_9}, // PWM6 - PA3 + { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // PWM7 - PA0 + { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // PWM8 - PA1 - // PAD3/4 - { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource0, GPIO_AF_1}, // PWM7 - PA0 - *TIM2_CH1 - { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource1, GPIO_AF_1}, // PWM8 - PA1 - *TIM2_CH2, TIM15_CH1N // UART3 RX/TX - { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7) - { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7) + { TIM2, GPIOB, Pin_10, TIM_Channel_3, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource10, GPIO_AF_1}, // PWM9 - PB10 - TIM2_CH3 / USART3_TX (AF7) + { TIM2, GPIOB, Pin_11, TIM_Channel_4, TIM2_IRQn, 1, Mode_AF_PP, GPIO_PinSource11, GPIO_AF_1}, // PWM10 - PB11 - TIM2_CH4 / USART3_RX (AF7) // LED Strip Pad { TIM1, GPIOA, Pin_8, TIM_Channel_1, TIM1_CC_IRQn, 1, Mode_AF_PP, GPIO_PinSource8, GPIO_AF_6}, // GPIO_TIMER / LED_STRIP diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f24e38e2c9..f61c068eea 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -782,16 +782,9 @@ void mixTable(void) uint32_t i; fix12_t vbatCompensationFactor; static fix12_t mixReduction; - uint8_t axis; bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code - if (motorLimitReached) { - uint8_t pidAttenuation = constrain(qPercent(mixReduction), 20, 100); - for (axis = 0; axis < 2; axis++) axisPID[axis] *= pidAttenuation / 100; - if (debugMode == DEBUG_AIRMODE) debug[0] = pidAttenuation; - } - if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) { acroPlusApply(); } @@ -829,18 +822,18 @@ void mixTable(void) int16_t throttleMin, throttleMax; static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions - // Find min and max throttle based on condition. Use rcData for 3D to prevent loss of power due to min_check + // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. - if ((rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling + if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; - throttlePrevious = throttle = rcData[THROTTLE]; - } else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling + throttlePrevious = throttle = rcCommand[THROTTLE]; + } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; - throttlePrevious = throttle = rcData[THROTTLE]; + throttlePrevious = throttle = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 94fe1a3de6..c8496db4a3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -213,6 +213,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + // -----calculate I component. errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f); @@ -407,7 +412,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended - // Constrain YAW by D value if not servo driven in that case servolimits apply + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) { PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); } @@ -504,6 +509,11 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; + // Constrain YAW by yaw_p_limit value if not servo driven in that case servolimits apply + if((motorCount >= 4 && pidProfile->yaw_p_limit) && axis == YAW) { + PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit); + } + // -----calculate I component // there should be no division before accumulating the error to integrator, because the precision would be reduced. // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 709c0309a2..8cd7c3621b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -20,7 +20,7 @@ #define GYRO_I_MAX 256 // Gyro I limiter #define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter -#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter +#define YAW_P_LIMIT_MAX 300 // Maximum value for yaw P limiter typedef enum { PIDROLL, diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 7a0e1d6b98..e7ec349649 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -185,7 +185,7 @@ static const beeperTableEntry_t *currentBeeperEntry = NULL; */ void beeper(beeperMode_e mode) { - if (mode == BEEPER_SILENCE || (mode == BEEPER_USB && (feature(FEATURE_VBAT) && (batteryCellCount < 2)))) { + if (mode == BEEPER_SILENCE || ((getBeeperOffMask() & (1 << (BEEPER_USB-1))) && (feature(FEATURE_VBAT) && (batteryCellCount < 2)))) { beeperSilence(); return; } diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index c89c50bef3..03a108d6f5 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -34,6 +34,7 @@ #include "drivers/system.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "drivers/gyro_sync.h" #include "sensors/barometer.h" #include "sensors/battery.h" @@ -215,7 +216,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { // GYRO calibration - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); + gyroSetCalibrationCycles(calculateCalibratingCycles()); #ifdef GPS if (feature(FEATURE_GPS)) { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 8cdc3c7750..c512ed64bb 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -49,7 +49,8 @@ typedef enum { BOXBLACKBOX, BOXFAILSAFE, BOXAIRMODE, - BOXACROPLUS, + BOXACROPLUS, + BOX3DDISABLESWITCH, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 80b0bd4aa7..e6abf1d86c 100644 --- a/src/main/io/rc_curves.c +++ b/src/main/io/rc_curves.c @@ -24,6 +24,8 @@ #include "io/rc_curves.h" +#include "config/config.h" + int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE @@ -48,6 +50,7 @@ void generateYawCurve(controlRateConfig_t *controlRateConfig) void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig) { uint8_t i; + uint16_t minThrottle = (feature(FEATURE_3D && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) ? PWM_RANGE_MIN : escAndServoConfig->minthrottle); for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { int16_t tmp = 10 * i - controlRateConfig->thrMid8; @@ -57,6 +60,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo if (tmp < 0) y = controlRateConfig->thrMid8; lookupThrottleRC[i] = 10 * controlRateConfig->thrMid8 + tmp * (100 - controlRateConfig->thrExpo8 + (int32_t) controlRateConfig->thrExpo8 * (tmp * tmp) / (y * y)) / 10; - lookupThrottleRC[i] = escAndServoConfig->minthrottle + (int32_t) (escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] + lookupThrottleRC[i] = minThrottle + (int32_t) (escAndServoConfig->maxthrottle - minThrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } } diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 4b84152896..b55f74aaca 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -134,9 +134,10 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.mag_hardware = 1; masterConfig.pid_process_denom = 2; } + masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); } else { - masterConfig.gyro_lpf = 1; - masterConfig.pid_process_denom = 1; + masterConfig.gyro_lpf = 0; + masterConfig.gyro_sync_denom = 8; masterConfig.acc_hardware = 0; masterConfig.baro_hardware = 0; masterConfig.mag_hardware = 0; @@ -160,15 +161,17 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.pid_process_denom = 2; } } + masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); } else { - masterConfig.gyro_lpf = 1; + masterConfig.gyro_lpf = 0; + + masterConfig.gyro_sync_denom = 8; masterConfig.acc_hardware = 0; masterConfig.baro_hardware = 0; masterConfig.mag_hardware = 0; masterConfig.pid_process_denom = 1; } #endif - masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); if (!(masterConfig.use_multiShot || masterConfig.use_oneshot42) && ((masterConfig.gyro_sync_denom * gyroSampleRate) == 125)) masterConfig.pid_process_denom = 3; } @@ -216,6 +219,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXFAILSAFE, "FAILSAFE;", 27 }, { BOXAIRMODE, "AIR MODE;", 28 }, { BOXACROPLUS, "ACRO PLUS;", 29 }, + { BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30}, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -543,7 +547,7 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; activeBoxIds[activeBoxIdCount++] = BOXACROPLUS; - + activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH; if (sensors(SENSOR_BARO)) { activeBoxIds[activeBoxIdCount++] = BOXBARO; diff --git a/src/main/main.c b/src/main/main.c index 395665a9c3..3402109942 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -605,7 +605,7 @@ void init(void) if (masterConfig.mixerMode == MIXER_GIMBAL) { accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); } - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); + gyroSetCalibrationCycles(calculateCalibratingCycles()); #ifdef BARO baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 4f7704d5f9..2fa5694ee7 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -271,16 +271,21 @@ void annexCode(void) rcCommand[axis] = -rcCommand[axis]; } - if (isUsingSticksForArming()) { + if (feature(FEATURE_3D)) { + tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); + tmp = (uint32_t)(tmp - PWM_RANGE_MIN); + } else { tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); - } else { - tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); - tmp = (uint32_t)(tmp - PWM_RANGE_MIN) * PWM_RANGE_MIN / (PWM_RANGE_MAX - PWM_RANGE_MIN); // [MINCHECK;2000] -> [0;1000] } tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] + if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) { + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc); + } + if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); @@ -747,6 +752,14 @@ void taskMotorUpdate(void) { } } +uint8_t setPidUpdateCountDown(void) { + if (masterConfig.gyro_soft_lpf_hz) { + return masterConfig.pid_process_denom - 1; + } else { + return 1; + } +} + // Check for oneshot125 protection. With fast looptimes oneshot125 pulse duration gets more near the pid looptime bool shouldUpdateMotorsAfterPIDLoop(void) { if (targetPidLooptime > 375 ) { @@ -788,7 +801,7 @@ void taskMainPidLoopCheck(void) { if (pidUpdateCountdown) { pidUpdateCountdown--; } else { - pidUpdateCountdown = masterConfig.pid_process_denom - 1; + pidUpdateCountdown = setPidUpdateCountDown(); taskMainPidLoop(); if (shouldUpdateMotorsAfterPIDLoop()) taskMotorUpdate(); runTaskMainSubprocesses = true; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5fe75d2819..de34127604 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -77,9 +77,13 @@ bool isOnFinalGyroCalibrationCycle(void) return calibratingG == 1; } +uint16_t calculateCalibratingCycles(void) { + return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES; +} + bool isOnFirstGyroCalibrationCycle(void) { - return calibratingG == CALIBRATING_GYRO_CYCLES; + return calibratingG == calculateCalibratingCycles(); } static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold) @@ -108,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho float dev = devStandardDeviation(&var[axis]); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) { - gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); + gyroSetCalibrationCycles(calculateCalibratingCycles()); return; } - gyroZero[axis] = (g[axis] + (CALIBRATING_GYRO_CYCLES / 2)) / CALIBRATING_GYRO_CYCLES; + gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles(); } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index bb7a152f06..48e5e677e2 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -43,4 +43,5 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroUpdate(void); bool isGyroCalibrationComplete(void); +uint16_t calculateCalibratingCycles(void); diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 817f39d2a7..b2fc739413 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -31,7 +31,7 @@ #define BEEP_PERIPHERAL RCC_AHBPeriph_GPIOC #define BEEPER_INVERTED -#define USABLE_TIMER_CHANNEL_COUNT 14 // 8 Outputs; PPM; LED Strip; SonarPads; 2 additional PWM pins also on UART3 RX/TX pins. +#define USABLE_TIMER_CHANNEL_COUNT 12 // 8 Outputs; PPM; LED Strip; 2 additional PWM pins also on UART3 RX/TX pins. #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect diff --git a/src/main/version.h b/src/main/version.h index 7b7bdd3379..65e7dc436e 100644 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 5 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 4 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x) diff --git a/testThrottle.c.save b/testThrottle.c.save new file mode 100644 index 0000000000..51cab990bc --- /dev/null +++ b/testThrottle.c.save @@ -0,0 +1,5 @@ +#include "stdint.h" +#include "stdio.h" + + +int main()