void readEEPROM(void) { uint8_t i; // Sanity check if (!validEEPROM()) failureMode(10); // Read flash memcpy(&mcfg, (char *)FLASH_WRITE_ADDR, sizeof(master_t)); // Copy current profile if (mcfg.current_profile > 2) // sanity check mcfg.current_profile = 0; memcpy(&cfg, &mcfg.profile[mcfg.current_profile], sizeof(config_t)); for (i = 0; i < 6; i++) lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t) cfg.rcRate8 / 2500; for (i = 0; i < 11; i++) { int16_t tmp = 10 * i - cfg.thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - cfg.thrMid8; if (tmp < 0) y = cfg.thrMid8; lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t) cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; // [0;1000] lookupThrottleRC[i] = mcfg.minthrottle + (int32_t) (mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [0;1000] -> [MINTHROTTLE;MAXTHROTTLE] } cfg.tri_yaw_middle = constrain(cfg.tri_yaw_middle, cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR setPIDController(cfg.pidController); }
void activateConfig(void) { static imuRuntimeConfig_t imuRuntimeConfig; activateControlRateConfig(); useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile); useGyroConfig(&masterConfig.gyroConfig); #ifdef TELEMETRY useTelemetryConfig(&masterConfig.telemetryConfig); #endif setPIDController(currentProfile->pidController); #ifdef GPS gpsUseProfile(¤tProfile->gpsProfile); gpsUsePIDs(¤tProfile->pidProfile); #endif useFailsafeConfig(¤tProfile->failsafeConfig); setAccelerationTrims(&masterConfig.accZero); mixerUseConfigs( currentProfile->servoConf, &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, ¤tProfile->mixerConfig, &masterConfig.airplaneConfig, &masterConfig.rxConfig, ¤tProfile->gimbalConfig ); imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor; imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor; imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; configureImu(&imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband); configureAltitudeHold(¤tProfile->pidProfile, ¤tProfile->barometerConfig, ¤tProfile->rcControlsConfig, &masterConfig.escAndServoConfig); calculateThrottleAngleScale(currentProfile->throttle_correction_angle); calculateAccZLowPassFilterRCTimeConstant(currentProfile->accz_lpf_cutoff); #ifdef BARO useBarometerConfig(¤tProfile->barometerConfig); #endif }
void activateConfig(void) { static imuRuntimeConfig_t imuRuntimeConfig; generatePitchCurve(¤tProfile.controlRateConfig); generateThrottleCurve(¤tProfile.controlRateConfig, &masterConfig.escAndServoConfig); useGyroConfig(&masterConfig.gyroConfig); useTelemetryConfig(&masterConfig.telemetryConfig); setPIDController(currentProfile.pidController); #ifdef GPS gpsUseProfile(¤tProfile.gpsProfile); gpsUsePIDs(¤tProfile.pidProfile); #endif useFailsafeConfig(¤tProfile.failsafeConfig); setAccelerationTrims(&masterConfig.accZero); mixerUseConfigs( currentProfile.servoConf, &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, ¤tProfile.mixerConfig, &masterConfig.airplaneConfig, &masterConfig.rxConfig, ¤tProfile.gimbalConfig ); imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor; imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor; imuRuntimeConfig.acc_lpf_factor = currentProfile.acc_lpf_factor; imuRuntimeConfig.acc_unarmedcal = currentProfile.acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; configureImu(&imuRuntimeConfig, ¤tProfile.pidProfile, ¤tProfile.barometerConfig, ¤tProfile.accDeadband); calculateThrottleAngleScale(currentProfile.throttle_correction_angle); #ifdef BARO useBarometerConfig(¤tProfile.barometerConfig); #endif }
void activateConfig(void) { uint8_t i; for (i = 0; i < PITCH_LOOKUP_LENGTH; i++) lookupPitchRollRC[i] = (2500 + cfg.rcExpo8 * (i * i - 25)) * i * (int32_t)cfg.rcRate8 / 2500; for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) { int16_t tmp = 10 * i - cfg.thrMid8; uint8_t y = 1; if (tmp > 0) y = 100 - cfg.thrMid8; if (tmp < 0) y = cfg.thrMid8; lookupThrottleRC[i] = 10 * cfg.thrMid8 + tmp * (100 - cfg.thrExpo8 + (int32_t)cfg.thrExpo8 * (tmp * tmp) / (y * y)) / 10; lookupThrottleRC[i] = mcfg.minthrottle + (int32_t)(mcfg.maxthrottle - mcfg.minthrottle) * lookupThrottleRC[i] / 1000; // [MINTHROTTLE;MAXTHROTTLE] } setPIDController(cfg.pidController); #ifdef GPS gpsSetPIDs(); #endif }