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

Redefine rate implementation // new power expo // remove super expo feature

This commit is contained in:
borisbstyle 2016-08-26 02:05:28 +02:00
parent 3fcf206bf5
commit 30175dcba0
9 changed files with 40 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -454,7 +454,7 @@ void createDefaultConfig(master_t *config)
memset(config, 0, sizeof(master_t));
intFeatureClearAll(config);
intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE | FEATURE_SUPEREXPO_RATES, config);
intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , config);
#ifdef DEFAULT_FEATURES
intFeatureSet(DEFAULT_FEATURES, config);
#endif

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

@ -50,7 +50,7 @@ typedef enum {
FEATURE_CHANNEL_FORWARDING = 1 << 20,
FEATURE_TRANSPONDER = 1 << 21,
FEATURE_AIRMODE = 1 << 22,
FEATURE_SUPEREXPO_RATES = 1 << 23,
//FEATURE_SUPEREXPO_RATES = 1 << 23,
FEATURE_VTX = 1 << 24,
FEATURE_RX_NRF24 = 1 << 25,
FEATURE_SOFTSPI = 1 << 26,

1
src/main/debug.h
View File

@ -54,5 +54,6 @@ typedef enum {
DEBUG_RC_INTERPOLATION,
DEBUG_VELOCITY,
DEBUG_DTERM_FILTER,
DEBUG_ANGLERATE,
DEBUG_COUNT
} debugType_e;

4
src/main/io/rc_controls.c
View File

@ -75,10 +75,6 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isSuperExpoActive(void) {
return (feature(FEATURE_SUPEREXPO_RATES));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX
UNUSED(adjustmentFunction);

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

@ -254,7 +254,6 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 15
bool isAirmodeActive(void);
bool isSuperExpoActive(void);
void resetAdjustmentStates(void);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);

6
src/main/io/rc_curves.c
View File

@ -45,12 +45,6 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
}
}
int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate)
{
float tmpf = tmp / 100.0f;
return (int16_t)((2500.0f + (float)expo * (tmpf * tmpf - 25.0f)) * tmpf * (float)(rate) / 2500.0f );
}
int16_t rcLookupThrottle(int32_t tmp)
{
const int32_t tmp2 = tmp / 100;

1
src/main/io/rc_curves.h
View File

@ -19,6 +19,5 @@
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);
int16_t rcLookup(int32_t tmp, uint8_t expo, uint8_t rate);
int16_t rcLookupThrottle(int32_t tmp);

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

@ -487,6 +487,7 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
"RC_INTERPOLATION",
"VELOCITY",
"DFILTER",
"ANGLERATE",
};
#ifdef OSD
@ -801,9 +802,9 @@ const clivalue_t valueTable[] = {
{ "rc_yaw_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rcYawExpo8, .config.minmax = { 0, 100 } },
{ "thr_mid", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrMid8, .config.minmax = { 0, 100 } },
{ "thr_expo", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].thrExpo8, .config.minmax = { 0, 100 } },
{ "roll_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "pitch_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "yaw_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "roll_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_ROLL], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "pitch_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_PITCH], .config.minmax = { 0, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX } },
{ "yaw_srate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } },
{ "tpa_rate", VAR_UINT8 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} },
{ "tpa_breakpoint", VAR_UINT16 | PROFILE_RATE_VALUE, &masterConfig.profile[0].controlRateProfile[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} },
{ "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } },

44
src/main/mw.c
View File

@ -173,22 +173,44 @@ bool isCalibrating()
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
#define RC_RATE_INCREMENTAL 14.54f
float calculateSetpointRate(int axis, int16_t rc) {
float angleRate;
float angleRate, rcRate, rcSuperfactor, rcCommandf;
uint8_t rcExpo;
if (isSuperExpoActive()) {
rcInput[axis] = (axis == YAW) ? (ABS(rc) / (500.0f * (currentControlRateProfile->rcYawRate8 / 100.0f))) : (ABS(rc) / (500.0f * (currentControlRateProfile->rcRate8 / 100.0f)));
float rcFactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate = rcFactor * ((27 * rc) / 16.0f);
if (axis != YAW) {
rcExpo = currentControlRateProfile->rcExpo8;
rcRate = currentControlRateProfile->rcRate8 / 100.0f;
} else {
angleRate = (float)((currentControlRateProfile->rates[axis] + 27) * rc) / 16.0f;
rcExpo = currentControlRateProfile->rcYawExpo8;
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
}
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
rcCommandf = rc / 500.0f;
rcInput[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;
rcCommandf = rcCommandf * (expof * (rcInput[axis] * rcInput[axis] * rcInput[axis]) + rcInput[axis]*(1-expof));
}
angleRate = 200.0f * rcRate * rcCommandf;
if (currentControlRateProfile->rates[axis]) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (rcInput[axis] * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate *= rcSuperfactor;
}
if (debugMode == DEBUG_ANGLERATE) {
debug[axis] = angleRate;
}
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
return constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection
else
return constrainf(angleRate / 4.1f, -1997.0f, 1997.0f); // Rate limit protection (deg/sec)
return constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
}
void scaleRcCommandToFpvCamAngle(void) {
@ -298,14 +320,14 @@ static void updateRcCommands(void)
} else {
tmp = 0;
}
rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcExpo8, currentControlRateProfile->rcRate8);
rcCommand[axis] = tmp;
} else if (axis == YAW) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = rcLookup(tmp, currentControlRateProfile->rcYawExpo8, currentControlRateProfile->rcYawRate8) * -masterConfig.yaw_control_direction;;
rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) {
rcCommand[axis] = -rcCommand[axis];