Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

merge upstream into sirinfpv branch

Browse source
This commit is contained in:
Evgeny Sychov 2016-03-11 21:06:33 -08:00
commit bd114b76da
19 changed files with 114 additions and 73 deletions
Download patch file Download diff file Expand all files Collapse all files

BIN
.testThrottle.c.swp Normal file
View file

Binary file not shown.

6
src/main/config/config.c
View file

@ -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

49
src/main/drivers/pwm_mapping.c
View file

@ -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) {

29
src/main/drivers/timer.c
View file

@ -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

17
src/main/flight/mixer.c
View file

@ -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;

12
src/main/flight/pid.c
View file

@ -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.

2
src/main/flight/pid.h
View file

@ -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,

2
src/main/io/beeper.c
View file

@ -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;
}

3
src/main/io/rc_controls.c
View file

@ -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)) {

1
src/main/io/rc_controls.h
View file

@ -50,6 +50,7 @@ typedef enum {
BOXFAILSAFE,
BOXAIRMODE,
BOXACROPLUS,
BOX3DDISABLESWITCH,
CHECKBOX_ITEM_COUNT
} boxId_e;

5
src/main/io/rc_curves.c
View file

@ -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]
}
}

14
src/main/io/serial_msp.c
View file

@ -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;

2
src/main/main.c
View file

@ -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

23
src/main/mw.c
View file

@ -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;

10
src/main/sensors/gyro.c
View file

@ -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();
}
}

1
src/main/sensors/gyro.h
View file

@ -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);

2
src/main/target/SPRACINGF3MINI/target.h
View file

@ -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

2
src/main/version.h
View file

@ -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)

5
testThrottle.c.save Normal file
View file

@ -0,0 +1,5 @@
#include "stdint.h"
#include "stdio.h"
int main()