rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Airmode rework // Fix 3D from negative to positive // cleanup

This commit is contained in:
borisbstyle 2016-02-05 21:38:02 +01:00
parent 18da3d08f8
commit fda0794441
7 changed files with 49 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/config/config.c
View File

@ -331,6 +331,7 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
void resetMixerConfig(mixerConfig_t *mixerConfig) {
mixerConfig->yaw_motor_direction = 1;
mixerConfig->agressive_airmode = 0;
mixerConfig->yaw_jump_prevention_limit = 200;
#ifdef USE_SERVOS
mixerConfig->tri_unarmed_servo = 1;

41
src/main/flight/mixer.c
View File

@ -53,6 +53,12 @@
#include "config/runtime_config.h"
#include "config/config.h"
typedef enum {
THROTTLE_3D_NEUTRAL = 0,
THROTTLE_3D_POSITIVE,
THROTTLE_3D_NEGATIVE
} throttle3dState_e;
uint8_t motorCount;
int16_t motor[MAX_SUPPORTED_MOTORS];
@ -67,8 +73,6 @@ static rxConfig_t *rxConfig;
static mixerMode_e currentMixerMode;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
bool motorLimitReached = false;
#ifdef USE_SERVOS
static uint8_t servoRuleCount = 0;
static servoMixer_t currentServoMixer[MAX_SERVO_RULES];
@ -778,27 +782,33 @@ void mixTable(void)
// Scale roll/pitch/yaw uniformly to fit within throttle range
int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin;
int16_t throttleRange, throttle;
int16_t throttleMin, throttleMax = 0;
static bool flightDirection3dReversed;
int16_t throttleMin, throttleMax;
throttle3dState_e throttle3dPosition = THROTTLE_3D_NEUTRAL; // Initiate in Neutral before all throttle checks
throttle = rcCommand[THROTTLE];
// Find min and max throttle based on condition
if (feature(FEATURE_3D)) {
static int16_t throttleMinPrevious, throttleMaxPrevious, throttlePrevious;
static int16_t throttleMinPrevious = 0, throttleMaxPrevious = 0, throttlePrevious = 0;
if (rcData[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) {
throttleMax = flight3DConfig->deadband3d_low;
throttleMin = escAndServoConfig->minthrottle;
flightDirection3dReversed = true;
throttle3dPosition = THROTTLE_3D_NEGATIVE;
} else if (rcData[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) {
throttleMax = escAndServoConfig->maxthrottle;
throttleMin = flight3DConfig->deadband3d_high;
flightDirection3dReversed = false;
throttle3dPosition = THROTTLE_3D_POSITIVE;
} else {
if (!throttleMin) { /* when starting in neutral */
// when starting in neutral go positive and when coming from positive. Keep positive throttle within deadband
if ((throttle3dPosition == THROTTLE_3D_NEUTRAL && !throttleMinPrevious)
|| (throttle3dPosition == THROTTLE_3D_NEUTRAL && (throttleMinPrevious >= flight3DConfig->deadband3d_high))) {
throttleMax = escAndServoConfig->maxthrottle;
throttle = throttleMin = flight3DConfig->deadband3d_high;
} else {
// When coming from negative. Keep negative throttle within deadband
} else if (throttle3dPosition == THROTTLE_3D_NEUTRAL && (throttleMinPrevious <= flight3DConfig->deadband3d_low)) {
throttleMax = escAndServoConfig->mincommand;
throttle = throttleMin = flight3DConfig->deadband3d_low;
} else {
throttleMax = throttleMaxPrevious;
throttleMin = throttleMinPrevious;
throttle = throttlePrevious;
@ -819,6 +829,9 @@ void mixTable(void)
motorLimitReached = true;
for (i = 0; i < motorCount; i++) {
rollPitchYawMix[i] = (rollPitchYawMix[i] * throttleRange) / rollPitchYawMixRange;
// Get the max correction from center when agressivity enabled. (Some setups don't like this option)
if (mixerConfig->agressive_airmode) throttleMin = throttleMax = throttleMin + (throttleRange / 2);
}
} else {
motorLimitReached = false;
@ -829,15 +842,12 @@ void mixTable(void)
// Now add in the desired throttle, but keep in a range that doesn't clip adjusted
// roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
for (i = 0; i < motorCount; i++) {
motor[i] = rollPitchYawMix[i] + constrainf(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax);
/* Double code. Preparations for full mixer replacement to airMode mixer. Copy from old mixer*/
// TODO - Should probably not be needed for constraining failsafe, but keep it till it is investigated.
if (isFailsafeActive) {
motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle);
} else if (feature(FEATURE_3D)) {
if (flightDirection3dReversed) {
if (throttle3dPosition == THROTTLE_3D_NEGATIVE) {
motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low);
} else {
motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle);
@ -854,6 +864,7 @@ void mixTable(void)
}
}
// Disarmed mode
if (!ARMING_FLAG(ARMED)) {
for (i = 0; i < motorCount; i++) {
motor[i] = motor_disarmed[i];

2
src/main/flight/mixer.h
View File

@ -71,6 +71,7 @@ typedef struct mixer_s {
typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
uint8_t agressive_airmode;
uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100)
#ifdef USE_SERVOS
uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed
@ -189,6 +190,7 @@ extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isMixerUsingServos(void);
void writeServos(void);
void filterServos(void);
bool motorLimitReached;
#endif
extern int16_t motor[MAX_SUPPORTED_MOTORS];

17
src/main/flight/pid.c
View File

@ -50,9 +50,6 @@
extern float dT;
extern bool motorLimitReached;
bool lowThrottleWindupProtection;
#define PREVENT_WINDUP(x,y) { if (ABS(x) > ABS(y)) { if (x < 0) { x = -ABS(y); } else { x = ABS(y); } } }
int16_t axisPID[3];
@ -201,11 +198,11 @@ 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) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroIf[axis] *= scaleItermToRcInput(axis);
if (motorLimitReached || lowThrottleWindupProtection) {
PREVENT_WINDUP(errorGyroIf[axis], errorGyroIfLimit[axis]);
errorGyroIf[axis] = (int32_t) (errorGyroIf[axis] * scaleItermToRcInput(axis));
if (antiWindupProtection || motorLimitReached) {
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
} else {
errorGyroIfLimit[axis] = errorGyroIf[axis];
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
}
}
@ -349,10 +346,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) || IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
errorGyroI[axis] = (int32_t) (errorGyroI[axis] * scaleItermToRcInput(axis));
if (motorLimitReached || lowThrottleWindupProtection) {
PREVENT_WINDUP(errorGyroI[axis], errorGyroILimit[axis]);
if (antiWindupProtection || motorLimitReached) {
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
} else {
errorGyroILimit[axis] = errorGyroI[axis];
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
}
}

1
src/main/flight/pid.h
View File

@ -78,6 +78,7 @@ typedef struct acroPlus_s {
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
bool antiWindupProtection;
void pidSetController(pidControllerType_e type);
void pidResetErrorGyro(void);

1
src/main/io/serial_cli.c
View File

@ -617,6 +617,7 @@ const clivalue_t valueTable[] = {
{ "yaw_control_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.yaw_control_direction, .config.minmax = { -1, 1 } },
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_motor_direction, .config.minmax = { -1, 1 } },
{ "agressive_airmode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.agressive_airmode, .config.lookup = { TABLE_OFF_ON } },
{ "yaw_jump_prevention_limit", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.yaw_jump_prevention_limit, .config.minmax = { YAW_JUMP_PREVENTION_LIMIT_LOW, YAW_JUMP_PREVENTION_LIMIT_HIGH } },
#ifdef USE_SERVOS
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },

16
src/main/mw.c
View File

@ -117,7 +117,7 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
extern uint32_t currentTime;
extern uint8_t PIDweight[3];
extern bool lowThrottleWindupProtection;
extern bool antiWindupProtection;
static bool isRXDataNew;
@ -478,16 +478,22 @@ void processRx(void)
}
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
if (throttleStatus == THROTTLE_LOW) {
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && rollPitchStatus == CENTERED) { /* Anti Windup when roll / pitch stick centered */
lowThrottleWindupProtection = true;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
if (rollPitchStatus == CENTERED) {
antiWindupProtection = true;
} else {
antiWindupProtection = false;
}
} else {
pidResetErrorGyro();
}
} else {
lowThrottleWindupProtection = false;
antiWindupProtection = false;
}
// When armed and motors aren't spinning, do beeps and then disarm