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