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

Move acroplus to mixer

This commit is contained in:
borisbstyle 2016-02-19 21:00:16 +01:00
parent fa391507b6
commit 293820f567
6 changed files with 40 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 123;
static const uint8_t EEPROM_CONF_VERSION = 124;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -180,8 +180,6 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->dterm_average_count = 4;
pidProfile->dterm_lpf_hz = 0; // filtering ON by default
pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT;
pidProfile->acroPlusFactor = 30;
pidProfile->acroPlusOffset = 40;
pidProfile->P_f[ROLL] = 1.1f;
pidProfile->I_f[ROLL] = 0.4f;
@ -448,6 +446,8 @@ static void resetConf(void)
masterConfig.rxConfig.rcSmoothing = 0;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 4;
masterConfig.rxConfig.acroPlusFactor = 30;
masterConfig.rxConfig.acroPlusOffset = 40;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

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

@ -749,6 +749,35 @@ STATIC_UNIT_TESTED void servoMixer(void)
#endif
void acroPlusApply(int16_t axisPID[3]) {
int axis;
for (axis = 0; axis < 2; axis++) {
int16_t factor;
q_number_t wowFactor;
int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
int16_t acroPlusStickOffset = rxConfig->acroPlusOffset * 5;
int16_t motorRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle;
if (feature(FEATURE_3D)) motorRange = (motorRange - (flight3DConfig->deadband3d_high - flight3DConfig->deadband3d_low)) / 2;
/* acro plus factor handling */
if (rxConfig->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
if (rcCommandDeflection > 0) {
rcCommandDeflection -= acroPlusStickOffset;
} else {
rcCommandDeflection += acroPlusStickOffset;
}
qConstruct(&wowFactor,ABS(rcCommandDeflection) * rxConfig->acroPlusFactor / 100, 500, Q12_NUMBER);
factor = qMultiply(wowFactor, (rcCommandDeflection * motorRange) / 500);
wowFactor.num = wowFactor.den - wowFactor.num;
} else {
qConstruct(&wowFactor, 1, 1, Q12_NUMBER);
factor = 0;
}
axisPID[axis] = factor + qMultiply(wowFactor, axisPID[axis]);
}
}
void mixTable(void)
{
uint32_t i;
@ -756,6 +785,10 @@ void mixTable(void)
bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code
if (IS_RC_MODE_ACTIVE(BOXACROPLUS)) {
acroPlusApply(axisPID);
}
if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW]));

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

@ -122,29 +122,8 @@ q_number_t scaleItermToRcInput(int axis) {
return qItermScaler[axis];
}
void acroPlusApply(acroPlus_t *axisState, int axis, pidProfile_t *pidProfile) {
int16_t rcCommandDeflection = constrain(rcCommand[axis], -500, 500); // Limit stick input to 500 (rcCommand 100)
int16_t acroPlusStickOffset = pidProfile->acroPlusOffset * 5;
/* acro plus factor handling */
if (pidProfile->acroPlusFactor && ABS(rcCommandDeflection) > acroPlusStickOffset + 10) {
if (rcCommandDeflection > 0) {
rcCommandDeflection -= acroPlusStickOffset;
} else {
rcCommandDeflection += acroPlusStickOffset;
}
qConstruct(&axisState->wowFactor,ABS(rcCommandDeflection) * pidProfile->acroPlusFactor / 100, 500, Q12_NUMBER);
axisState->factor = qMultiply(axisState->wowFactor, rcCommandDeflection << 1); // Max factor 1000 on rcCommand of 500
axisState->wowFactor.num = axisState->wowFactor.den - axisState->wowFactor.num;
} else {
qConstruct(&axisState->wowFactor, 1, 1, Q12_NUMBER);
axisState->factor = 0;
}
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static acroPlus_t acroPlusState[3];
static biquad_t deltaBiQuadState[3];
static bool deltaStateIsSet;
@ -264,11 +243,6 @@ 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(BOXACROPLUS) && axis != YAW) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
@ -380,11 +354,6 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control
axisPID[axis] = PTerm + ITerm + DTerm;
if (IS_RC_MODE_ACTIVE(BOXACROPLUS) && axis != YAW) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
@ -554,11 +523,6 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (IS_RC_MODE_ACTIVE(BOXACROPLUS) && axis != YAW) {
acroPlusApply(&acroPlusState[axis], axis, pidProfile);
axisPID[axis] = acroPlusState[axis].factor + qMultiply(acroPlusState[axis].wowFactor, axisPID[axis]);
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);

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

@ -64,8 +64,6 @@ typedef struct pidProfile_s {
float H_level;
uint8_t H_sensitivity;
uint8_t acroPlusFactor; // Air mode acrobility factor
uint8_t acroPlusOffset; // Air mode stick offset
float dterm_lpf_hz; // Delta Filter in hz
uint8_t deltaMethod; // Alternative delta Calculation
uint16_t yaw_p_limit;
@ -80,11 +78,6 @@ typedef struct pidProfile_s {
#endif
} pidProfile_t;
typedef struct acroPlus_s {
int16_t factor;
q_number_t wowFactor;
} acroPlus_t;
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
bool antiWindupProtection;

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

@ -644,6 +644,8 @@ const clivalue_t valueTable[] = {
{ "yaw_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].controlRateProfile.tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "acro_plus_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusFactor, .config.minmax = {1, 100 } },
{ "acro_plus_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.rxConfig.acroPlusOffset, .config.minmax = {1, 90 } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } },
@ -717,9 +719,6 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "acro_plus_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acroPlusFactor, .config.minmax = {1, 100 } },
{ "acro_plus_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acroPlusOffset, .config.minmax = {1, 90 } },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, .config.minmax = { 1, 32 } },

2
src/main/rx/rx.h
View File

@ -122,6 +122,8 @@ typedef struct rxConfig_s {
uint8_t rcSmoothing;
uint8_t fpvCamAngleDegrees; // Camera angle to be scaled into rc commands
uint8_t max_aux_channel;
uint8_t acroPlusFactor; // Air mode acrobility factor
uint8_t acroPlusOffset; // Air mode stick offset
uint16_t rx_min_usec;
uint16_t rx_max_usec;