src/main/config/config.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #include "build_config.h"
  25
  26 #include "common/color.h"
  27 #include "common/axis.h"
  28 #include "common/maths.h"
  29
  30 #include "drivers/sensor.h"
  31 #include "drivers/accgyro.h"
  32 #include "drivers/compass.h"
  33 #include "drivers/system.h"
  34 #include "drivers/gpio.h"
  35 #include "drivers/timer.h"
  36 #include "drivers/pwm_rx.h"
  37 #include "drivers/serial.h"
  38
  39 #include "sensors/sensors.h"
  40 #include "sensors/gyro.h"
  41 #include "sensors/compass.h"
  42 #include "sensors/acceleration.h"
  43 #include "sensors/barometer.h"
  44 #include "sensors/boardalignment.h"
  45 #include "sensors/battery.h"
  46
  47 #include "io/beeper.h"
  48 #include "io/serial.h"
  49 #include "io/gimbal.h"
  50 #include "io/escservo.h"
  51 #include "io/rc_controls.h"
  52 #include "io/rc_curves.h"
  53 #include "io/ledstrip.h"
  54 #include "io/gps.h"
  55
  56 #include "rx/rx.h"
  57
  58 #include "telemetry/telemetry.h"
  59
  60 #include "flight/mixer.h"
  61 #include "flight/pid.h"
  62 #include "flight/imu.h"
  63 #include "flight/failsafe.h"
  64 #include "flight/altitudehold.h"
  65 #include "flight/navigation.h"
  66
  67 #include "config/runtime_config.h"
  68 #include "config/config.h"
  69
  70 #include "config/config_profile.h"
  71 #include "config/config_master.h"
  72
  73 #define BRUSHED_MOTORS_PWM_RATE 16000
  74 #define BRUSHLESS_MOTORS_PWM_RATE 400
  75
  76 void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
  77
  78 #define FLASH_TO_RESERVE_FOR_CONFIG 0x800
  79
  80 #if !defined(FLASH_SIZE)
  81 #error "Flash size not defined for target. (specify in KB)"
  82 #endif
  83
  84
  85 #ifndef FLASH_PAGE_SIZE
  86     #ifdef STM32F303xC
  87         #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
  88     #endif
  89
  90     #ifdef STM32F10X_MD
  91         #define FLASH_PAGE_SIZE                 ((uint16_t)0x400)
  92     #endif
  93
  94     #ifdef STM32F10X_HD
  95         #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
  96     #endif
  97 #endif
  98
  99 #if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
 100     #ifdef STM32F10X_MD
 101         #define FLASH_PAGE_COUNT 128
 102     #endif
 103
 104     #ifdef STM32F10X_HD
 105         #define FLASH_PAGE_COUNT 128
 106     #endif
 107 #endif
 108
 109 #if defined(FLASH_SIZE)
 110 #define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
 111 #endif
 112
 113 #if !defined(FLASH_PAGE_SIZE)
 114 #error "Flash page size not defined for target."
 115 #endif
 116
 117 #if !defined(FLASH_PAGE_COUNT)
 118 #error "Flash page count not defined for target."
 119 #endif
 120
 121 // use the last flash pages for storage
 122 #define CONFIG_START_FLASH_ADDRESS (0x08000000 + (uint32_t)((FLASH_PAGE_SIZE * FLASH_PAGE_COUNT) - FLASH_TO_RESERVE_FOR_CONFIG))
 123
 124 master_t masterConfig;                 // master config struct with data independent from profiles
 125 profile_t *currentProfile;
 126 static uint32_t activeFeaturesLatch = 0;
 127
 128 static uint8_t currentControlRateProfileIndex = 0;
 129 controlRateConfig_t *currentControlRateProfile;
 130
 131 static const uint8_t EEPROM_CONF_VERSION = 103;
 132
 133 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 134 {
 135     accelerometerTrims->values.pitch = 0;
 136     accelerometerTrims->values.roll = 0;
 137     accelerometerTrims->values.yaw = 0;
 138 }
 139
 140 static void resetPidProfile(pidProfile_t *pidProfile)
 141 {
 142     pidProfile->pidController = 0;
 143
 144     pidProfile->P8[ROLL] = 40;
 145     pidProfile->I8[ROLL] = 30;
 146     pidProfile->D8[ROLL] = 23;
 147     pidProfile->P8[PITCH] = 40;
 148     pidProfile->I8[PITCH] = 30;
 149     pidProfile->D8[PITCH] = 23;
 150     pidProfile->P8[YAW] = 85;
 151     pidProfile->I8[YAW] = 45;
 152     pidProfile->D8[YAW] = 0;
 153     pidProfile->P8[PIDALT] = 50;
 154     pidProfile->I8[PIDALT] = 0;
 155     pidProfile->D8[PIDALT] = 0;
 156     pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100;
 157     pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
 158     pidProfile->D8[PIDPOS] = 0;
 159     pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10;
 160     pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100;
 161     pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000;
 162     pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
 163     pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
 164     pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
 165     pidProfile->P8[PIDLEVEL] = 90;
 166     pidProfile->I8[PIDLEVEL] = 10;
 167     pidProfile->D8[PIDLEVEL] = 100;
 168     pidProfile->P8[PIDMAG] = 40;
 169     pidProfile->P8[PIDVEL] = 120;
 170     pidProfile->I8[PIDVEL] = 45;
 171     pidProfile->D8[PIDVEL] = 1;
 172
 173     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
 174     pidProfile->dterm_cut_hz = 0;
 175     pidProfile->pterm_cut_hz = 0;
 176     pidProfile->gyro_cut_hz = 0;
 177
 178     pidProfile->P_f[ROLL] = 2.5f;     // new PID with preliminary defaults test carefully
 179     pidProfile->I_f[ROLL] = 0.6f;
 180     pidProfile->D_f[ROLL] = 0.06f;
 181     pidProfile->P_f[PITCH] = 2.5f;
 182     pidProfile->I_f[PITCH] = 0.6f;
 183     pidProfile->D_f[PITCH] = 0.06f;
 184     pidProfile->P_f[YAW] = 8.0f;
 185     pidProfile->I_f[YAW] = 0.5f;
 186     pidProfile->D_f[YAW] = 0.05f;
 187     pidProfile->A_level = 5.0f;
 188     pidProfile->H_level = 3.0f;
 189     pidProfile->H_sensitivity = 75;
 190 }
 191
 192 #ifdef GPS
 193 void resetGpsProfile(gpsProfile_t *gpsProfile)
 194 {
 195     gpsProfile->gps_wp_radius = 200;
 196     gpsProfile->gps_lpf = 20;
 197     gpsProfile->nav_slew_rate = 30;
 198     gpsProfile->nav_controls_heading = 1;
 199     gpsProfile->nav_speed_min = 100;
 200     gpsProfile->nav_speed_max = 300;
 201     gpsProfile->ap_mode = 40;
 202 }
 203 #endif
 204
 205 void resetBarometerConfig(barometerConfig_t *barometerConfig)
 206 {
 207     barometerConfig->baro_sample_count = 21;
 208     barometerConfig->baro_noise_lpf = 0.6f;
 209     barometerConfig->baro_cf_vel = 0.985f;
 210     barometerConfig->baro_cf_alt = 0.965f;
 211 }
 212
 213 void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
 214 {
 215     sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT;
 216     sensorAlignmentConfig->acc_align = ALIGN_DEFAULT;
 217     sensorAlignmentConfig->mag_align = ALIGN_DEFAULT;
 218 }
 219
 220 void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
 221 {
 222     escAndServoConfig->minthrottle = 1150;
 223     escAndServoConfig->maxthrottle = 1850;
 224     escAndServoConfig->mincommand = 1000;
 225     escAndServoConfig->servoCenterPulse = 1500;
 226 }
 227
 228 void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
 229 {
 230     flight3DConfig->deadband3d_low = 1406;
 231     flight3DConfig->deadband3d_high = 1514;
 232     flight3DConfig->neutral3d = 1460;
 233     flight3DConfig->deadband3d_throttle = 50;
 234 }
 235
 236 void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
 237 {
 238     telemetryConfig->telemetry_inversion = 0;
 239     telemetryConfig->telemetry_switch = 0;
 240     telemetryConfig->gpsNoFixLatitude = 0;
 241     telemetryConfig->gpsNoFixLongitude = 0;
 242     telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS;
 243     telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
 244     telemetryConfig->frsky_vfas_precision = 0;
 245     telemetryConfig->hottAlarmSoundInterval = 5;
 246 }
 247
 248 void resetBatteryConfig(batteryConfig_t *batteryConfig)
 249 {
 250     batteryConfig->vbatscale = VBAT_SCALE_DEFAULT;
 251     batteryConfig->vbatmaxcellvoltage = 43;
 252     batteryConfig->vbatmincellvoltage = 33;
 253     batteryConfig->vbatwarningcellvoltage = 35;
 254     batteryConfig->currentMeterOffset = 0;
 255     batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
 256     batteryConfig->batteryCapacity = 0;
 257     batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
 258 }
 259
 260 #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
 261 #define FIRST_PORT_INDEX 1
 262 #define SECOND_PORT_INDEX 0
 263 #else
 264 #define FIRST_PORT_INDEX 0
 265 #define SECOND_PORT_INDEX 1
 266 #endif
 267
 268 void resetSerialConfig(serialConfig_t *serialConfig)
 269 {
 270     uint8_t index;
 271     memset(serialConfig, 0, sizeof(serialConfig_t));
 272
 273     for (index = 0; index < SERIAL_PORT_COUNT; index++) {
 274         serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index];
 275         serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200;
 276         serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600;
 277         serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO;
 278         serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200;
 279     }
 280
 281     serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
 282
 283 #ifdef CC3D
 284     // This allows MSP connection via USART & VCP so the board can be reconfigured.
 285     serialConfig->portConfigs[1].functionMask = FUNCTION_MSP;
 286 #endif
 287
 288     serialConfig->reboot_character = 'R';
 289 }
 290
 291 static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
 292     controlRateConfig->rcRate8 = 90;
 293     controlRateConfig->rcExpo8 = 65;
 294     controlRateConfig->thrMid8 = 50;
 295     controlRateConfig->thrExpo8 = 0;
 296     controlRateConfig->dynThrPID = 0;
 297     controlRateConfig->rcYawExpo8 = 0;
 298     controlRateConfig->tpa_breakpoint = 1500;
 299
 300     for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
 301         controlRateConfig->rates[axis] = 0;
 302     }
 303
 304 }
 305
 306 void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
 307     rcControlsConfig->deadband = 0;
 308     rcControlsConfig->yaw_deadband = 0;
 309     rcControlsConfig->alt_hold_deadband = 40;
 310     rcControlsConfig->alt_hold_fast_change = 1;
 311 }
 312
 313 void resetMixerConfig(mixerConfig_t *mixerConfig) {
 314     mixerConfig->pid_at_min_throttle = 1;
 315     mixerConfig->yaw_motor_direction = 1;
 316     mixerConfig->yaw_jump_prevention_limit = 200;
 317 #ifdef USE_SERVOS
 318     mixerConfig->tri_unarmed_servo = 1;
 319     mixerConfig->servo_lowpass_freq = 400;
 320     mixerConfig->servo_lowpass_enable = 0;
 321 #endif
 322 }
 323
 324 uint8_t getCurrentProfile(void)
 325 {
 326     return masterConfig.current_profile_index;
 327 }
 328
 329 static void setProfile(uint8_t profileIndex)
 330 {
 331     currentProfile = &masterConfig.profile[profileIndex];
 332 }
 333
 334 uint8_t getCurrentControlRateProfile(void)
 335 {
 336     return currentControlRateProfileIndex;
 337 }
 338
 339 controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) {
 340     return &masterConfig.controlRateProfiles[profileIndex];
 341 }
 342
 343 static void setControlRateProfile(uint8_t profileIndex)
 344 {
 345     currentControlRateProfileIndex = profileIndex;
 346     currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex];
 347 }
 348
 349 // Default settings
 350 static void resetConf(void)
 351 {
 352     int i;
 353
 354     // Clear all configuration
 355     memset(&masterConfig, 0, sizeof(master_t));
 356     setProfile(0);
 357     setControlRateProfile(0);
 358
 359     masterConfig.version = EEPROM_CONF_VERSION;
 360     masterConfig.mixerMode = MIXER_QUADX;
 361     featureClearAll();
 362 #if defined(CJMCU) || defined(SPARKY)
 363     featureSet(FEATURE_RX_PPM);
 364 #endif
 365
 366 #ifdef BOARD_HAS_VOLTAGE_DIVIDER
 367     // only enable the VBAT feature by default if the board has a voltage divider otherwise
 368     // the user may see incorrect readings and unexpected issues with pin mappings may occur.
 369     featureSet(FEATURE_VBAT);
 370 #endif
 371
 372     featureSet(FEATURE_FAILSAFE);
 373
 374     // global settings
 375     masterConfig.current_profile_index = 0;     // default profile
 376     masterConfig.gyro_cmpf_factor = 600;        // default MWC
 377     masterConfig.gyro_cmpfm_factor = 250;       // default MWC
 378     masterConfig.gyro_lpf = 42;                 // supported by all gyro drivers now. In case of ST gyro, will default to 32Hz instead
 379
 380     resetAccelerometerTrims(&masterConfig.accZero);
 381
 382     resetSensorAlignment(&masterConfig.sensorAlignmentConfig);
 383
 384     masterConfig.boardAlignment.rollDegrees = 0;
 385     masterConfig.boardAlignment.pitchDegrees = 0;
 386     masterConfig.boardAlignment.yawDegrees = 0;
 387     masterConfig.acc_hardware = ACC_DEFAULT;     // default/autodetect
 388     masterConfig.max_angle_inclination = 500;    // 50 degrees
 389     masterConfig.yaw_control_direction = 1;
 390     masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
 391
 392     masterConfig.mag_hardware = MAG_DEFAULT;     // default/autodetect
 393
 394     resetBatteryConfig(&masterConfig.batteryConfig);
 395
 396     resetTelemetryConfig(&masterConfig.telemetryConfig);
 397
 398     masterConfig.rxConfig.serialrx_provider = 0;
 399     masterConfig.rxConfig.spektrum_sat_bind = 0;
 400     masterConfig.rxConfig.midrc = 1500;
 401     masterConfig.rxConfig.mincheck = 1100;
 402     masterConfig.rxConfig.maxcheck = 1900;
 403     masterConfig.rxConfig.rx_min_usec = 885;          // any of first 4 channels below this value will trigger rx loss detection
 404     masterConfig.rxConfig.rx_max_usec = 2115;         // any of first 4 channels above this value will trigger rx loss detection
 405
 406     for (i = 0; i < MAX_AUX_CHANNEL_COUNT; i++) {
 407         masterConfig.rxConfig.rx_fail_usec_steps[i] = CHANNEL_VALUE_TO_RXFAIL_STEP(masterConfig.rxConfig.midrc);
 408     }
 409
 410     masterConfig.rxConfig.rssi_channel = 0;
 411     masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
 412     masterConfig.rxConfig.rssi_ppm_invert = 0;
 413
 414     masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;
 415
 416     masterConfig.retarded_arm = 0;
 417     masterConfig.disarm_kill_switch = 1;
 418     masterConfig.auto_disarm_delay = 5;
 419     masterConfig.small_angle = 25;
 420
 421     resetMixerConfig(&masterConfig.mixerConfig);
 422
 423     masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
 424
 425     // Motor/ESC/Servo
 426     resetEscAndServoConfig(&masterConfig.escAndServoConfig);
 427     resetFlight3DConfig(&masterConfig.flight3DConfig);
 428
 429 #ifdef BRUSHED_MOTORS
 430     masterConfig.motor_pwm_rate = BRUSHED_MOTORS_PWM_RATE;
 431 #else
 432     masterConfig.motor_pwm_rate = BRUSHLESS_MOTORS_PWM_RATE;
 433 #endif
 434     masterConfig.servo_pwm_rate = 50;
 435
 436 #ifdef GPS
 437     // gps/nav stuff
 438     masterConfig.gpsConfig.provider = GPS_NMEA;
 439     masterConfig.gpsConfig.sbasMode = SBAS_AUTO;
 440     masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON;
 441     masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF;
 442 #endif
 443
 444     resetSerialConfig(&masterConfig.serialConfig);
 445
 446     masterConfig.looptime = 3500;
 447     masterConfig.emf_avoidance = 0;
 448
 449     resetPidProfile(&currentProfile->pidProfile);
 450
 451     resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
 452
 453     // for (i = 0; i < CHECKBOXITEMS; i++)
 454     //     cfg.activate[i] = 0;
 455
 456     resetRollAndPitchTrims(&currentProfile->accelerometerTrims);
 457
 458     currentProfile->mag_declination = 0;
 459     currentProfile->acc_lpf_factor = 4;
 460     currentProfile->accz_lpf_cutoff = 5.0f;
 461     currentProfile->accDeadband.xy = 40;
 462     currentProfile->accDeadband.z = 40;
 463
 464     resetBarometerConfig(&currentProfile->barometerConfig);
 465
 466     currentProfile->acc_unarmedcal = 1;
 467
 468     // Radio
 469     parseRcChannels("AETR1234", &masterConfig.rxConfig);
 470
 471     resetRcControlsConfig(&currentProfile->rcControlsConfig);
 472
 473     currentProfile->throttle_correction_value = 0;      // could 10 with althold or 40 for fpv
 474     currentProfile->throttle_correction_angle = 800;    // could be 80.0 deg with atlhold or 45.0 for fpv
 475
 476     // Failsafe Variables
 477     masterConfig.failsafeConfig.failsafe_delay = 10;              // 1sec
 478     masterConfig.failsafeConfig.failsafe_off_delay = 200;         // 20sec
 479     masterConfig.failsafeConfig.failsafe_throttle = 1000;         // default throttle off.
 480
 481 #ifdef USE_SERVOS
 482     // servos
 483     for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
 484         currentProfile->servoConf[i].min = DEFAULT_SERVO_MIN;
 485         currentProfile->servoConf[i].max = DEFAULT_SERVO_MAX;
 486         currentProfile->servoConf[i].middle = DEFAULT_SERVO_MIDDLE;
 487         currentProfile->servoConf[i].rate = 100;
 488         currentProfile->servoConf[i].angleAtMin = DEFAULT_SERVO_MIN_ANGLE;
 489         currentProfile->servoConf[i].angleAtMax = DEFAULT_SERVO_MAX_ANGLE;
 490         currentProfile->servoConf[i].forwardFromChannel = CHANNEL_FORWARDING_DISABLED;
 491     }
 492
 493     // gimbal
 494     currentProfile->gimbalConfig.mode = GIMBAL_MODE_NORMAL;
 495 #endif
 496
 497 #ifdef GPS
 498     resetGpsProfile(&currentProfile->gpsProfile);
 499 #endif
 500
 501     // custom mixer. clear by defaults.
 502     for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
 503         masterConfig.customMotorMixer[i].throttle = 0.0f;
 504
 505 #ifdef LED_STRIP
 506     applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
 507     applyDefaultLedStripConfig(masterConfig.ledConfigs);
 508 #endif
 509
 510 #ifdef BLACKBOX
 511 #ifdef SPRACINGF3
 512     featureSet(FEATURE_BLACKBOX);
 513     masterConfig.blackbox_device = 1;
 514 #else
 515     masterConfig.blackbox_device = 0;
 516 #endif
 517     masterConfig.blackbox_rate_num = 1;
 518     masterConfig.blackbox_rate_denom = 1;
 519 #endif
 520
 521     // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets
 522 #ifdef ALIENWII32
 523     featureSet(FEATURE_RX_SERIAL);
 524     featureSet(FEATURE_MOTOR_STOP);
 525 #ifdef ALIENWIIF3
 526     masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
 527     masterConfig.batteryConfig.vbatscale = 20;
 528 #else
 529     masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
 530 #endif
 531     masterConfig.rxConfig.serialrx_provider = 1;
 532     masterConfig.rxConfig.spektrum_sat_bind = 5;
 533     masterConfig.escAndServoConfig.minthrottle = 1000;
 534     masterConfig.escAndServoConfig.maxthrottle = 2000;
 535     masterConfig.motor_pwm_rate = 32000;
 536     masterConfig.looptime = 2000;
 537     currentProfile->pidProfile.pidController = 3;
 538     currentProfile->pidProfile.P8[ROLL] = 36;
 539     currentProfile->pidProfile.P8[PITCH] = 36;
 540     masterConfig.failsafeConfig.failsafe_delay = 2;
 541     masterConfig.failsafeConfig.failsafe_off_delay = 0;
 542     currentControlRateProfile->rcRate8 = 130;
 543     currentControlRateProfile->rates[FD_PITCH] = 20;
 544     currentControlRateProfile->rates[FD_ROLL] = 20;
 545     currentControlRateProfile->rates[FD_YAW] = 100;
 546     parseRcChannels("TAER1234", &masterConfig.rxConfig);
 547
 548     //  { 1.0f, -0.414178f,  1.0f, -1.0f },          // REAR_R
 549     masterConfig.customMotorMixer[0].throttle = 1.0f;
 550     masterConfig.customMotorMixer[0].roll = -0.414178f;
 551     masterConfig.customMotorMixer[0].pitch = 1.0f;
 552     masterConfig.customMotorMixer[0].yaw = -1.0f;
 553
 554     //  { 1.0f, -0.414178f, -1.0f,  1.0f },          // FRONT_R
 555     masterConfig.customMotorMixer[1].throttle = 1.0f;
 556     masterConfig.customMotorMixer[1].roll = -0.414178f;
 557     masterConfig.customMotorMixer[1].pitch = -1.0f;
 558     masterConfig.customMotorMixer[1].yaw = 1.0f;
 559
 560     //  { 1.0f,  0.414178f,  1.0f,  1.0f },          // REAR_L
 561     masterConfig.customMotorMixer[2].throttle = 1.0f;
 562     masterConfig.customMotorMixer[2].roll = 0.414178f;
 563     masterConfig.customMotorMixer[2].pitch = 1.0f;
 564     masterConfig.customMotorMixer[2].yaw = 1.0f;
 565
 566     //  { 1.0f,  0.414178f, -1.0f, -1.0f },          // FRONT_L
 567     masterConfig.customMotorMixer[3].throttle = 1.0f;
 568     masterConfig.customMotorMixer[3].roll = 0.414178f;
 569     masterConfig.customMotorMixer[3].pitch = -1.0f;
 570     masterConfig.customMotorMixer[3].yaw = -1.0f;
 571
 572     //  { 1.0f, -1.0f, -0.414178f, -1.0f },          // MIDFRONT_R
 573     masterConfig.customMotorMixer[4].throttle = 1.0f;
 574     masterConfig.customMotorMixer[4].roll = -1.0f;
 575     masterConfig.customMotorMixer[4].pitch = -0.414178f;
 576     masterConfig.customMotorMixer[4].yaw = -1.0f;
 577
 578     //  { 1.0f,  1.0f, -0.414178f,  1.0f },          // MIDFRONT_L
 579     masterConfig.customMotorMixer[5].throttle = 1.0f;
 580     masterConfig.customMotorMixer[5].roll = 1.0f;
 581     masterConfig.customMotorMixer[5].pitch = -0.414178f;
 582     masterConfig.customMotorMixer[5].yaw = 1.0f;
 583
 584     //  { 1.0f, -1.0f,  0.414178f,  1.0f },          // MIDREAR_R
 585     masterConfig.customMotorMixer[6].throttle = 1.0f;
 586     masterConfig.customMotorMixer[6].roll = -1.0f;
 587     masterConfig.customMotorMixer[6].pitch = 0.414178f;
 588     masterConfig.customMotorMixer[6].yaw = 1.0f;
 589
 590     //  { 1.0f,  1.0f,  0.414178f, -1.0f },          // MIDREAR_L
 591     masterConfig.customMotorMixer[7].throttle = 1.0f;
 592     masterConfig.customMotorMixer[7].roll = 1.0f;
 593     masterConfig.customMotorMixer[7].pitch = 0.414178f;
 594     masterConfig.customMotorMixer[7].yaw = -1.0f;
 595 #endif
 596
 597     // copy first profile into remaining profile
 598     for (i = 1; i < MAX_PROFILE_COUNT; i++) {
 599         memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
 600     }
 601
 602     // copy first control rate config into remaining profile
 603     for (i = 1; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) {
 604         memcpy(&masterConfig.controlRateProfiles[i], currentControlRateProfile, sizeof(controlRateConfig_t));
 605     }
 606
 607     for (i = 1; i < MAX_PROFILE_COUNT; i++) {
 608         masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT;
 609     }
 610 }
 611
 612 static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
 613 {
 614     uint8_t checksum = 0;
 615     const uint8_t *byteOffset;
 616
 617     for (byteOffset = data; byteOffset < (data + length); byteOffset++)
 618         checksum ^= *byteOffset;
 619     return checksum;
 620 }
 621
 622 static bool isEEPROMContentValid(void)
 623 {
 624     const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
 625     uint8_t checksum = 0;
 626
 627     // check version number
 628     if (EEPROM_CONF_VERSION != temp->version)
 629         return false;
 630
 631     // check size and magic numbers
 632     if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
 633         return false;
 634
 635     // verify integrity of temporary copy
 636     checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
 637     if (checksum != 0)
 638         return false;
 639
 640     // looks good, let's roll!
 641     return true;
 642 }
 643
 644 void activateControlRateConfig(void)
 645 {
 646     generatePitchRollCurve(currentControlRateProfile);
 647     generateYawCurve(currentControlRateProfile);
 648     generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
 649 }
 650
 651 void activateConfig(void)
 652 {
 653     static imuRuntimeConfig_t imuRuntimeConfig;
 654
 655     activateControlRateConfig();
 656
 657     resetAdjustmentStates();
 658
 659     useRcControlsConfig(
 660         currentProfile->modeActivationConditions,
 661         &masterConfig.escAndServoConfig,
 662         &currentProfile->pidProfile
 663     );
 664
 665     useGyroConfig(&masterConfig.gyroConfig);
 666
 667 #ifdef TELEMETRY
 668     telemetryUseConfig(&masterConfig.telemetryConfig);
 669 #endif
 670
 671     pidSetController(currentProfile->pidProfile.pidController);
 672
 673 #ifdef GPS
 674     gpsUseProfile(&currentProfile->gpsProfile);
 675     gpsUsePIDs(&currentProfile->pidProfile);
 676 #endif
 677
 678     useFailsafeConfig(&masterConfig.failsafeConfig);
 679     setAccelerationTrims(&masterConfig.accZero);
 680
 681     mixerUseConfigs(
 682 #ifdef USE_SERVOS
 683         currentProfile->servoConf,
 684         &currentProfile->gimbalConfig,
 685 #endif
 686         &masterConfig.flight3DConfig,
 687         &masterConfig.escAndServoConfig,
 688         &masterConfig.mixerConfig,
 689         &masterConfig.airplaneConfig,
 690         &masterConfig.rxConfig
 691     );
 692
 693     imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
 694     imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
 695     imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor;
 696     imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
 697     imuRuntimeConfig.small_angle = masterConfig.small_angle;
 698
 699     imuConfigure(
 700         &imuRuntimeConfig,
 701         &currentProfile->pidProfile,
 702         &currentProfile->accDeadband,
 703         currentProfile->accz_lpf_cutoff,
 704         currentProfile->throttle_correction_angle
 705     );
 706
 707     configureAltitudeHold(
 708         &currentProfile->pidProfile,
 709         &currentProfile->barometerConfig,
 710         &currentProfile->rcControlsConfig,
 711         &masterConfig.escAndServoConfig
 712     );
 713
 714 #ifdef BARO
 715     useBarometerConfig(&currentProfile->barometerConfig);
 716 #endif
 717 }
 718
 719 void validateAndFixConfig(void)
 720 {
 721     if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
 722         featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
 723     }
 724
 725     if (featureConfigured(FEATURE_RX_PPM)) {
 726         featureClear(FEATURE_RX_PARALLEL_PWM);
 727     }
 728
 729     if (featureConfigured(FEATURE_RX_MSP)) {
 730         featureClear(FEATURE_RX_SERIAL);
 731         featureClear(FEATURE_RX_PARALLEL_PWM);
 732         featureClear(FEATURE_RX_PPM);
 733     }
 734
 735     if (featureConfigured(FEATURE_RX_SERIAL)) {
 736         featureClear(FEATURE_RX_PARALLEL_PWM);
 737         featureClear(FEATURE_RX_PPM);
 738     }
 739
 740     if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
 741 #if defined(STM32F10X)
 742         // rssi adc needs the same ports
 743         featureClear(FEATURE_RSSI_ADC);
 744         // current meter needs the same ports
 745         if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
 746             featureClear(FEATURE_CURRENT_METER);
 747         }
 748 #endif
 749
 750 #if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
 751         // led strip needs the same ports
 752         featureClear(FEATURE_LED_STRIP);
 753 #endif
 754
 755         // software serial needs free PWM ports
 756         featureClear(FEATURE_SOFTSERIAL);
 757     }
 758
 759
 760 #if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
 761     if (featureConfigured(FEATURE_SOFTSERIAL) && (
 762             0
 763 #ifdef USE_SOFTSERIAL1
 764             || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
 765 #endif
 766 #ifdef USE_SOFTSERIAL2
 767             || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
 768 #endif
 769     )) {
 770         // led strip needs the same timer as softserial
 771         featureClear(FEATURE_LED_STRIP);
 772     }
 773 #endif
 774
 775 #if defined(NAZE) && defined(SONAR)
 776     if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
 777         featureClear(FEATURE_CURRENT_METER);
 778     }
 779 #endif
 780
 781 #if defined(OLIMEXINO) && defined(SONAR)
 782     if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
 783         featureClear(FEATURE_CURRENT_METER);
 784     }
 785 #endif
 786
 787 #if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
 788     if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
 789         featureClear(FEATURE_DISPLAY);
 790     }
 791 #endif
 792
 793     /*
 794      * The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
 795      * The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
 796      */
 797     if (masterConfig.retarded_arm && masterConfig.mixerConfig.pid_at_min_throttle) {
 798         masterConfig.mixerConfig.pid_at_min_throttle = 0;
 799     }
 800
 801 #if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1)
 802     if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
 803         featureClear(FEATURE_SONAR);
 804     }
 805 #endif
 806
 807     useRxConfig(&masterConfig.rxConfig);
 808
 809     serialConfig_t *serialConfig = &masterConfig.serialConfig;
 810
 811     if (!isSerialConfigValid(serialConfig)) {
 812         resetSerialConfig(serialConfig);
 813     }
 814 }
 815
 816 void initEEPROM(void)
 817 {
 818 }
 819
 820 void readEEPROM(void)
 821 {
 822     // Sanity check
 823     if (!isEEPROMContentValid())
 824         failureMode(10);
 825
 826     // Read flash
 827     memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
 828
 829     if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
 830         masterConfig.current_profile_index = 0;
 831
 832     setProfile(masterConfig.current_profile_index);
 833
 834     if (currentProfile->defaultRateProfileIndex > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check
 835         currentProfile->defaultRateProfileIndex = 0;
 836
 837     setControlRateProfile(currentProfile->defaultRateProfileIndex);
 838
 839     validateAndFixConfig();
 840     activateConfig();
 841 }
 842
 843 void readEEPROMAndNotify(void)
 844 {
 845     // re-read written data
 846     readEEPROM();
 847     beeperConfirmationBeeps(1);
 848 }
 849
 850 void writeEEPROM(void)
 851 {
 852     // Generate compile time error if the config does not fit in the reserved area of flash.
 853     BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);
 854
 855     FLASH_Status status = 0;
 856     uint32_t wordOffset;
 857     int8_t attemptsRemaining = 3;
 858
 859     // prepare checksum/version constants
 860     masterConfig.version = EEPROM_CONF_VERSION;
 861     masterConfig.size = sizeof(master_t);
 862     masterConfig.magic_be = 0xBE;
 863     masterConfig.magic_ef = 0xEF;
 864     masterConfig.chk = 0; // erase checksum before recalculating
 865     masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));
 866
 867     // write it
 868     FLASH_Unlock();
 869     while (attemptsRemaining--) {
 870 #ifdef STM32F303
 871         FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
 872 #endif
 873 #ifdef STM32F10X
 874         FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
 875 #endif
 876         for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
 877             if (wordOffset % FLASH_PAGE_SIZE == 0) {
 878                 status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
 879                 if (status != FLASH_COMPLETE) {
 880                     break;
 881                 }
 882             }
 883
 884             status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
 885                     *(uint32_t *) ((char *) &masterConfig + wordOffset));
 886             if (status != FLASH_COMPLETE) {
 887                 break;
 888             }
 889         }
 890         if (status == FLASH_COMPLETE) {
 891             break;
 892         }
 893     }
 894     FLASH_Lock();
 895
 896     // Flash write failed - just die now
 897     if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
 898         failureMode(10);
 899     }
 900 }
 901
 902 void ensureEEPROMContainsValidData(void)
 903 {
 904     if (isEEPROMContentValid()) {
 905         return;
 906     }
 907
 908     resetEEPROM();
 909 }
 910
 911 void resetEEPROM(void)
 912 {
 913     resetConf();
 914     writeEEPROM();
 915 }
 916
 917 void saveConfigAndNotify(void)
 918 {
 919     writeEEPROM();
 920     readEEPROMAndNotify();
 921 }
 922
 923 void changeProfile(uint8_t profileIndex)
 924 {
 925     masterConfig.current_profile_index = profileIndex;
 926     writeEEPROM();
 927     readEEPROM();
 928     beeperConfirmationBeeps(profileIndex + 1);
 929 }
 930
 931 void changeControlRateProfile(uint8_t profileIndex)
 932 {
 933     if (profileIndex > MAX_CONTROL_RATE_PROFILE_COUNT) {
 934         profileIndex = MAX_CONTROL_RATE_PROFILE_COUNT - 1;
 935     }
 936     setControlRateProfile(profileIndex);
 937     activateControlRateConfig();
 938 }
 939
 940 void handleOneshotFeatureChangeOnRestart(void)
 941 {
 942     // Shutdown PWM on all motors prior to soft restart
 943     StopPwmAllMotors();
 944     delay(50);
 945     // Apply additional delay when OneShot125 feature changed from on to off state
 946     if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) {
 947         delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS);
 948     }
 949 }
 950
 951 void latchActiveFeatures()
 952 {
 953     activeFeaturesLatch = masterConfig.enabledFeatures;
 954 }
 955
 956 bool featureConfigured(uint32_t mask)
 957 {
 958     return masterConfig.enabledFeatures & mask;
 959 }
 960
 961 bool feature(uint32_t mask)
 962 {
 963     return activeFeaturesLatch & mask;
 964 }
 965
 966 void featureSet(uint32_t mask)
 967 {
 968     masterConfig.enabledFeatures |= mask;
 969 }
 970
 971 void featureClear(uint32_t mask)
 972 {
 973     masterConfig.enabledFeatures &= ~(mask);
 974 }
 975
 976 void featureClearAll()
 977 {
 978     masterConfig.enabledFeatures = 0;
 979 }
 980
 981 uint32_t featureMask(void)
 982 {
 983     return masterConfig.enabledFeatures;
 984 }
 985