void createDefaultConfig(master_t *config) { // Clear all configuration memset(config, 0, sizeof(master_t)); uint32_t *featuresPtr = &config->enabledFeatures; intFeatureClearAll(featuresPtr); intFeatureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE , featuresPtr); #ifdef DEFAULT_FEATURES intFeatureSet(DEFAULT_FEATURES, featuresPtr); #endif #ifdef OSD intFeatureSet(FEATURE_OSD, featuresPtr); osdResetConfig(&config->osdProfile); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. intFeatureSet(FEATURE_VBAT, featuresPtr); #endif config->version = EEPROM_CONF_VERSION; config->mixerMode = MIXER_QUADX; // global settings config->current_profile_index = 0; // default profile config->dcm_kp = 2500; // 1.0 * 10000 config->dcm_ki = 0; // 0.003 * 10000 config->gyro_lpf = 0; // 256HZ default #ifdef STM32F10X config->gyro_sync_denom = 8; config->pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) config->gyro_sync_denom = 1; config->pid_process_denom = 4; #else config->gyro_sync_denom = 4; config->pid_process_denom = 2; #endif config->gyro_soft_type = FILTER_PT1; config->gyro_soft_lpf_hz = 90; config->gyro_soft_notch_hz_1 = 400; config->gyro_soft_notch_cutoff_1 = 300; config->gyro_soft_notch_hz_2 = 200; config->gyro_soft_notch_cutoff_2 = 100; config->debug_mode = DEBUG_NONE; resetAccelerometerTrims(&config->accZero); resetSensorAlignment(&config->sensorAlignmentConfig); config->boardAlignment.rollDegrees = 0; config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; config->acc_hardware = ACC_DEFAULT; // default/autodetect config->max_angle_inclination = 700; // 70 degrees config->yaw_control_direction = 1; config->gyroConfig.gyroMovementCalibrationThreshold = 32; // xxx_hardware: 0:default/autodetect, 1: disable config->mag_hardware = 1; config->baro_hardware = 1; resetBatteryConfig(&config->batteryConfig); #ifndef SKIP_RX_PWM_PPM resetPpmConfig(&config->ppmConfig); resetPwmConfig(&config->pwmConfig); #endif #ifdef TELEMETRY resetTelemetryConfig(&config->telemetryConfig); #endif #ifdef USE_ADC resetAdcConfig(&config->adcConfig); #endif #ifdef BEEPER resetBeeperConfig(&config->beeperConfig); #endif #ifdef SONAR resetSonarConfig(&config->sonarConfig); #endif #ifdef USE_SDCARD intFeatureSet(FEATURE_SDCARD, featuresPtr); resetsdcardConfig(&config->sdcardConfig); #endif #ifdef SERIALRX_PROVIDER config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else config->rxConfig.serialrx_provider = 0; #endif config->rxConfig.rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL; config->rxConfig.sbus_inversion = 1; config->rxConfig.spektrum_sat_bind = 0; config->rxConfig.spektrum_sat_bind_autoreset = 1; config->rxConfig.midrc = 1500; config->rxConfig.mincheck = 1100; config->rxConfig.maxcheck = 1900; config->rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection config->rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &config->rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(config->rxConfig.midrc); } config->rxConfig.rssi_channel = 0; config->rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; config->rxConfig.rssi_ppm_invert = 0; config->rxConfig.rcInterpolation = RC_SMOOTHING_AUTO; config->rxConfig.rcInterpolationInterval = 19; config->rxConfig.fpvCamAngleDegrees = 0; config->rxConfig.max_aux_channel = MAX_AUX_CHANNELS; config->rxConfig.airModeActivateThreshold = 1350; resetAllRxChannelRangeConfigurations(config->rxConfig.channelRanges); config->inputFilteringMode = INPUT_FILTERING_DISABLED; config->gyro_cal_on_first_arm = 0; // TODO - Cleanup retarded arm support config->disarm_kill_switch = 1; config->auto_disarm_delay = 5; config->small_angle = 25; config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo resetMixerConfig(&config->mixerConfig); resetMotorConfig(&config->motorConfig); #ifdef USE_SERVOS resetServoMixerConfig(&config->servoMixerConfig); resetServoConfig(&config->servoConfig); #endif resetFlight3DConfig(&config->flight3DConfig); #ifdef LED_STRIP resetLedStripConfig(&config->ledStripConfig); #endif #ifdef GPS // gps/nav stuff config->gpsConfig.provider = GPS_NMEA; config->gpsConfig.sbasMode = SBAS_AUTO; config->gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; config->gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif resetSerialConfig(&config->serialConfig); resetProfile(&config->profile[0]); resetRollAndPitchTrims(&config->accelerometerTrims); config->mag_declination = 0; config->acc_lpf_hz = 10.0f; config->accDeadband.xy = 40; config->accDeadband.z = 40; config->acc_unarmedcal = 1; #ifdef BARO resetBarometerConfig(&config->barometerConfig); #endif // Radio #ifdef RX_CHANNELS_TAER parseRcChannels("TAER1234", &config->rxConfig); #else parseRcChannels("AETR1234", &config->rxConfig); #endif resetRcControlsConfig(&config->rcControlsConfig); config->throttle_correction_value = 0; // could 10 with althold or 40 for fpv config->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables config->failsafeConfig.failsafe_delay = 10; // 1sec config->failsafeConfig.failsafe_off_delay = 10; // 1sec config->failsafeConfig.failsafe_throttle = 1000; // default throttle off. config->failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss config->failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition config->failsafeConfig.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT;// default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { config->servoConf[i].min = DEFAULT_SERVO_MIN; config->servoConf[i].max = DEFAULT_SERVO_MAX; config->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; config->servoConf[i].rate = 100; config->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; config->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; config->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal config->gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif #ifdef GPS resetGpsProfile(&config->gpsProfile); #endif // custom mixer. clear by defaults. for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { config->customMotorMixer[i].throttle = 0.0f; } #ifdef VTX config->vtx_band = 4; //Fatshark/Airwaves config->vtx_channel = 1; //CH1 config->vtx_mode = 0; //CH+BAND mode config->vtx_mhz = 5740; //F0 #endif #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware memcpy(config->transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); #endif #ifdef BLACKBOX #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) intFeatureSet(FEATURE_BLACKBOX, featuresPtr); config->blackbox_device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) intFeatureSet(FEATURE_BLACKBOX, featuresPtr); config->blackbox_device = BLACKBOX_DEVICE_SDCARD; #else config->blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif config->blackbox_rate_num = 1; config->blackbox_rate_denom = 1; config->blackbox_on_motor_test = 0; // default off #endif // BLACKBOX #ifdef SERIALRX_UART if (featureConfigured(FEATURE_RX_SERIAL)) { int serialIndex = findSerialPortIndexByIdentifier(SERIALRX_UART); if (serialIndex >= 0) { config->serialConfig.portConfigs[serialIndex].functionMask = FUNCTION_RX_SERIAL; } } #endif #if defined(TARGET_CONFIG) targetConfiguration(config); #endif // copy first profile into remaining profile for (int i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&config->profile[i], &config->profile[0], sizeof(profile_t)); } }
// Default settings static void resetConf(void) { int i; #ifdef USE_SERVOS int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 }; ; #endif // Clear all configuration memset(&masterConfig, 0, sizeof(master_t)); setProfile(0); setControlRateProfile(0); masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); #if defined(CJMCU) || defined(SPARKY) featureSet(FEATURE_RX_PPM); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. featureSet(FEATURE_VBAT); #endif featureSet(FEATURE_FAILSAFE); // global settings masterConfig.current_profile_index = 0; // default profile masterConfig.gyro_cmpf_factor = 600; // default MWC masterConfig.gyro_cmpfm_factor = 250; // default MWC masterConfig.gyro_lpf = 42; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead resetAccelerometerTrims(&masterConfig.accZero); resetSensorAlignment(&masterConfig.sensorAlignmentConfig); masterConfig.boardAlignment.rollDegrees = 0; masterConfig.boardAlignment.pitchDegrees = 0; masterConfig.boardAlignment.yawDegrees = 0; masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect masterConfig.max_angle_inclination = 500; // 50 degrees masterConfig.yaw_control_direction = 1; masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; masterConfig.mag_hardware = MAG_DEFAULT; // default/autodetect resetBatteryConfig(&masterConfig.batteryConfig); resetTelemetryConfig(&masterConfig.telemetryConfig); masterConfig.rxConfig.serialrx_provider = 0; masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; masterConfig.rxConfig.maxcheck = 1900; masterConfig.rxConfig.rx_min_usec = 985; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; masterConfig.retarded_arm = 0; masterConfig.disarm_kill_switch = 1; masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; resetMixerConfig(&masterConfig.mixerConfig); masterConfig.airplaneConfig.flaps_speed = 0; masterConfig.airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo resetEscAndServoConfig(&masterConfig.escAndServoConfig); resetFlight3DConfig(&masterConfig.flight3DConfig); #ifdef BRUSHED_MOTORS masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; #else masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif masterConfig.servo_pwm_rate = 50; #ifdef GPS // gps/nav stuff masterConfig.gpsConfig.provider = GPS_NMEA; masterConfig.gpsConfig.sbasMode = SBAS_AUTO; masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF; #endif resetSerialConfig(&masterConfig.serialConfig); masterConfig.looptime = 3500; masterConfig.emf_avoidance = 0; resetPidProfile(¤tProfile->pidProfile); resetControlRateConfig(&masterConfig.controlRateProfiles[0]); // for (i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; resetRollAndPitchTrims(¤tProfile->accelerometerTrims); currentProfile->mag_declination = 0; currentProfile->acc_lpf_factor = 4; currentProfile->accz_lpf_cutoff = 5.0f; currentProfile->accDeadband.xy = 40; currentProfile->accDeadband.z = 40; resetBarometerConfig(¤tProfile->barometerConfig); currentProfile->acc_unarmedcal = 1; // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); resetRcControlsConfig(¤tProfile->rcControlsConfig); currentProfile->throttle_correction_value = 0; // could 10 with althold or 40 for fpv currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. #ifdef USE_SERVOS // servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; currentProfile->servoConf[i].rate = servoRates[i]; currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL; #endif #ifdef GPS resetGpsProfile(¤tProfile->gpsProfile); #endif // custom mixer. clear by defaults. for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMixer[i].throttle = 0.0f; #ifdef LED_STRIP applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); applyDefaultLedStripConfig(masterConfig.ledConfigs); #endif #ifdef BLACKBOX #ifdef SPRACINGF3 featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = 1; #else masterConfig.blackbox_device = 0; #endif masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; #endif // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets #ifdef ALIENWII32 featureSet(FEATURE_RX_SERIAL); featureSet(FEATURE_MOTOR_STOP); featureSet(FEATURE_FAILSAFE); #ifdef ALIENWIIF3 masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; masterConfig.batteryConfig.vbatscale = 20; #else masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; #endif masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; currentProfile->pidProfile.pidController = 3; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; masterConfig.failsafeConfig.failsafe_throttle = 1000; currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rates[FD_PITCH] = 20; currentControlRateProfile->rates[FD_ROLL] = 20; currentControlRateProfile->rates[FD_YAW] = 100; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.5f, 1.0f, -1.0f }, // REAR_R masterConfig.customMixer[0].throttle = 1.0f; masterConfig.customMixer[0].roll = -0.5f; masterConfig.customMixer[0].pitch = 1.0f; masterConfig.customMixer[0].yaw = -1.0f; // { 1.0f, -0.5f, -1.0f, 1.0f }, // FRONT_R masterConfig.customMixer[1].throttle = 1.0f; masterConfig.customMixer[1].roll = -0.5f; masterConfig.customMixer[1].pitch = -1.0f; masterConfig.customMixer[1].yaw = 1.0f; // { 1.0f, 0.5f, 1.0f, 1.0f }, // REAR_L masterConfig.customMixer[2].throttle = 1.0f; masterConfig.customMixer[2].roll = 0.5f; masterConfig.customMixer[2].pitch = 1.0f; masterConfig.customMixer[2].yaw = 1.0f; // { 1.0f, 0.5f, -1.0f, -1.0f }, // FRONT_L masterConfig.customMixer[3].throttle = 1.0f; masterConfig.customMixer[3].roll = 0.5f; masterConfig.customMixer[3].pitch = -1.0f; masterConfig.customMixer[3].yaw = -1.0f; // { 1.0f, -1.0f, -0.5f, -1.0f }, // MIDFRONT_R masterConfig.customMixer[4].throttle = 1.0f; masterConfig.customMixer[4].roll = -1.0f; masterConfig.customMixer[4].pitch = -0.5f; masterConfig.customMixer[4].yaw = -1.0f; // { 1.0f, 1.0f, -0.5f, 1.0f }, // MIDFRONT_L masterConfig.customMixer[5].throttle = 1.0f; masterConfig.customMixer[5].roll = 1.0f; masterConfig.customMixer[5].pitch = -0.5f; masterConfig.customMixer[5].yaw = 1.0f; // { 1.0f, -1.0f, 0.5f, 1.0f }, // MIDREAR_R masterConfig.customMixer[6].throttle = 1.0f; masterConfig.customMixer[6].roll = -1.0f; masterConfig.customMixer[6].pitch = 0.5f; masterConfig.customMixer[6].yaw = 1.0f; // { 1.0f, 1.0f, 0.5f, -1.0f }, // MIDREAR_L masterConfig.customMixer[7].throttle = 1.0f; masterConfig.customMixer[7].roll = 1.0f; masterConfig.customMixer[7].pitch = 0.5f; masterConfig.customMixer[7].yaw = -1.0f; #endif // copy first profile into remaining profile for (i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); } // copy first control rate config into remaining profile for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t)); } for (i = 1; i < MAX_PROFILE_COUNT; i++) { masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT; } }
// Default settings static void resetConf(void) { int i; // Clear all configuration memset(&masterConfig, 0, sizeof(master_t)); setProfile(0); setControlRateProfile(0); masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); persistentFlagClearAll(); featureSet(DEFAULT_RX_FEATURE | FEATURE_FAILSAFE); #ifdef DEFAULT_FEATURES featureSet(DEFAULT_FEATURES); #endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. featureSet(FEATURE_VBAT); #endif // global settings masterConfig.current_profile_index = 0; // default profile masterConfig.dcm_kp_acc = 2500; // 0.25 * 10000 masterConfig.dcm_ki_acc = 50; // 0.005 * 10000 masterConfig.dcm_kp_mag = 10000; // 1.00 * 10000 masterConfig.dcm_ki_mag = 0; // 0.00 * 10000 masterConfig.gyro_lpf = 3; // INV_FILTER_42HZ, In case of ST gyro, will default to 32Hz instead resetAccelerometerTrims(&masterConfig.accZero, &masterConfig.accGain); resetSensorAlignment(&masterConfig.sensorAlignmentConfig); masterConfig.boardAlignment.rollDeciDegrees = 0; masterConfig.boardAlignment.pitchDeciDegrees = 0; masterConfig.boardAlignment.yawDeciDegrees = 0; masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; masterConfig.mag_hardware = MAG_DEFAULT; // default/autodetect masterConfig.baro_hardware = BARO_DEFAULT; // default/autodetect resetBatteryConfig(&masterConfig.batteryConfig); resetTelemetryConfig(&masterConfig.telemetryConfig); masterConfig.rxConfig.serialrx_provider = 0; masterConfig.rxConfig.spektrum_sat_bind = 0; masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; masterConfig.rxConfig.maxcheck = 1900; masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); } masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; masterConfig.rxConfig.rcSmoothing = 1; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED; masterConfig.disarm_kill_switch = 1; masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; resetMixerConfig(&masterConfig.mixerConfig); // Motor/ESC/Servo resetEscAndServoConfig(&masterConfig.escAndServoConfig); resetFlight3DConfig(&masterConfig.flight3DConfig); #ifdef BRUSHED_MOTORS masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; #else masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif masterConfig.servo_pwm_rate = 50; #ifdef GPS // gps/nav stuff masterConfig.gpsConfig.provider = GPS_UBLOX; masterConfig.gpsConfig.sbasMode = SBAS_AUTO; masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_ON; masterConfig.gpsConfig.dynModel = GPS_DYNMODEL_AIR_1G; #endif #ifdef NAV resetNavConfig(&masterConfig.navConfig); #endif resetSerialConfig(&masterConfig.serialConfig); masterConfig.looptime = 2000; masterConfig.emf_avoidance = 0; masterConfig.i2c_overclock = 0; masterConfig.gyroSync = 0; masterConfig.gyroSyncDenominator = 2; resetPidProfile(¤tProfile->pidProfile); resetControlRateConfig(&masterConfig.controlRateProfiles[0]); // for (i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; currentProfile->mag_declination = 0; resetBarometerConfig(&masterConfig.barometerConfig); // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); resetRcControlsConfig(¤tProfile->rcControlsConfig); currentProfile->throttle_tilt_compensation_strength = 0; // 0-100, 0 - disabled // Failsafe Variables masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. masterConfig.failsafeConfig.failsafe_kill_switch = 0; // default failsafe switch action is identical to rc link loss masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition masterConfig.failsafeConfig.failsafe_procedure = 0; // default full failsafe procedure is 0: auto-landing #ifdef USE_SERVOS // servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; currentProfile->servoConf[i].rate = 100; currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif // custom mixer. clear by defaults. for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) masterConfig.customMotorMixer[i].throttle = 0.0f; #ifdef LED_STRIP applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); applyDefaultLedStripConfig(masterConfig.ledConfigs); #endif #ifdef BLACKBOX #ifdef ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = BLACKBOX_DEVICE_FLASH; #else masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; #endif // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.looptime = 1000; masterConfig.rxConfig.rcmap[0] = 1; masterConfig.rxConfig.rcmap[1] = 2; masterConfig.rxConfig.rcmap[2] = 3; masterConfig.rxConfig.rcmap[3] = 0; masterConfig.rxConfig.rcmap[4] = 4; masterConfig.rxConfig.rcmap[5] = 5; masterConfig.rxConfig.rcmap[6] = 6; masterConfig.rxConfig.rcmap[7] = 7; featureSet(FEATURE_ONESHOT125); featureSet(FEATURE_VBAT); featureSet(FEATURE_LED_STRIP); featureSet(FEATURE_FAILSAFE); #endif // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets #ifdef ALIENWII32 #ifdef ALIENWIIF3 masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; masterConfig.batteryConfig.vbatscale = 20; #else masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; #endif masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rates[FD_PITCH] = 20; currentControlRateProfile->rates[FD_ROLL] = 20; currentControlRateProfile->rates[FD_YAW] = 100; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R masterConfig.customMotorMixer[0].throttle = 1.0f; masterConfig.customMotorMixer[0].roll = -0.414178f; masterConfig.customMotorMixer[0].pitch = 1.0f; masterConfig.customMotorMixer[0].yaw = -1.0f; // { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R masterConfig.customMotorMixer[1].throttle = 1.0f; masterConfig.customMotorMixer[1].roll = -0.414178f; masterConfig.customMotorMixer[1].pitch = -1.0f; masterConfig.customMotorMixer[1].yaw = 1.0f; // { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L masterConfig.customMotorMixer[2].throttle = 1.0f; masterConfig.customMotorMixer[2].roll = 0.414178f; masterConfig.customMotorMixer[2].pitch = 1.0f; masterConfig.customMotorMixer[2].yaw = 1.0f; // { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L masterConfig.customMotorMixer[3].throttle = 1.0f; masterConfig.customMotorMixer[3].roll = 0.414178f; masterConfig.customMotorMixer[3].pitch = -1.0f; masterConfig.customMotorMixer[3].yaw = -1.0f; // { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R masterConfig.customMotorMixer[4].throttle = 1.0f; masterConfig.customMotorMixer[4].roll = -1.0f; masterConfig.customMotorMixer[4].pitch = -0.414178f; masterConfig.customMotorMixer[4].yaw = -1.0f; // { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L masterConfig.customMotorMixer[5].throttle = 1.0f; masterConfig.customMotorMixer[5].roll = 1.0f; masterConfig.customMotorMixer[5].pitch = -0.414178f; masterConfig.customMotorMixer[5].yaw = 1.0f; // { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R masterConfig.customMotorMixer[6].throttle = 1.0f; masterConfig.customMotorMixer[6].roll = -1.0f; masterConfig.customMotorMixer[6].pitch = 0.414178f; masterConfig.customMotorMixer[6].yaw = 1.0f; // { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L masterConfig.customMotorMixer[7].throttle = 1.0f; masterConfig.customMotorMixer[7].roll = 1.0f; masterConfig.customMotorMixer[7].pitch = 0.414178f; masterConfig.customMotorMixer[7].yaw = -1.0f; #endif // copy first profile into remaining profile for (i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); } // copy first control rate config into remaining profile for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t)); } for (i = 1; i < MAX_PROFILE_COUNT; i++) { masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT; } }
// Default settings STATIC_UNIT_TESTED void resetConf(void) { int i; // Clear all configuration memset(&masterConfig, 0, sizeof(master_t)); setProfile(0); setControlRateProfile(0); masterConfig.version = EEPROM_CONF_VERSION; masterConfig.mixerMode = MIXER_QUADX; featureClearAll(); #if defined(CJMCU) || defined(SPARKY) || defined(COLIBRI_RACE) || defined(MOTOLAB) || defined(SPRACINGF3MINI) || defined(LUX_RACE) featureSet(FEATURE_RX_PPM); #endif //#if defined(SPRACINGF3MINI) // featureSet(FEATURE_DISPLAY); //#endif #ifdef BOARD_HAS_VOLTAGE_DIVIDER // only enable the VBAT feature by default if the board has a voltage divider otherwise // the user may see incorrect readings and unexpected issues with pin mappings may occur. featureSet(FEATURE_VBAT); #endif featureSet(FEATURE_FAILSAFE); // global settings masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.gyro_lpf = 1; // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead masterConfig.soft_gyro_lpf_hz = 60; // Software based lpf filter for gyro masterConfig.max_angle_inclination = 500; // 50 degrees masterConfig.yaw_control_direction = 1; masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; resetBatteryConfig(&masterConfig.batteryConfig); #ifdef TELEMETRY masterConfig.telemetryConfig.hottAlarmSoundInterval = 5; #endif masterConfig.rxConfig.sbus_inversion = 1; masterConfig.rxConfig.midrc = 1500; masterConfig.rxConfig.mincheck = 1100; masterConfig.rxConfig.maxcheck = 1900; masterConfig.rxConfig.rx_min_usec = 885; // any of first 4 channels below this value will trigger rx loss detection masterConfig.rxConfig.rx_max_usec = 2115; // any of first 4 channels above this value will trigger rx loss detection for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { rxFailsafeChannelConfiguration_t *channelFailsafeConfiguration = &masterConfig.rxConfig.failsafe_channel_configurations[i]; channelFailsafeConfiguration->mode = (i < NON_AUX_CHANNEL_COUNT) ? RX_FAILSAFE_MODE_AUTO : RX_FAILSAFE_MODE_HOLD; channelFailsafeConfiguration->step = (i == THROTTLE) ? CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.rx_min_usec) : CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc); } masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); masterConfig.disarm_kill_switch = 1; masterConfig.auto_disarm_delay = 5; masterConfig.small_angle = 25; resetMixerConfig(&masterConfig.mixerConfig); masterConfig.airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo resetEscAndServoConfig(&masterConfig.escAndServoConfig); resetFlight3DConfig(&masterConfig.flight3DConfig); #ifdef BRUSHED_MOTORS masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE; #else masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE; #endif masterConfig.servo_pwm_rate = 50; #ifdef GPS // gps/nav stuff masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON; #endif resetSerialConfig(&masterConfig.serialConfig); masterConfig.looptime = 2000; masterConfig.i2c_highspeed = 1; masterConfig.gyroSync = 1; masterConfig.gyroSyncDenominator = 1; resetPidProfile(¤tProfile->pidProfile); resetControlRateConfig(&masterConfig.controlRateProfiles[0]); // for (i = 0; i < CHECKBOXITEMS; i++) // cfg.activate[i] = 0; resetRollAndPitchTrims(¤tProfile->accelerometerTrims); currentProfile->mag_declination = 0; currentProfile->acc_cut_hz = 15; currentProfile->accz_lpf_cutoff = 5.0f; currentProfile->accDeadband.xy = 40; currentProfile->accDeadband.z = 40; currentProfile->acc_unarmedcal = 1; #ifdef BARO resetBarometerConfig(¤tProfile->barometerConfig); #endif // Radio parseRcChannels("AETR1234", &masterConfig.rxConfig); resetRcControlsConfig(¤tProfile->rcControlsConfig); currentProfile->throttle_correction_value = 0; // could 10 with althold or 40 for fpv currentProfile->throttle_correction_angle = 800; // could be 80.0 deg with atlhold or 45.0 for fpv // Failsafe Variables masterConfig.failsafeConfig.failsafe_delay = 10; // 1sec masterConfig.failsafeConfig.failsafe_off_delay = 200; // 20sec masterConfig.failsafeConfig.failsafe_throttle = 1000; // default throttle off. masterConfig.failsafeConfig.failsafe_throttle_low_delay = 100; // default throttle low delay for "just disarm" on failsafe condition #ifdef USE_SERVOS // servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN; currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX; currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE; currentProfile->servoConf[i].rate = 100; currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE; currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE; currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED; } // gimbal currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL; #endif #ifdef GPS resetGpsProfile(¤tProfile->gpsProfile); #endif #ifdef LED_STRIP applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT); applyDefaultLedStripConfig(masterConfig.ledConfigs); #endif #ifdef TRANSPONDER static const uint8_t defaultTransponderData[6] = { 0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC }; // Note, this is NOT a valid transponder code, it's just for testing production hardware memcpy(masterConfig.transponderData, &defaultTransponderData, sizeof(defaultTransponderData)); #endif #ifdef BLACKBOX #if defined(ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT) featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = BLACKBOX_DEVICE_FLASH; #elif defined(ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT) featureSet(FEATURE_BLACKBOX); masterConfig.blackbox_device = BLACKBOX_DEVICE_SDCARD; #else masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; #endif masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; #endif // alternative defaults settings for COLIBRI RACE targets #if defined(COLIBRI_RACE) masterConfig.looptime = 1000; currentProfile->pidProfile.pidController = 1; masterConfig.rxConfig.rcmap[0] = 1; masterConfig.rxConfig.rcmap[1] = 2; masterConfig.rxConfig.rcmap[2] = 3; masterConfig.rxConfig.rcmap[3] = 0; masterConfig.rxConfig.rcmap[4] = 4; masterConfig.rxConfig.rcmap[5] = 5; masterConfig.rxConfig.rcmap[6] = 6; masterConfig.rxConfig.rcmap[7] = 7; featureSet(FEATURE_ONESHOT125); featureSet(FEATURE_VBAT); featureSet(FEATURE_LED_STRIP); featureSet(FEATURE_FAILSAFE); #endif // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets #ifdef ALIENWII32 featureSet(FEATURE_RX_SERIAL); featureSet(FEATURE_MOTOR_STOP); #ifdef ALIENWIIF3 masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL; masterConfig.batteryConfig.vbatscale = 20; #else masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL; #endif masterConfig.rxConfig.serialrx_provider = 1; masterConfig.rxConfig.spektrum_sat_bind = 5; masterConfig.escAndServoConfig.minthrottle = 1000; masterConfig.escAndServoConfig.maxthrottle = 2000; masterConfig.motor_pwm_rate = 32000; masterConfig.looptime = 2000; currentProfile->pidProfile.pidController = 3; currentProfile->pidProfile.P8[ROLL] = 36; currentProfile->pidProfile.P8[PITCH] = 36; masterConfig.failsafeConfig.failsafe_delay = 2; masterConfig.failsafeConfig.failsafe_off_delay = 0; currentControlRateProfile->rcRate8 = 130; currentControlRateProfile->rates[FD_PITCH] = 20; currentControlRateProfile->rates[FD_ROLL] = 20; currentControlRateProfile->rates[FD_YAW] = 100; parseRcChannels("TAER1234", &masterConfig.rxConfig); // { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R masterConfig.customMotorMixer[0].throttle = 1.0f; masterConfig.customMotorMixer[0].roll = -0.414178f; masterConfig.customMotorMixer[0].pitch = 1.0f; masterConfig.customMotorMixer[0].yaw = -1.0f; // { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R masterConfig.customMotorMixer[1].throttle = 1.0f; masterConfig.customMotorMixer[1].roll = -0.414178f; masterConfig.customMotorMixer[1].pitch = -1.0f; masterConfig.customMotorMixer[1].yaw = 1.0f; // { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L masterConfig.customMotorMixer[2].throttle = 1.0f; masterConfig.customMotorMixer[2].roll = 0.414178f; masterConfig.customMotorMixer[2].pitch = 1.0f; masterConfig.customMotorMixer[2].yaw = 1.0f; // { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L masterConfig.customMotorMixer[3].throttle = 1.0f; masterConfig.customMotorMixer[3].roll = 0.414178f; masterConfig.customMotorMixer[3].pitch = -1.0f; masterConfig.customMotorMixer[3].yaw = -1.0f; // { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R masterConfig.customMotorMixer[4].throttle = 1.0f; masterConfig.customMotorMixer[4].roll = -1.0f; masterConfig.customMotorMixer[4].pitch = -0.414178f; masterConfig.customMotorMixer[4].yaw = -1.0f; // { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L masterConfig.customMotorMixer[5].throttle = 1.0f; masterConfig.customMotorMixer[5].roll = 1.0f; masterConfig.customMotorMixer[5].pitch = -0.414178f; masterConfig.customMotorMixer[5].yaw = 1.0f; // { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R masterConfig.customMotorMixer[6].throttle = 1.0f; masterConfig.customMotorMixer[6].roll = -1.0f; masterConfig.customMotorMixer[6].pitch = 0.414178f; masterConfig.customMotorMixer[6].yaw = 1.0f; // { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L masterConfig.customMotorMixer[7].throttle = 1.0f; masterConfig.customMotorMixer[7].roll = 1.0f; masterConfig.customMotorMixer[7].pitch = 0.414178f; masterConfig.customMotorMixer[7].yaw = -1.0f; #endif // copy first profile into remaining profile for (i = 1; i < MAX_PROFILE_COUNT; i++) { memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t)); } // copy first control rate config into remaining profile for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t)); } for (i = 1; i < MAX_PROFILE_COUNT; i++) { masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT; } }