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

Merge remote-tracking branch 'borisbstyle/betaflight' into betaflight

This commit is contained in:
Gary Keeble 2016-06-01 13:46:27 +01:00
parent 2cc6262776 d4c22f1c28
commit 6096e85bb4
10 changed files with 91 additions and 160 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
src/main/blackbox/blackbox.c
View File

@ -1195,146 +1195,140 @@ static bool blackboxWriteSysinfo()
case 18:
blackboxPrintfHeaderLine("tpa_breakpoint:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].tpa_breakpoint);
break;
case 19:
blackboxPrintfHeaderLine("superExpoFactor:%d, %d", masterConfig.rxConfig.superExpoFactor, masterConfig.rxConfig.superExpoFactorYaw);
break;
case 20:
case 21:
blackboxPrintfHeaderLine("rates:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[ROLL],
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[PITCH],
masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rates[YAW]);
break;
case 21:
case 22:
blackboxPrintfHeaderLine("looptime:%d", targetLooptime);
break;
case 22:
case 23:
blackboxPrintfHeaderLine("pidController:%d", masterConfig.profile[masterConfig.current_profile_index].pidProfile.pidController);
break;
case 23:
case 24:
blackboxPrintfHeaderLine("rollPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[ROLL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[ROLL]);
break;
case 24:
case 25:
blackboxPrintfHeaderLine("pitchPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PITCH],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PITCH]);
break;
case 25:
case 26:
blackboxPrintfHeaderLine("yawPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[YAW],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[YAW]);
break;
case 26:
case 27:
blackboxPrintfHeaderLine("altPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDALT],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDALT]);
break;
case 27:
case 28:
blackboxPrintfHeaderLine("posPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOS],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOS]);
break;
case 28:
case 29:
blackboxPrintfHeaderLine("posrPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDPOSR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDPOSR]);
break;
case 29:
case 30:
blackboxPrintfHeaderLine("navrPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDNAVR],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDNAVR]);
break;
case 30:
case 31:
blackboxPrintfHeaderLine("levelPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDLEVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDLEVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDLEVEL]);
break;
case 31:
case 32:
blackboxPrintfHeaderLine("magPID:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDMAG]);
break;
case 32:
case 33:
blackboxPrintfHeaderLine("velPID:%d,%d,%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.P8[PIDVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.I8[PIDVEL],
masterConfig.profile[masterConfig.current_profile_index].pidProfile.D8[PIDVEL]);
break;
case 33:
case 34:
blackboxPrintfHeaderLine("yaw_p_limit:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_p_limit);
break;
case 34:
case 35:
blackboxPrintfHeaderLine("yaw_lpf_hz:%d",
(int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.yaw_lpf_hz * 100.0f));
break;
case 35:
case 36:
blackboxPrintfHeaderLine("dterm_average_count:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
break;
case 36:
case 37:
blackboxPrintfHeaderLine("dynamic_pid:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid);
break;
case 37:
case 38:
blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate);
break;
case 38:
case 39:
blackboxPrintfHeaderLine("yawItermResetRate:%d",
masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate);
break;
case 39:
case 40:
blackboxPrintfHeaderLine("dterm_lpf_hz:%d",
(int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
break;
case 40:
case 41:
blackboxPrintfHeaderLine("airmode_activate_throttle:%d",
masterConfig.rxConfig.airModeActivateThreshold);
break;
case 41:
case 42:
blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband);
break;
case 42:
case 43:
blackboxPrintfHeaderLine("yaw_deadband:%d", masterConfig.rcControlsConfig.yaw_deadband);
break;
case 43:
case 44:
blackboxPrintfHeaderLine("gyro_lpf:%d", masterConfig.gyro_lpf);
break;
case 44:
case 45:
blackboxPrintfHeaderLine("gyro_lowpass_hz:%d", (int)(masterConfig.gyro_soft_lpf_hz * 100.0f));
break;
case 45:
case 46:
blackboxPrintfHeaderLine("acc_lpf_hz:%d", (int)(masterConfig.acc_lpf_hz * 100.0f));
break;
case 46:
case 47:
blackboxPrintfHeaderLine("acc_hardware:%d", masterConfig.acc_hardware);
break;
case 47:
case 48:
blackboxPrintfHeaderLine("baro_hardware:%d", masterConfig.baro_hardware);
break;
case 48:
case 49:
blackboxPrintfHeaderLine("mag_hardware:%d", masterConfig.mag_hardware);
break;
case 49:
case 50:
blackboxPrintfHeaderLine("gyro_cal_on_first_arm:%d", masterConfig.gyro_cal_on_first_arm);
break;
case 50:
case 51:
blackboxPrintfHeaderLine("vbat_pid_compensation:%d", masterConfig.batteryConfig.vbatPidCompensation);
break;
case 51:
blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing);
break;
case 52:
blackboxPrintfHeaderLine("superExpoYawMode:%d", masterConfig.rxConfig.superExpoYawMode);
blackboxPrintfHeaderLine("rc_smoothing:%d", masterConfig.rxConfig.rcSmoothing);
break;
case 53:
blackboxPrintfHeaderLine("features:%d", masterConfig.enabledFeatures);

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

@ -141,7 +141,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 138;
static const uint8_t EEPROM_CONF_VERSION = 139;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
@ -310,15 +310,15 @@ void resetSerialConfig(serialConfig_t *serialConfig)
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
controlRateConfig->rcRate8 = 100;
controlRateConfig->rcExpo8 = 70;
controlRateConfig->rcExpo8 = 10;
controlRateConfig->thrMid8 = 50;
controlRateConfig->thrExpo8 = 0;
controlRateConfig->dynThrPID = 20;
controlRateConfig->rcYawExpo8 = 20;
controlRateConfig->rcYawExpo8 = 10;
controlRateConfig->tpa_breakpoint = 1650;
for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
controlRateConfig->rates[axis] = 50;
controlRateConfig->rates[axis] = 70;
}
}
@ -460,10 +460,7 @@ static void resetConf(void)
masterConfig.rxConfig.rcSmoothing = 0;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.superExpoFactor = 30;
masterConfig.rxConfig.airModeActivateThreshold = 1350;
masterConfig.rxConfig.superExpoFactorYaw = 40;
masterConfig.rxConfig.superExpoYawMode = 1;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);
@ -746,8 +743,6 @@ static bool isEEPROMContentValid(void)
void activateControlRateConfig(void)
{
generatePitchRollCurve(currentControlRateProfile);
generateYawCurve(currentControlRateProfile);
generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
}

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

@ -74,19 +74,19 @@ void setTargetPidLooptime(uint8_t pidProcessDenom) {
targetPidLooptime = targetLooptime * pidProcessDenom;
}
float calculateExpoPlus(int axis, const rxConfig_t *rxConfig, const controlRateConfig_t *controlRateConfig) {
float propFactor, superExpoFactor, rcFactor;
float calculateRate(int axis, const controlRateConfig_t *controlRateConfig) {
float angleRate;
if (axis == YAW && !rxConfig->superExpoYawMode) {
propFactor = 1.0f;
if (isSuperExpoActive()) {
float rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
rcFactor = 1.0f / (1.0f - (rcFactor * (controlRateConfig->rates[axis] / 100.0f)));
angleRate = rcFactor * ((27 * rcCommand[axis]) / 16.0f);
} else {
superExpoFactor = (axis == YAW) ? rxConfig->superExpoFactorYaw : rxConfig->superExpoFactor;
rcFactor = (axis == YAW) ? (ABS(rcCommand[axis]) / 500.0f) : (ABS(rcCommand[axis]) / (500.0f * (controlRateConfig->rcRate8 / 100.0f)));
propFactor = constrainf(1.0f - ((superExpoFactor / 100.0f) * rcFactor * rcFactor * rcFactor), 0.0f, 1.0f);
angleRate = (float)((controlRateConfig->rates[axis] + 27) * rcCommand[axis]) / 16.0f;
}
return propFactor;
return angleRate;
}
uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) {
@ -169,31 +169,26 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
AngleRate = (float)((rate + 47) * rcCommand[YAW]) / 32.0f;
} else {
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to the max inclination
// ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
AngleRate = calculateRate(axis, controlRateConfig);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to the max inclination
#ifdef GPS
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
const float errorAngle = (constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#else
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
const float errorAngle = (constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]); // 16 bits is ok here
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
} else {
// HORIZON mode - direct sticks control is applied to rate PID
// mix up angle error to desired AngleRate to add a little auto-level feel
AngleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
}
}
@ -208,11 +203,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// -----calculate P component
if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig, controlRateConfig));
} else {
PTerm = luxPTermScale * RateError * kP * tpaFactor;
}
PTerm = luxPTermScale * RateError * kP * tpaFactor;
// 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) {
@ -295,31 +286,26 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
// ----------PID controller----------
for (axis = 0; axis < 3; axis++) {
uint8_t rate = controlRateConfig->rates[axis];
// -----Get the desired angle rate depending on flight mode
if (axis == FD_YAW) {
// YAW is always gyro-controlled (MAG correction is applied to rcCommand)
AngleRateTmp = ((int32_t)(rate + 47) * rcCommand[YAW]) >> 5;
} else {
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to max configured inclination
AngleRateTmp = (int32_t)calculateRate(axis, controlRateConfig);
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
// calculate error angle and limit the angle to max configured inclination
#ifdef GPS
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#else
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
#endif
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
} else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
}
if (FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
} else {
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
// horizonLevelStrength is scaled to the stick input
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
}
}
@ -333,11 +319,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
// -----calculate P component
if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig, controlRateConfig))) >> 7;
} else {
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;
}
PTerm = (RateError * kP * 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) {

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

@ -76,7 +76,7 @@ bool isAirmodeActive(void) {
}
bool isSuperExpoActive(void) {
return (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) || feature(FEATURE_SUPEREXPO));
return (feature(FEATURE_SUPEREXPO));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
@ -503,13 +503,11 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
case ADJUSTMENT_RC_RATE:
newValue = constrain((int)controlRateConfig->rcRate8 + delta, 0, 250); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcRate8 = newValue;
generatePitchRollCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_RATE, newValue);
break;
case ADJUSTMENT_RC_EXPO:
newValue = constrain((int)controlRateConfig->rcExpo8 + delta, 0, 100); // FIXME magic numbers repeated in serial_cli.c
controlRateConfig->rcExpo8 = newValue;
generatePitchRollCurve(controlRateConfig);
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_RC_EXPO, newValue);
break;
case ADJUSTMENT_THROTTLE_EXPO:

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

@ -26,37 +26,9 @@
#include "config/config.h"
#define PITCH_LOOKUP_LENGTH 31
#define YAW_LOOKUP_LENGTH 31
#define THROTTLE_LOOKUP_LENGTH 12
static int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
static int16_t lookupYawRC[YAW_LOOKUP_LENGTH]; // lookup table for expo & RC rate YAW
static int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
{
uint8_t i;
float j = 0;
for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) {
lookupPitchRollRC[i] = (2500 + controlRateConfig->rcExpo8 * (j * j - 25)) * j * (int32_t) controlRateConfig->rcRate8 / 2500;
j += 0.2f;
}
}
void generateYawCurve(controlRateConfig_t *controlRateConfig)
{
uint8_t i;
float j = 0;
for (i = 0; i < YAW_LOOKUP_LENGTH; i++) {
lookupYawRC[i] = (2500 + controlRateConfig->rcYawExpo8 * (j * j - 25)) * j / 25;
j += 0.2f;
}
}
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig)
{
uint8_t i;
@ -74,16 +46,16 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
}
}
int16_t rcLookupPitchRoll(int32_t tmp)
int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig)
{
const int32_t tmp2 = tmp / 20;
return lookupPitchRollRC[tmp2] + (tmp - tmp2 * 20) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 20;
float tmpf = tmp / 100.0f;
return (int16_t)((2500.0f + (float)controlRateConfig->rcExpo8 * (tmpf * tmpf - 25.0f)) * tmpf * (float)(controlRateConfig->rcRate8) / 2500.0f );
}
int16_t rcLookupYaw(int32_t tmp)
int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig)
{
const int32_t tmp2 = tmp / 20;
return lookupYawRC[tmp2] + (tmp - tmp2 * 20) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 20;
float tmpf = tmp / 100.0f;
return (int16_t)((2500.0f + (float)controlRateConfig->rcYawExpo8* (tmpf * tmpf - 25.0f)) * tmpf / 25.0f );
}
int16_t rcLookupThrottle(int32_t tmp)

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

@ -17,11 +17,9 @@
#pragma once
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateYawCurve(controlRateConfig_t *controlRateConfig);
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);
int16_t rcLookupPitchRoll(int32_t tmp);
int16_t rcLookupYaw(int32_t tmp);
int16_t rcLookupPitchRoll(int32_t tmp, controlRateConfig_t *controlRateConfig);
int16_t rcLookupYaw(int32_t tmp, controlRateConfig_t *controlRateConfig);
int16_t rcLookupThrottle(int32_t tmp);

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

@ -704,9 +704,6 @@ const clivalue_t valueTable[] = {
{ "yaw_rate", 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} },
{ "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
{ "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } },
{ "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } },
{ "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } },
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },

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

@ -216,7 +216,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXBLACKBOX, "BLACKBOX;", 26 },
{ BOXFAILSAFE, "FAILSAFE;", 27 },
{ BOXAIRMODE, "AIR MODE;", 28 },
{ BOXSUPEREXPO, "SUPER EXPO;", 29 },
//{ BOXSUPEREXPO, "SUPER EXPO;", 29 },
{ BOX3DDISABLESWITCH, "DISABLE 3D SWITCH;", 30},
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -544,7 +544,6 @@ void mspInit(serialConfig_t *serialConfig)
}
if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
if (!feature(FEATURE_SUPEREXPO)) activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
if (sensors(SENSOR_BARO)) {
@ -650,8 +649,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) << BOXSUPEREXPO;
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE;
for (i = 0; i < activeBoxIdCount; i++) {
int flag = (tmp & (1 << activeBoxIds[i]));

4
src/main/mw.c
View File

@ -244,14 +244,14 @@ static void updateRcCommands(void)
} else {
tmp = 0;
}
rcCommand[axis] = rcLookupPitchRoll(tmp);
rcCommand[axis] = rcLookupPitchRoll(tmp, currentControlRateProfile);
} else if (axis == YAW) {
if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
tmp -= masterConfig.rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
rcCommand[axis] = rcLookupYaw(tmp) * -masterConfig.yaw_control_direction;
rcCommand[axis] = rcLookupYaw(tmp, currentControlRateProfile) * -masterConfig.yaw_control_direction;
}
if (rcData[axis] < masterConfig.rxConfig.midrc) {
rcCommand[axis] = -rcCommand[axis];

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

@ -124,9 +124,6 @@ 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 superExpoFactor; // Super Expo Factor
uint8_t superExpoFactorYaw; // Super Expo Factor Yaw
uint8_t superExpoYawMode; // Seperate Super expo for yaw
uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated
uint16_t rx_min_usec;