static long cmsx_menuGyro_onExit(const OSD_Entry *self)
{
UNUSED(self);
gyroConfigMutable()->gyroSync = cmsx_gyroSync;
gyroConfigMutable()->gyroSyncDenominator = cmsx_gyroSyncDenom;
gyroConfigMutable()->gyro_lpf = cmsx_gyroLpf;
return 0;
}
static long cmsx_menuGyro_onExit(const OSD_Entry *self)
{
UNUSED(self);
gyroConfigMutable()->gyro_soft_lpf_hz = gyroConfig_gyro_soft_lpf_hz;
gyroConfigMutable()->gyro_soft_notch_hz_1 = gyroConfig_gyro_soft_notch_hz_1;
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
return 0;
}
static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self)
{
UNUSED(self);
pidProfileMutable()->dterm_lpf_hz = cmsx_dterm_lpf_hz;
gyroConfigMutable()->gyro_soft_lpf_hz = cmsx_gyroSoftLpf;
pidProfileMutable()->yaw_p_limit = cmsx_yaw_p_limit;
pidProfileMutable()->yaw_lpf_hz = cmsx_yaw_lpf_hz;
return 0;
}
void targetConfiguration(void)
{
gyroConfigMutable()->looptime = 1000;
rxConfigMutable()->rcmap[0] = 1;
rxConfigMutable()->rcmap[1] = 2;
rxConfigMutable()->rcmap[2] = 3;
rxConfigMutable()->rcmap[3] = 0;
featureSet(FEATURE_VBAT);
featureSet(FEATURE_LED_STRIP);
serialConfigMutable()->portConfigs[0].functionMask = FUNCTION_MSP;
if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
}
}
bool gyroInit(void)
{
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
overflowAxisMask = GYRO_OVERFLOW_Z;
} else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
} else {
overflowAxisMask = 0;
}
#endif
gyroDebugMode = DEBUG_NONE;
useDualGyroDebugging = false;
switch (debugMode) {
case DEBUG_FFT:
case DEBUG_FFT_FREQ:
case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED:
case DEBUG_DYN_LPF:
gyroDebugMode = debugMode;
break;
case DEBUG_DUAL_GYRO:
case DEBUG_DUAL_GYRO_COMBINE:
case DEBUG_DUAL_GYRO_DIFF:
case DEBUG_DUAL_GYRO_RAW:
case DEBUG_DUAL_GYRO_SCALED:
useDualGyroDebugging = true;
break;
}
firstArmingCalibrationWasStarted = false;
gyroDetectionFlags = NO_GYROS_DETECTED;
gyroToUse = gyroConfig()->gyro_to_use;
if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) {
gyroDetectionFlags |= DETECTED_GYRO_1;
}
#if defined(USE_MULTI_GYRO)
if (gyroDetectSensor(&gyroSensor2, gyroDeviceConfig(1))) {
gyroDetectionFlags |= DETECTED_GYRO_2;
}
#endif
if (gyroDetectionFlags == NO_GYROS_DETECTED) {
return false;
}
#if defined(USE_MULTI_GYRO)
if ((gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !(gyroDetectionFlags & DETECTED_BOTH_GYROS))
|| (gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1))
|| (gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) {
if (gyroDetectionFlags & DETECTED_GYRO_1) {
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
} else {
gyroToUse = GYRO_CONFIG_USE_GYRO_2;
}
gyroConfigMutable()->gyro_to_use = gyroToUse;
}
// Only allow using both gyros simultaneously if they are the same hardware type.
if ((gyroDetectionFlags & DETECTED_BOTH_GYROS) && gyroSensor1.gyroDev.gyroHardware == gyroSensor2.gyroDev.gyroHardware) {
gyroDetectionFlags |= DETECTED_DUAL_GYROS;
} else if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = gyroToUse;
}
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1));
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
}
#endif
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0));
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
}
return true;
}
// alternative defaults settings for AlienFlight targets
void targetConfiguration(void)
{
/* depending on revision ... depends on the LEDs to be utilised. */
if (hardwareRevision == AFF3_REV_2) {
statusLedConfigMutable()->inversion = 0
#ifdef LED0_A_INVERTED
| BIT(0)
#endif
#ifdef LED1_A_INVERTED
| BIT(1)
#endif
#ifdef LED2_A_INVERTED
| BIT(2)
#endif
;
for (int i = 0; i < STATUS_LED_NUMBER; i++) {
statusLedConfigMutable()->ioTags[i] = IO_TAG_NONE;
}
#ifdef LED0_A
statusLedConfigMutable()->ioTags[0] = IO_TAG(LED0_A);
#endif
#ifdef LED1_A
statusLedConfigMutable()->ioTags[1] = IO_TAG(LED1_A);
#endif
#ifdef LED2_A
statusLedConfigMutable()->ioTags[2] = IO_TAG(LED2_A);
#endif
} else {
gyroConfigMutable()->gyro_sync_denom = 2;
pidConfigMutable()->pid_process_denom = 2;
}
if (!haveFrSkyRX) {
rxConfigMutable()->serialrx_provider = SERIALRX_SPEKTRUM2048;
rxConfigMutable()->spektrum_sat_bind = 5;
rxConfigMutable()->spektrum_sat_bind_autoreset = 1;
parseRcChannels("TAER1234", rxConfigMutable());
} else {
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->serialrx_inverted = true;
serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIALRX_UART)].functionMask = FUNCTION_TELEMETRY_FRSKY_HUB | FUNCTION_RX_SERIAL;
telemetryConfigMutable()->telemetry_inverted = false;
featureSet(FEATURE_TELEMETRY);
beeperDevConfigMutable()->isOpenDrain = false;
beeperDevConfigMutable()->isInverted = true;
parseRcChannels("AETR1234", rxConfigMutable());
}
if (hardwareMotorType == MOTOR_BRUSHED) {
motorConfigMutable()->dev.motorPwmRate = BRUSHED_MOTORS_PWM_RATE;
pidConfigMutable()->pid_process_denom = 1;
}
for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) {
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->pid[PID_ROLL].P = 90;
pidProfile->pid[PID_ROLL].I = 44;
pidProfile->pid[PID_ROLL].D = 60;
pidProfile->pid[PID_PITCH].P = 90;
pidProfile->pid[PID_PITCH].I = 44;
pidProfile->pid[PID_PITCH].D = 60;
}
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}
void targetConfiguration(void)
{
#ifdef BEEBRAIN
// alternative defaults settings for Beebrain target
motorConfigMutable()->dev.motorPwmRate = 4000;
failsafeConfigMutable()->failsafe_delay = 2;
failsafeConfigMutable()->failsafe_off_delay = 0;
motorConfigMutable()->minthrottle = 1049;
gyroConfigMutable()->gyro_lpf = GYRO_LPF_188HZ;
gyroConfigMutable()->gyro_soft_lpf_hz = 100;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
/*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) {
rxChannelRangeConfigsMutable(channel)->min = 1180;
rxChannelRangeConfigsMutable(channel)->max = 1860;
}*/
for (int profileId = 0; profileId < 2; profileId++) {
pidProfilesMutable(0)->P8[ROLL] = 60;
pidProfilesMutable(0)->I8[ROLL] = 70;
pidProfilesMutable(0)->D8[ROLL] = 17;
pidProfilesMutable(0)->P8[PITCH] = 80;
pidProfilesMutable(0)->I8[PITCH] = 90;
pidProfilesMutable(0)->D8[PITCH] = 18;
pidProfilesMutable(0)->P8[YAW] = 200;
pidProfilesMutable(0)->I8[YAW] = 45;
pidProfilesMutable(0)->P8[PIDLEVEL] = 30;
pidProfilesMutable(0)->D8[PIDLEVEL] = 30;
for (int rateProfileId = 0; rateProfileId < CONTROL_RATE_PROFILE_COUNT; rateProfileId++) {
controlRateProfilesMutable(rateProfileId)->rcRate8 = 100;
controlRateProfilesMutable(rateProfileId)->rcYawRate8 = 110;
controlRateProfilesMutable(rateProfileId)->rcExpo8 = 0;
controlRateProfilesMutable(rateProfileId)->rates[FD_ROLL] = 77;
controlRateProfilesMutable(rateProfileId)->rates[FD_PITCH] = 77;
controlRateProfilesMutable(rateProfileId)->rates[FD_YAW] = 80;
pidProfilesMutable(0)->dtermSetpointWeight = 200;
pidProfilesMutable(0)->setpointRelaxRatio = 50;
}
}
#endif
#if !defined(AFROMINI) && !defined(BEEBRAIN)
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperDevConfigMutable()->isOpenDrain = false;
beeperDevConfigMutable()->isInverted = true;
} else {
beeperDevConfigMutable()->isOpenDrain = true;
beeperDevConfigMutable()->isInverted = false;
flashConfigMutable()->csTag = IO_TAG_NONE;
}
#endif
#ifdef MAG_INT_EXTI
if (hardwareRevision < NAZE32_REV5) {
compassConfigMutable()->interruptTag = IO_TAG(PB12);
}
#endif
}
void targetConfiguration(void)
{
#ifdef BEEBRAIN
// alternative defaults settings for Beebrain target
motorConfigMutable()->dev.motorPwmRate = 4000;
failsafeConfigMutable()->failsafe_delay = 2;
failsafeConfigMutable()->failsafe_off_delay = 0;
motorConfigMutable()->minthrottle = 1049;
gyroConfigMutable()->gyro_hardware_lpf = GYRO_HARDWARE_LPF_1KHZ_SAMPLE;
gyroConfigMutable()->gyro_lowpass_hz = 100;
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
/*for (int channel = 0; channel < NON_AUX_CHANNEL_COUNT; channel++) {
rxChannelRangeConfigsMutable(channel)->min = 1180;
rxChannelRangeConfigsMutable(channel)->max = 1860;
}*/
for (uint8_t pidProfileIndex = 0; pidProfileIndex < MAX_PROFILE_COUNT; pidProfileIndex++) {
pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
pidProfile->pid[PID_ROLL].P = 60;
pidProfile->pid[PID_ROLL].I = 70;
pidProfile->pid[PID_ROLL].D = 17;
pidProfile->pid[PID_PITCH].P = 80;
pidProfile->pid[PID_PITCH].I = 90;
pidProfile->pid[PID_PITCH].D = 18;
pidProfile->pid[PID_YAW].P = 200;
pidProfile->pid[PID_YAW].I = 45;
pidProfile->pid[PID_LEVEL].P = 30;
pidProfile->pid[PID_LEVEL].D = 30;
pidProfile->pid[PID_PITCH].F = 200;
pidProfile->pid[PID_ROLL].F = 200;
pidProfile->feedForwardTransition = 50;
}
for (uint8_t rateProfileIndex = 0; rateProfileIndex < CONTROL_RATE_PROFILE_COUNT; rateProfileIndex++) {
controlRateConfig_t *controlRateConfig = controlRateProfilesMutable(rateProfileIndex);
controlRateConfig->rcRates[FD_ROLL] = 100;
controlRateConfig->rcRates[FD_PITCH] = 100;
controlRateConfig->rcRates[FD_YAW] = 110;
controlRateConfig->rcExpo[FD_ROLL] = 0;
controlRateConfig->rcExpo[FD_PITCH] = 0;
controlRateConfig->rates[FD_ROLL] = 77;
controlRateConfig->rates[FD_PITCH] = 77;
controlRateConfig->rates[FD_YAW] = 80;
}
#endif
#if !defined(AFROMINI) && !defined(BEEBRAIN)
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperDevConfigMutable()->isOpenDrain = false;
beeperDevConfigMutable()->isInverted = true;
} else {
beeperDevConfigMutable()->isOpenDrain = true;
beeperDevConfigMutable()->isInverted = false;
flashConfigMutable()->csTag = IO_TAG_NONE;
}
#endif
#ifdef MAG_INT_EXTI
if (hardwareRevision < NAZE32_REV5) {
compassConfigMutable()->interruptTag = IO_TAG(PB12);
}
#endif
#ifdef BLACKBOX
if (hardwareRevision >= NAZE32_REV5)
blackboxConfigMutable()->device = BLACKBOX_DEVICE_FLASH;
#endif
}
void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
}
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
}
if (gyroConfig()->gyro_use_32khz) {
// F1 and F3 can't handle high sample speed.
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
} else {
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
#endif
}
float samplingTime;
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 9000.0f;
break;
case BMI_160_SPI:
samplingTime = 0.0003125f;
break;
default:
samplingTime = 0.000125f;
break;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 1100.0f;
break;
default:
samplingTime = 0.001f;
break;
}
}
if (gyroConfig()->gyro_use_32khz) {
samplingTime = 0.00003125;
}
// check for looptime restrictions based on motor protocol. Motor times have safety margin
float motorUpdateRestriction;
switch (motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_STANDARD:
motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
break;
case PWM_TYPE_ONESHOT125:
motorUpdateRestriction = 0.0005f;
break;
case PWM_TYPE_ONESHOT42:
motorUpdateRestriction = 0.0001f;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT150:
motorUpdateRestriction = 0.000250f;
break;
case PWM_TYPE_DSHOT300:
motorUpdateRestriction = 0.0001f;
break;
#endif
default:
motorUpdateRestriction = 0.00003125f;
break;
}
if (motorConfig()->dev.useUnsyncedPwm) {
// Prevent overriding the max rate of motors
if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) {
const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
}
} else {
const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
if (pidLooptime < motorUpdateRestriction) {
const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
}
}
}
