Add feature for SuperExpo and Airmode // Super Expo by default activated
This commit is contained in:
parent
23b0e79eff
commit
3470181a0f
|
@ -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);
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue