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

Remove old mixer // Separate Acro Plus from Airmode // Fix MOTOLAB merge issues

Browse source
This commit is contained in:
borisbstyle 2016-02-03 00:58:38 +01:00
parent 61186c0227
commit a1ebe6fd4f
8 changed files with 143 additions and 247 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -126,8 +126,6 @@ void mpu6000SpiGyroInit(uint8_t lpf)
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting

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

@ -759,17 +759,6 @@ void mixTable(void)
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));
}
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE))) {
motorLimitReached = false; // It always needs to be reset so it can't get stuck when flipping back and fourth
// motors for non-servo mixes
for (i = 0; i < motorCount; i++) {
motor[i] =
rcCommand[THROTTLE] * currentMixer[i].throttle +
axisPID[PITCH] * currentMixer[i].pitch +
axisPID[ROLL] * currentMixer[i].roll +
-mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw;
}
} else {
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS];
int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero.
@ -864,66 +853,8 @@ void mixTable(void)
}
}
}
}
if (ARMING_FLAG(ARMED)) {
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE))) {
int16_t maxThrottleDifference = 0;
if (!(feature(FEATURE_3D))) {
// Find the maximum motor output.
int16_t maxMotor = motor[0];
for (i = 1; i < motorCount; i++) {
// If one motor is above the maxthrottle threshold, we reduce the value
// of all motors by the amount of overshoot. That way, only one motor
// is at max and the relative power of each motor is preserved.
if (motor[i] > maxMotor) {
maxMotor = motor[i];
}
}
if (maxMotor > escAndServoConfig->maxthrottle) {
maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle;
}
}
for (i = 0; i < motorCount; i++) {
if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) {
// this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxThrottleDifference;
}
if (feature(FEATURE_3D)) {
if (rcData[THROTTLE] <= rxConfig->midrc - flight3DConfig->deadband3d_throttle
|| rcData[THROTTLE] >= rxConfig->midrc + flight3DConfig->deadband3d_throttle) {
if (rcData[THROTTLE] > rxConfig->midrc) {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
} else {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low);
}
} else {
if (rcData[THROTTLE] > rxConfig->midrc) {
motor[i] = flight3DConfig->deadband3d_high;
} else {
motor[i] = flight3DConfig->deadband3d_low;
}
}
} else {
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else {
// If we're at minimum throttle and FEATURE_MOTOR_STOP enabled,
// do not spin the motors.
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
if (((rcData[THROTTLE]) < rxConfig->mincheck)) {
if (feature(FEATURE_MOTOR_STOP)) {
motor[i] = escAndServoConfig->mincommand;
}
}
}
}
}
}
} else {
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];
}

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

@ -50,7 +50,8 @@
extern float dT;
extern bool motorLimitReached;
extern bool preventItermWindup;
#define PREVENT_WINDUP(x,y) { if (ABS(x) > ABS(y)) { if (x < 0) { x = -ABS(y); } else { x = ABS(y); } } }
int16_t axisPID[3];
@ -63,8 +64,8 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
uint8_t PIDweight[3];
static int32_t errorGyroI[3] = { 0, 0, 0 };
static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
static int32_t errorGyroI[3], previousErrorGyroI[3];
static float errorGyroIf[3], previousErrorGyroIf[3];
static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
@ -76,32 +77,42 @@ pidControllerFuncPtr pid_controller = pidRewrite; // which pid controller are we
void pidResetErrorGyro(void)
{
errorGyroI[ROLL] = 0;
errorGyroI[PITCH] = 0;
errorGyroI[YAW] = 0;
int axis;
errorGyroIf[ROLL] = 0.0f;
errorGyroIf[PITCH] = 0.0f;
errorGyroIf[YAW] = 0.0f;
for (axis = 0; axis < 3; axis++) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
PREVENT_WINDUP(errorGyroI[axis], previousErrorGyroI[axis]);
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
errorGyroI[axis] = 0;
errorGyroIf[axis] = 0.0f;
}
}
}
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float scaleItermToRcInput(int axis) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
static float iTermScaler[3] = {1.0f, 1.0f, 1.0f};
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Reset Iterm on high stick inputs. No scaling necessary here */
iTermScaler[axis] = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries. Slowly enable Iterm activity. Perhaps more scaling to looptime needed for consistency */
if (iTermScaler[axis] < 1) {
iTermScaler[axis] = constrainf(iTermScaler[axis] + 0.001f, 0.0f, 1.0f);
} else {
iTermScaler[axis] = 1;
}
}
return iTermScaler[axis];
}
void acroPlusApply(acroPlus_t *axisState, int axis, pidProfile_t *pidProfile) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
axisState->wowFactor = 1;
axisState->factor = 0;
if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */
/* Ki scaler axis*/
axisState->iTermScaler = 0.0f;
} else {
/* Prevent rapid windup during acro recoveries */
if (axisState->iTermScaler < 1) {
axisState->iTermScaler = constrainf(axisState->iTermScaler + 0.001f, 0.0f, 1.0f);
} else {
axisState->iTermScaler = 1;
}
}
/* acro plus factor handling */
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor && (!flightModeFlags)) {
axisState->wowFactor = ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f); //0-1f
@ -112,7 +123,7 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static airModePlus_t airModePlusAxisState[3];
static acroPlus_t acroPlusState[3];
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
@ -126,7 +137,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float delta, deltaSum;
int axis, deltaCount;
float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
@ -196,17 +206,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
}
if (preventItermWindup || motorLimitReached) {
if (ABS(errorGyroIf[axis]) > ABS(previousErrorGyroIf[axis])) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -ABS(previousErrorGyroIf[axis]), ABS(previousErrorGyroIf[axis]));
}
errorGyroIf[axis] *= scaleItermToRcInput(axis);
if (motorLimitReached) {
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
previousErrorGyroIf[axis] = errorGyroIf[axis];
}
}
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
@ -241,8 +247,9 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis];
}
#ifdef GTUNE
@ -268,7 +275,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t PTerm, ITerm, DTerm, delta, deltaSum;
static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES];
static int32_t previousErrorGyroI[3], lastGyroRate[3];
static int32_t lastGyroRate[3];
int32_t AngleRateTmp, RateError, gyroRate;
int8_t horizonLevelStrength = 100;
@ -345,20 +352,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
ITerm = errorGyroI[axis] >> 13;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile);
errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler;
}
if (preventItermWindup || motorLimitReached) {
if (ABS(errorGyroI[axis]) > ABS(previousErrorGyroI[axis])) {
errorGyroI[axis] = constrain(errorGyroI[axis], -ABS(previousErrorGyroI[axis]), ABS(previousErrorGyroI[axis]));
}
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
if (motorLimitReached) {
PREVENT_WINDUP(errorGyroIf[axis], previousErrorGyroIf[axis]);
} else {
previousErrorGyroI[axis] = errorGyroI[axis];
}
}
ITerm = errorGyroI[axis] >> 13;
//-----calculate D-term
if (!pidProfile->deltaFromGyro) { // quick and dirty solution for testing
@ -389,8 +392,9 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = lrintf(acroPlusState[axis].factor + acroPlusState[axis].wowFactor * axisPID[axis]);
}
#ifdef GTUNE

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

@ -72,11 +72,10 @@ typedef struct pidProfile_s {
#endif
} pidProfile_t;
typedef struct airModePlus {
typedef struct acroPlus_s {
float factor;
float wowFactor;
float iTermScaler;
} airModePlus_t;
} acroPlus_t;
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

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

@ -120,6 +120,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
return THROTTLE_HIGH;
}
/* TODO - Cleanup
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
{
if (((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND)))
@ -128,6 +129,7 @@ rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
return NOT_CENTERED;
}
*/
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
{

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

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

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

@ -366,6 +366,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOXACROPLUS, "ACRO PLUS;", 29 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -720,6 +721,7 @@ void mspInit(serialConfig_t *serialConfig)
}
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOXACROPLUS;
if (sensors(SENSOR_BARO)) {
activeBoxIds[activeBoxIdCount++] = BOXBARO;

43
src/main/mw.c
View file

@ -103,11 +103,6 @@ enum {
#define GYRO_WATCHDOG_DELAY 100 // Watchdog delay for gyro sync
// AIR MODE Reset timers
#define ERROR_RESET_DEACTIVATE_DELAY (1 * 1000) // 1 sec delay to disable AIR MODE Iterm resetting
bool preventItermWindup = false;
static bool ResetErrorActivated = true;
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
float dT;
@ -482,40 +477,8 @@ void processRx(void)
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
static bool airModeErrorResetIsEnabled = true; // Should always initialize with reset enabled
static uint32_t airModeErrorResetTimeout = 0; // Timeout for both activate and deactivate mode
if (throttleStatus == THROTTLE_LOW) {
// When in AIR Mode LOW Throttle and reset was already disabled we will only prevent further growing
if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled) {
if (calculateRollPitchCenterStatus(&masterConfig.rxConfig) == CENTERED) {
preventItermWindup = true; // Iterm is now limited to the last value
} else {
preventItermWindup = false; // Iterm should considered safe to increase
}
}
// Conditions to reset Error
if (!ARMING_FLAG(ARMED) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled) || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
ResetErrorActivated = true; // As RX code is not executed each loop a flag has to be set for fast looptimes
airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
airModeErrorResetIsEnabled = true; // Enable Reset again especially after Disarm
preventItermWindup = false; // Reset limiting
}
} else {
if (!(feature(FEATURE_MOTOR_STOP)) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (airModeErrorResetIsEnabled) {
if (millis() > airModeErrorResetTimeout && calculateRollPitchCenterStatus(&masterConfig.rxConfig) == NOT_CENTERED) { // Only disable error reset when roll and pitch not centered
airModeErrorResetIsEnabled = false;
preventItermWindup = false; // Reset limiting for Iterm
}
} else {
preventItermWindup = false; // Reset limiting for Iterm
}
} else {
preventItermWindup = false; // Reset limiting. Usefull when flipping between normal and AIR mode
}
ResetErrorActivated = false; // Disable resetting of error
pidResetErrorGyro();
}
// When armed and motors aren't spinning, do beeps and then disarm
@ -723,10 +686,6 @@ void taskMainPidLoop(void)
}
#endif
if (ResetErrorActivated) {
pidResetErrorGyro();
}
// PID - note this is function pointer set by setPIDController()
pid_controller(
&currentProfile->pidProfile,