Add feature for SuperExpo and Airmode // Super Expo by default activated

This commit is contained in:
borisbstyle 2016-05-30 20:34:33 +02:00
parent 23b0e79eff
commit 3470181a0f
8 changed files with 25 additions and 13 deletions

View File

@ -398,7 +398,7 @@ static void resetConf(void)
#endif
featureSet(FEATURE_FAILSAFE);
featureSet(FEATURE_ONESHOT125);
featureSet(FEATURE_SUPEREXPO);
// global settings
masterConfig.current_profile_index = 0; // default profile
@ -461,8 +461,8 @@ static void resetConf(void)
masterConfig.rxConfig.max_aux_channel = 6;
masterConfig.rxConfig.superExpoFactor = 30;
masterConfig.rxConfig.airModeActivateThreshold = 1350;
masterConfig.rxConfig.superExpoFactorYaw = 30;
masterConfig.rxConfig.superExpoYawMode = 0;
masterConfig.rxConfig.superExpoFactorYaw = 40;
masterConfig.rxConfig.superExpoYawMode = 1;
resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges);

View File

@ -44,6 +44,8 @@ typedef enum {
FEATURE_BLACKBOX = 1 << 19,
FEATURE_CHANNEL_FORWARDING = 1 << 20,
FEATURE_TRANSPONDER = 1 << 21,
FEATURE_AIRMODE = 1 << 22,
FEATURE_SUPEREXPO = 1 << 23,
} features_e;
void handleOneshotFeatureChangeOnRestart(void);

View File

@ -209,7 +209,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 ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (luxPTermScale * kP * tpaFactor) * (AngleRate - gyroRate * calculateExpoPlus(axis, rxConfig));
} else {
PTerm = luxPTermScale * RateError * kP * tpaFactor;
@ -334,7 +334,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 ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
if ((isSuperExpoActive() && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
PTerm = (kP * PIDweight[axis] / 100) * (AngleRateTmp - (int32_t)(gyroRate * calculateExpoPlus(axis, rxConfig))) >> 7;
} else {
PTerm = (RateError * kP * PIDweight[axis] / 100) >> 7;

View File

@ -70,6 +70,13 @@ int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e
bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isSuperExpoActive(void) {
return (IS_RC_MODE_ACTIVE(BOXSUPEREXPO) || feature(FEATURE_SUPEREXPO));
}
void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) {
#ifndef BLACKBOX

View File

@ -243,6 +243,8 @@ typedef struct adjustmentState_s {
#define MAX_ADJUSTMENT_RANGE_COUNT 15
bool isAirmodeActive(void);
bool isSuperExpoActive(void);
void resetAdjustmentStates(void);
void configureAdjustment(uint8_t index, uint8_t auxChannelIndex, const adjustmentConfig_t *adjustmentConfig);
void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges);

View File

@ -194,7 +194,8 @@ static const char * const featureNames[] = {
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125",
"BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", NULL
"BLACKBOX", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE", "SUPEREXPO",
NULL
};
// sync this with rxFailsafeChannelMode_e

View File

@ -542,8 +542,8 @@ void mspInit(serialConfig_t *serialConfig)
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
}
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO;
if (!feature(FEATURE_AIRMODE)) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
if (!feature(FEATURE_SUPEREXPO)) activeBoxIds[activeBoxIdCount++] = BOXSUPEREXPO;
activeBoxIds[activeBoxIdCount++] = BOX3DDISABLESWITCH;
if (sensors(SENSOR_BARO)) {

View File

@ -454,7 +454,7 @@ void updateMagHold(void)
void processRx(void)
{
static bool armedBeeperOn = false;
static bool airmodeIsActivated;
static bool wasAirmodeIsActivated;
calculateRxChannelsAndUpdateFailsafe(currentTime);
@ -477,15 +477,15 @@ void processRx(void)
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset
} else {
airmodeIsActivated = false;
wasAirmodeIsActivated = false;
}
/* 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 && !airmodeIsActivated) {
if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) {
pidResetErrorGyroState();
}