config.c 29.7 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "platform.h"

#include "build_config.h"

26
#include "common/color.h"
27
#include "common/axis.h"
Dominic Clifton's avatar
Dominic Clifton committed
28
29
#include "common/maths.h"

30
#include "drivers/sensor.h"
31
#include "drivers/accgyro.h"
32
#include "drivers/compass.h"
33
34
35
36
#include "drivers/system.h"
#include "drivers/gpio.h"
#include "drivers/timer.h"
#include "drivers/pwm_rx.h"
37
#include "drivers/serial.h"
38
39
40

#include "sensors/sensors.h"
#include "sensors/gyro.h"
41
#include "sensors/compass.h"
42
43
44
45
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
46

47
#include "io/beeper.h"
48
#include "io/serial.h"
49
50
51
52
#include "io/gimbal.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
53
#include "io/ledstrip.h"
54
#include "io/gps.h"
55
56
57
58
59
60

#include "rx/rx.h"

#include "telemetry/telemetry.h"

#include "flight/mixer.h"
61
62
#include "flight/pid.h"
#include "flight/imu.h"
63
#include "flight/failsafe.h"
Dominic Clifton's avatar
Dominic Clifton committed
64
#include "flight/altitudehold.h"
65
66
67
68
#include "flight/navigation.h"

#include "config/runtime_config.h"
#include "config/config.h"
69

70
71
72
73
74
75
#include "config/config_profile.h"
#include "config/config_master.h"

#define BRUSHED_MOTORS_PWM_RATE 16000
#define BRUSHLESS_MOTORS_PWM_RATE 400

76
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse);
77
78
79

#define FLASH_TO_RESERVE_FOR_CONFIG 0x800

80
81
#if !defined(FLASH_SIZE)
#error "Flash size not defined for target. (specify in KB)"
82
83
#endif

84
85
86
87
88
89
90
91
92
93
94
95
96

#ifndef FLASH_PAGE_SIZE
    #ifdef STM32F303xC
        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
    #endif

    #ifdef STM32F10X_MD
        #define FLASH_PAGE_SIZE                 ((uint16_t)0x400)
    #endif

    #ifdef STM32F10X_HD
        #define FLASH_PAGE_SIZE                 ((uint16_t)0x800)
    #endif
97
98
#endif

99
100
101
102
103
104
105
106
#if !defined(FLASH_SIZE) && !defined(FLASH_PAGE_COUNT)
    #ifdef STM32F10X_MD
        #define FLASH_PAGE_COUNT 128
    #endif

    #ifdef STM32F10X_HD
        #define FLASH_PAGE_COUNT 128
    #endif
107
108
#endif

109
110
#if defined(FLASH_SIZE)
#define FLASH_PAGE_COUNT ((FLASH_SIZE * 0x400) / FLASH_PAGE_SIZE)
111
#endif
112
113
114

#if !defined(FLASH_PAGE_SIZE)
#error "Flash page size not defined for target."
115
#endif
116

117
#if !defined(FLASH_PAGE_COUNT)
118
119
120
#error "Flash page count not defined for target."
#endif

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
125
master_t masterConfig;                 // master config struct with data independent from profiles
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 = 102;
132
133
134
135
136
137
138
139
140
141

static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{
    accelerometerTrims->values.pitch = 0;
    accelerometerTrims->values.roll = 0;
    accelerometerTrims->values.yaw = 0;
}

static void resetPidProfile(pidProfile_t *pidProfile)
{
142
143
    pidProfile->pidController = 0;

144
145
146
147
148
149
150
151
152
153
154
155
    pidProfile->P8[ROLL] = 40;
    pidProfile->I8[ROLL] = 30;
    pidProfile->D8[ROLL] = 23;
    pidProfile->P8[PITCH] = 40;
    pidProfile->I8[PITCH] = 30;
    pidProfile->D8[PITCH] = 23;
    pidProfile->P8[YAW] = 85;
    pidProfile->I8[YAW] = 45;
    pidProfile->D8[YAW] = 0;
    pidProfile->P8[PIDALT] = 50;
    pidProfile->I8[PIDALT] = 0;
    pidProfile->D8[PIDALT] = 0;
156
    pidProfile->P8[PIDPOS] = 15; // POSHOLD_P * 100;
157
158
    pidProfile->I8[PIDPOS] = 0; // POSHOLD_I * 100;
    pidProfile->D8[PIDPOS] = 0;
159
160
161
162
163
164
    pidProfile->P8[PIDPOSR] = 34; // POSHOLD_RATE_P * 10;
    pidProfile->I8[PIDPOSR] = 14; // POSHOLD_RATE_I * 100;
    pidProfile->D8[PIDPOSR] = 53; // POSHOLD_RATE_D * 1000;
    pidProfile->P8[PIDNAVR] = 25; // NAV_P * 10;
    pidProfile->I8[PIDNAVR] = 33; // NAV_I * 100;
    pidProfile->D8[PIDNAVR] = 83; // NAV_D * 1000;
165
166
167
168
169
170
171
172
    pidProfile->P8[PIDLEVEL] = 90;
    pidProfile->I8[PIDLEVEL] = 10;
    pidProfile->D8[PIDLEVEL] = 100;
    pidProfile->P8[PIDMAG] = 40;
    pidProfile->P8[PIDVEL] = 120;
    pidProfile->I8[PIDVEL] = 45;
    pidProfile->D8[PIDVEL] = 1;

173
    pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
174

175
176
177
178
179
180
181
182
183
184
185
    pidProfile->P_f[ROLL] = 2.5f;     // new PID with preliminary defaults test carefully
    pidProfile->I_f[ROLL] = 0.6f;
    pidProfile->D_f[ROLL] = 0.06f;
    pidProfile->P_f[PITCH] = 2.5f;
    pidProfile->I_f[PITCH] = 0.6f;
    pidProfile->D_f[PITCH] = 0.06f;
    pidProfile->P_f[YAW] = 8.0f;
    pidProfile->I_f[YAW] = 0.5f;
    pidProfile->D_f[YAW] = 0.05f;
    pidProfile->A_level = 5.0f;
    pidProfile->H_level = 3.0f;
Ben Hitchcock's avatar
Ben Hitchcock committed
186
    pidProfile->H_sensitivity = 75;
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
}

#ifdef GPS
void resetGpsProfile(gpsProfile_t *gpsProfile)
{
    gpsProfile->gps_wp_radius = 200;
    gpsProfile->gps_lpf = 20;
    gpsProfile->nav_slew_rate = 30;
    gpsProfile->nav_controls_heading = 1;
    gpsProfile->nav_speed_min = 100;
    gpsProfile->nav_speed_max = 300;
    gpsProfile->ap_mode = 40;
}
#endif

void resetBarometerConfig(barometerConfig_t *barometerConfig)
{
    barometerConfig->baro_sample_count = 21;
    barometerConfig->baro_noise_lpf = 0.6f;
    barometerConfig->baro_cf_vel = 0.985f;
    barometerConfig->baro_cf_alt = 0.965f;
}

void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)
{
    sensorAlignmentConfig->gyro_align = ALIGN_DEFAULT;
    sensorAlignmentConfig->acc_align = ALIGN_DEFAULT;
    sensorAlignmentConfig->mag_align = ALIGN_DEFAULT;
}

void resetEscAndServoConfig(escAndServoConfig_t *escAndServoConfig)
{
    escAndServoConfig->minthrottle = 1150;
    escAndServoConfig->maxthrottle = 1850;
    escAndServoConfig->mincommand = 1000;
222
    escAndServoConfig->servoCenterPulse = 1500;
223
224
225
226
227
228
229
230
231
232
233
234
}

void resetFlight3DConfig(flight3DConfig_t *flight3DConfig)
{
    flight3DConfig->deadband3d_low = 1406;
    flight3DConfig->deadband3d_high = 1514;
    flight3DConfig->neutral3d = 1460;
    flight3DConfig->deadband3d_throttle = 50;
}

void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
{
235
    telemetryConfig->telemetry_inversion = 0;
236
    telemetryConfig->telemetry_switch = 0;
237
238
    telemetryConfig->gpsNoFixLatitude = 0;
    telemetryConfig->gpsNoFixLongitude = 0;
Airmamaf's avatar
Airmamaf committed
239
240
    telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS;
    telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
241
    telemetryConfig->frsky_vfas_precision = 0;
Pierre-A's avatar
Pierre-A committed
242
    telemetryConfig->hottAlarmSoundInterval = 5;
243
244
245
246
247
248
249
}

void resetBatteryConfig(batteryConfig_t *batteryConfig)
{
    batteryConfig->vbatscale = VBAT_SCALE_DEFAULT;
    batteryConfig->vbatmaxcellvoltage = 43;
    batteryConfig->vbatmincellvoltage = 33;
250
    batteryConfig->vbatwarningcellvoltage = 35;
251
252
253
    batteryConfig->currentMeterOffset = 0;
    batteryConfig->currentMeterScale = 400; // for Allegro ACS758LCB-100U (40mV/A)
    batteryConfig->batteryCapacity = 0;
tracernz's avatar
tracernz committed
254
    batteryConfig->currentMeterType = CURRENT_SENSOR_ADC;
255
256
}

257
258
259
260
261
262
263
264
#ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
#define FIRST_PORT_INDEX 1
#define SECOND_PORT_INDEX 0
#else
#define FIRST_PORT_INDEX 0
#define SECOND_PORT_INDEX 1
#endif

265
266
void resetSerialConfig(serialConfig_t *serialConfig)
{
267
268
269
270
271
    uint8_t index;
    memset(serialConfig, 0, sizeof(serialConfig_t));

    for (index = 0; index < SERIAL_PORT_COUNT; index++) {
        serialConfig->portConfigs[index].identifier = serialPortIdentifiers[index];
272
273
274
275
        serialConfig->portConfigs[index].msp_baudrateIndex = BAUD_115200;
        serialConfig->portConfigs[index].gps_baudrateIndex = BAUD_57600;
        serialConfig->portConfigs[index].telemetry_baudrateIndex = BAUD_AUTO;
        serialConfig->portConfigs[index].blackbox_baudrateIndex = BAUD_115200;
276
277
278
    }

    serialConfig->portConfigs[0].functionMask = FUNCTION_MSP;
279
280

#ifdef CC3D
281
    // This allows MSP connection via USART & VCP so the board can be reconfigured.
282
    serialConfig->portConfigs[1].functionMask = FUNCTION_MSP;
283
#endif
284

285
286
287
    serialConfig->reboot_character = 'R';
}

288
289
290
291
292
293
static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) {
    controlRateConfig->rcRate8 = 90;
    controlRateConfig->rcExpo8 = 65;
    controlRateConfig->thrMid8 = 50;
    controlRateConfig->thrExpo8 = 0;
    controlRateConfig->dynThrPID = 0;
borisbstyle's avatar
borisbstyle committed
294
    controlRateConfig->rcYawExpo8 = 0;
295
    controlRateConfig->tpa_breakpoint = 1500;
296
297
298
299
300

    for (uint8_t axis = 0; axis < FLIGHT_DYNAMICS_INDEX_COUNT; axis++) {
        controlRateConfig->rates[axis] = 0;
    }

301
}
302

303
304
305
306
307
void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) {
    rcControlsConfig->deadband = 0;
    rcControlsConfig->yaw_deadband = 0;
    rcControlsConfig->alt_hold_deadband = 40;
    rcControlsConfig->alt_hold_fast_change = 1;
308
}
309

310
311
312
void resetMixerConfig(mixerConfig_t *mixerConfig) {
    mixerConfig->pid_at_min_throttle = 1;
    mixerConfig->yaw_direction = 1;
BorisB's avatar
BorisB committed
313
    mixerConfig->yaw_jump_prevention_limit = 200;
314
315
316
317
318
319
320
#ifdef USE_SERVOS
    mixerConfig->tri_unarmed_servo = 1;
    mixerConfig->servo_lowpass_freq = 400;
    mixerConfig->servo_lowpass_enable = 0;
#endif
}

321
322
323
324
325
uint8_t getCurrentProfile(void)
{
    return masterConfig.current_profile_index;
}

326
327
328
329
330
static void setProfile(uint8_t profileIndex)
{
    currentProfile = &masterConfig.profile[profileIndex];
}

331
332
333
334
335
uint8_t getCurrentControlRateProfile(void)
{
    return currentControlRateProfileIndex;
}

336
337
338
339
controlRateConfig_t *getControlRateConfig(uint8_t profileIndex) {
    return &masterConfig.controlRateProfiles[profileIndex];
}

340
341
342
343
344
345
static void setControlRateProfile(uint8_t profileIndex)
{
    currentControlRateProfileIndex = profileIndex;
    currentControlRateProfile = &masterConfig.controlRateProfiles[profileIndex];
}

346
347
348
349
// Default settings
static void resetConf(void)
{
    int i;
350
#ifdef USE_SERVOS
351
    int8_t servoRates[MAX_SUPPORTED_SERVOS] = { 30, 30, 100, 100, 100, 100, 100, 100 };
352
353
    ;
#endif
354
355
356

    // Clear all configuration
    memset(&masterConfig, 0, sizeof(master_t));
357
    setProfile(0);
358
    setControlRateProfile(0);
359
360

    masterConfig.version = EEPROM_CONF_VERSION;
361
    masterConfig.mixerMode = MIXER_QUADX;
362
    featureClearAll();
363
#if defined(CJMCU) || defined(SPARKY)
364
365
    featureSet(FEATURE_RX_PPM);
#endif
366
367
368
369

#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.
370
    featureSet(FEATURE_VBAT);
371
372
#endif

373
    featureSet(FEATURE_FAILSAFE);
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392

    // 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;

393
394
    masterConfig.mag_hardware = MAG_DEFAULT;     // default/autodetect

395
    resetBatteryConfig(&masterConfig.batteryConfig);
396
397
398
399

    resetTelemetryConfig(&masterConfig.telemetryConfig);

    masterConfig.rxConfig.serialrx_provider = 0;
400
    masterConfig.rxConfig.spektrum_sat_bind = 0;
401
402
403
    masterConfig.rxConfig.midrc = 1500;
    masterConfig.rxConfig.mincheck = 1100;
    masterConfig.rxConfig.maxcheck = 1900;
404
405
    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
406

407
    masterConfig.rxConfig.rssi_channel = 0;
Pierre-A's avatar
Pierre-A committed
408
    masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
DaTwo's avatar
DaTwo committed
409
    masterConfig.rxConfig.rssi_ppm_invert = 0;
410
411
412

    masterConfig.inputFilteringMode = INPUT_FILTERING_DISABLED;

413
414
    masterConfig.retarded_arm = 0;
    masterConfig.disarm_kill_switch = 1;
415
    masterConfig.auto_disarm_delay = 5;
416
417
    masterConfig.small_angle = 25;

418
    resetMixerConfig(&masterConfig.mixerConfig);
419

420
    masterConfig.airplaneConfig.flaps_speed = 0;
421
    masterConfig.airplaneConfig.fixedwing_althold_dir = 1;
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437

    // 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;
438
439
    masterConfig.gpsConfig.autoConfig = GPS_AUTOCONFIG_ON;
    masterConfig.gpsConfig.autoBaud = GPS_AUTOBAUD_OFF;
440
441
442
443
444
445
446
#endif

    resetSerialConfig(&masterConfig.serialConfig);

    masterConfig.looptime = 3500;
    masterConfig.emf_avoidance = 0;

447
    resetPidProfile(&currentProfile->pidProfile);
448

449
    resetControlRateConfig(&masterConfig.controlRateProfiles[0]);
450
451
452
453

    // for (i = 0; i < CHECKBOXITEMS; i++)
    //     cfg.activate[i] = 0;

454
    resetRollAndPitchTrims(&currentProfile->accelerometerTrims);
455

456
457
458
459
460
    currentProfile->mag_declination = 0;
    currentProfile->acc_lpf_factor = 4;
    currentProfile->accz_lpf_cutoff = 5.0f;
    currentProfile->accDeadband.xy = 40;
    currentProfile->accDeadband.z = 40;
461

462
    resetBarometerConfig(&currentProfile->barometerConfig);
463

464
    currentProfile->acc_unarmedcal = 1;
465
466
467

    // Radio
    parseRcChannels("AETR1234", &masterConfig.rxConfig);
468
469
470

    resetRcControlsConfig(&currentProfile->rcControlsConfig);

471
472
    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
473
474

    // Failsafe Variables
475
476
477
    masterConfig.failsafeConfig.failsafe_delay = 10;              // 1sec
    masterConfig.failsafeConfig.failsafe_off_delay = 200;         // 20sec
    masterConfig.failsafeConfig.failsafe_throttle = 1000;         // default throttle off.
478

479
#ifdef USE_SERVOS
480
    // servos
481
    for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
482
483
484
485
486
        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;
487
488
489
    }

    // gimbal
490
    currentProfile->gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
491
#endif
492
493

#ifdef GPS
494
    resetGpsProfile(&currentProfile->gpsProfile);
495
496
497
498
499
500
#endif

    // custom mixer. clear by defaults.
    for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
        masterConfig.customMixer[i].throttle = 0.0f;

501
#ifdef LED_STRIP
502
    applyDefaultColors(masterConfig.colors, CONFIGURABLE_COLOR_COUNT);
503
    applyDefaultLedStripConfig(masterConfig.ledConfigs);
504
#endif
505

Nicholas Sherlock's avatar
Nicholas Sherlock committed
506
#ifdef BLACKBOX
507
508
509
510
#ifdef SPRACINGF3
    featureSet(FEATURE_BLACKBOX);
    masterConfig.blackbox_device = 1;
#else
511
    masterConfig.blackbox_device = 0;
512
#endif
Nicholas Sherlock's avatar
Nicholas Sherlock committed
513
514
515
516
    masterConfig.blackbox_rate_num = 1;
    masterConfig.blackbox_rate_denom = 1;
#endif

517
    // alternative defaults settings for ALIENWIIF1 and ALIENWIIF3 targets
518
#ifdef ALIENWII32
Michael Jakob's avatar
Michael Jakob committed
519
    featureSet(FEATURE_RX_SERIAL);
520
    featureSet(FEATURE_MOTOR_STOP);
521
#ifdef ALIENWIIF3
522
    masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
523
    masterConfig.batteryConfig.vbatscale = 20;
524
#else
525
    masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
526
#endif
Michael Jakob's avatar
Michael Jakob committed
527
    masterConfig.rxConfig.serialrx_provider = 1;
528
    masterConfig.rxConfig.spektrum_sat_bind = 5;
529
530
    masterConfig.escAndServoConfig.minthrottle = 1000;
    masterConfig.escAndServoConfig.maxthrottle = 2000;
Michael Jakob's avatar
Michael Jakob committed
531
    masterConfig.motor_pwm_rate = 32000;
532
    masterConfig.looptime = 2000;
Dominic Clifton's avatar
Dominic Clifton committed
533
    currentProfile->pidProfile.pidController = 3;
534
535
    currentProfile->pidProfile.P8[ROLL] = 36;
    currentProfile->pidProfile.P8[PITCH] = 36;
536
537
538
    masterConfig.failsafeConfig.failsafe_delay = 2;
    masterConfig.failsafeConfig.failsafe_off_delay = 0;
    masterConfig.failsafeConfig.failsafe_throttle = 1000;
539
    currentControlRateProfile->rcRate8 = 130;
540
541
542
    currentControlRateProfile->rates[FD_PITCH] = 20;
    currentControlRateProfile->rates[FD_ROLL] = 20;
    currentControlRateProfile->rates[FD_YAW] = 100;
543
    parseRcChannels("TAER1234", &masterConfig.rxConfig);
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591

    //  { 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;
592
593
#endif

594
    // copy first profile into remaining profile
595
    for (i = 1; i < MAX_PROFILE_COUNT; i++) {
596
        memcpy(&masterConfig.profile[i], currentProfile, sizeof(profile_t));
597
598
599
600
601
602
603
604
    }

    // 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++) {
605
        masterConfig.profile[i].defaultRateProfileIndex = i % MAX_CONTROL_RATE_PROFILE_COUNT;
606
    }
607
608
609
610
611
612
613
614
615
616
617
618
619
620
}

static uint8_t calculateChecksum(const uint8_t *data, uint32_t length)
{
    uint8_t checksum = 0;
    const uint8_t *byteOffset;

    for (byteOffset = data; byteOffset < (data + length); byteOffset++)
        checksum ^= *byteOffset;
    return checksum;
}

static bool isEEPROMContentValid(void)
{
621
    const master_t *temp = (const master_t *) CONFIG_START_FLASH_ADDRESS;
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
    uint8_t checksum = 0;

    // check version number
    if (EEPROM_CONF_VERSION != temp->version)
        return false;

    // check size and magic numbers
    if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF)
        return false;

    // verify integrity of temporary copy
    checksum = calculateChecksum((const uint8_t *) temp, sizeof(master_t));
    if (checksum != 0)
        return false;

    // looks good, let's roll!
    return true;
}

641
642
643
void activateControlRateConfig(void)
{
    generatePitchRollCurve(currentControlRateProfile);
borisbstyle's avatar
borisbstyle committed
644
    generateYawCurve(currentControlRateProfile);
645
646
647
    generateThrottleCurve(currentControlRateProfile, &masterConfig.escAndServoConfig);
}

648
649
650
651
void activateConfig(void)
{
    static imuRuntimeConfig_t imuRuntimeConfig;

652
653
    activateControlRateConfig();

654
655
    resetAdjustmentStates();

656
657
658
659
660
    useRcControlsConfig(
        currentProfile->modeActivationConditions,
        &masterConfig.escAndServoConfig,
        &currentProfile->pidProfile
    );
661
662

    useGyroConfig(&masterConfig.gyroConfig);
663

664
665
666
#ifdef TELEMETRY
    useTelemetryConfig(&masterConfig.telemetryConfig);
#endif
667

668
    pidSetController(currentProfile->pidProfile.pidController);
669

670
#ifdef GPS
671
672
    gpsUseProfile(&currentProfile->gpsProfile);
    gpsUsePIDs(&currentProfile->pidProfile);
673
#endif
674

675
    useFailsafeConfig(&masterConfig.failsafeConfig);
676
    setAccelerationTrims(&masterConfig.accZero);
677

678
    mixerUseConfigs(
679
#ifdef USE_SERVOS
680
        currentProfile->servoConf,
681
682
        &currentProfile->gimbalConfig,
#endif
683
684
        &masterConfig.flight3DConfig,
        &masterConfig.escAndServoConfig,
685
        &masterConfig.mixerConfig,
686
        &masterConfig.airplaneConfig,
687
        &masterConfig.rxConfig
688
    );
689
690
691

    imuRuntimeConfig.gyro_cmpf_factor = masterConfig.gyro_cmpf_factor;
    imuRuntimeConfig.gyro_cmpfm_factor = masterConfig.gyro_cmpfm_factor;
692
693
    imuRuntimeConfig.acc_lpf_factor = currentProfile->acc_lpf_factor;
    imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
694
695
    imuRuntimeConfig.small_angle = masterConfig.small_angle;

Dominic Clifton's avatar
Dominic Clifton committed
696
    imuConfigure(
697
698
        &imuRuntimeConfig,
        &currentProfile->pidProfile,
699
700
701
        &currentProfile->accDeadband,
        currentProfile->accz_lpf_cutoff,
        currentProfile->throttle_correction_angle
702
703
704
705
706
707
708
709
    );

    configureAltitudeHold(
        &currentProfile->pidProfile,
        &currentProfile->barometerConfig,
        &currentProfile->rcControlsConfig,
        &masterConfig.escAndServoConfig
    );
710
711

#ifdef BARO
712
    useBarometerConfig(&currentProfile->barometerConfig);
713
714
715
716
717
#endif
}

void validateAndFixConfig(void)
{
718
    if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
719
720
721
        featureSet(FEATURE_RX_PARALLEL_PWM); // Consider changing the default to PPM
    }

722
    if (featureConfigured(FEATURE_RX_PPM)) {
723
724
725
        featureClear(FEATURE_RX_PARALLEL_PWM);
    }

726
    if (featureConfigured(FEATURE_RX_MSP)) {
727
728
729
730
731
        featureClear(FEATURE_RX_SERIAL);
        featureClear(FEATURE_RX_PARALLEL_PWM);
        featureClear(FEATURE_RX_PPM);
    }

732
    if (featureConfigured(FEATURE_RX_SERIAL)) {
733
734
735
736
        featureClear(FEATURE_RX_PARALLEL_PWM);
        featureClear(FEATURE_RX_PPM);
    }

737
    if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
738
#if defined(STM32F10X)
739
740
741
        // rssi adc needs the same ports
        featureClear(FEATURE_RSSI_ADC);
        // current meter needs the same ports
742
743
744
        if (masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
            featureClear(FEATURE_CURRENT_METER);
        }
745
746
#endif

747
#if defined(STM32F10X) || defined(CHEBUZZ) || defined(STM32F3DISCOVERY)
748
749
750
751
752
753
754
755
756
        // led strip needs the same ports
        featureClear(FEATURE_LED_STRIP);
#endif

        // software serial needs free PWM ports
        featureClear(FEATURE_SOFTSERIAL);
    }


757
#if defined(LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
758
    if (featureConfigured(FEATURE_SOFTSERIAL) && (
759
            0
Petr Ledvina's avatar
Petr Ledvina committed
760
761
#ifdef USE_SOFTSERIAL1
            || (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
762
763
#endif
#ifdef USE_SOFTSERIAL2
Petr Ledvina's avatar
Petr Ledvina committed
764
            || (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
765
766
767
#endif
    )) {
        // led strip needs the same timer as softserial
768
769
770
771
        featureClear(FEATURE_LED_STRIP);
    }
#endif

772
#if defined(NAZE) && defined(SONAR)
773
    if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
774
775
776
777
778
        featureClear(FEATURE_CURRENT_METER);
    }
#endif

#if defined(OLIMEXINO) && defined(SONAR)
779
    if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC) {
780
781
782
        featureClear(FEATURE_CURRENT_METER);
    }
#endif
783

784
785
786
787
788
789
#if defined(CC3D) && defined(DISPLAY) && defined(USE_USART3)
    if (doesConfigurationUsePort(SERIAL_PORT_USART3) && feature(FEATURE_DISPLAY)) {
        featureClear(FEATURE_DISPLAY);
    }
#endif

790
791
792
793
794
795
796
797
    /*
     * The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
     * The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
     */
    if (masterConfig.retarded_arm && masterConfig.mixerConfig.pid_at_min_throttle) {
        masterConfig.mixerConfig.pid_at_min_throttle = 0;
    }

Dominic Clifton's avatar
Dominic Clifton committed
798
799
800
801
802
803
#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1)
    if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
        featureClear(FEATURE_SONAR);
    }
#endif

804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
    useRxConfig(&masterConfig.rxConfig);

    serialConfig_t *serialConfig = &masterConfig.serialConfig;

    if (!isSerialConfigValid(serialConfig)) {
        resetSerialConfig(serialConfig);
    }
}

void initEEPROM(void)
{
}

void readEEPROM(void)
{
    // Sanity check
    if (!isEEPROMContentValid())
        failureMode(10);

    // Read flash
824
    memcpy(&masterConfig, (char *) CONFIG_START_FLASH_ADDRESS, sizeof(master_t));
825
826

    if (masterConfig.current_profile_index > MAX_PROFILE_COUNT - 1) // sanity check
827
        masterConfig.current_profile_index = 0;
828

829
    setProfile(masterConfig.current_profile_index);
830

831
832
    if (currentProfile->defaultRateProfileIndex > MAX_CONTROL_RATE_PROFILE_COUNT - 1) // sanity check
        currentProfile->defaultRateProfileIndex = 0;
833

834
    setControlRateProfile(currentProfile->defaultRateProfileIndex);
835

836
837
838
839
840
841
842
843
    validateAndFixConfig();
    activateConfig();
}

void readEEPROMAndNotify(void)
{
    // re-read written data
    readEEPROM();
844
    beeperConfirmationBeeps(1);
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
}

void writeEEPROM(void)
{
    // Generate compile time error if the config does not fit in the reserved area of flash.
    BUILD_BUG_ON(sizeof(master_t) > FLASH_TO_RESERVE_FOR_CONFIG);

    FLASH_Status status = 0;
    uint32_t wordOffset;
    int8_t attemptsRemaining = 3;

    // prepare checksum/version constants
    masterConfig.version = EEPROM_CONF_VERSION;
    masterConfig.size = sizeof(master_t);
    masterConfig.magic_be = 0xBE;
    masterConfig.magic_ef = 0xEF;
    masterConfig.chk = 0; // erase checksum before recalculating
    masterConfig.chk = calculateChecksum((const uint8_t *) &masterConfig, sizeof(master_t));

    // write it
    FLASH_Unlock();
    while (attemptsRemaining--) {
867
#ifdef STM32F303
868
869
        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPERR);
#endif
870
#ifdef STM32F10X
871
872
873
874
        FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
#endif
        for (wordOffset = 0; wordOffset < sizeof(master_t); wordOffset += 4) {
            if (wordOffset % FLASH_PAGE_SIZE == 0) {
875
                status = FLASH_ErasePage(CONFIG_START_FLASH_ADDRESS + wordOffset);
876
877
878
879
880
                if (status != FLASH_COMPLETE) {
                    break;
                }
            }

881
            status = FLASH_ProgramWord(CONFIG_START_FLASH_ADDRESS + wordOffset,
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
                    *(uint32_t *) ((char *) &masterConfig + wordOffset));
            if (status != FLASH_COMPLETE) {
                break;
            }
        }
        if (status == FLASH_COMPLETE) {
            break;
        }
    }
    FLASH_Lock();

    // Flash write failed - just die now
    if (status != FLASH_COMPLETE || !isEEPROMContentValid()) {
        failureMode(10);
    }
}

void ensureEEPROMContainsValidData(void)
{
    if (isEEPROMContentValid()) {
        return;
    }

    resetEEPROM();
}

void resetEEPROM(void)
{
    resetConf();
    writeEEPROM();
}

914
void saveConfigAndNotify(void)
915
916
917
918
919
920
921
922
923
924
{
    writeEEPROM();
    readEEPROMAndNotify();
}

void changeProfile(uint8_t profileIndex)
{
    masterConfig.current_profile_index = profileIndex;
    writeEEPROM();
    readEEPROM();
925
    beeperConfirmationBeeps(profileIndex + 1);
926
927
}

928
929
930
931
932
933
934
935
936
void changeControlRateProfile(uint8_t profileIndex)
{
    if (profileIndex > MAX_CONTROL_RATE_PROFILE_COUNT) {
        profileIndex = MAX_CONTROL_RATE_PROFILE_COUNT - 1;
    }
    setControlRateProfile(profileIndex);
    activateControlRateConfig();
}

937
938
void handleOneshotFeatureChangeOnRestart(void)
{
939
940
941
    // Shutdown PWM on all motors prior to soft restart
    StopPwmAllMotors();
    delay(50);
942
943
    // Apply additional delay when OneShot125 feature changed from on to off state
    if (feature(FEATURE_ONESHOT125) && !featureConfigured(FEATURE_ONESHOT125)) {
944
945
946
947
948
949
950
951
952
953
        delay(ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS);
    }
}

void latchActiveFeatures()
{
    activeFeaturesLatch = masterConfig.enabledFeatures;
}

bool featureConfigured(uint32_t mask)
954
955
956
957
{
    return masterConfig.enabledFeatures & mask;
}

958
959
960
961
962
bool feature(uint32_t mask)
{
    return activeFeaturesLatch & mask;
}

963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
void featureSet(uint32_t mask)
{
    masterConfig.enabledFeatures |= mask;
}

void featureClear(uint32_t mask)
{
    masterConfig.enabledFeatures &= ~(mask);
}

void featureClearAll()
{
    masterConfig.enabledFeatures = 0;
}

uint32_t featureMask(void)
{
    return masterConfig.enabledFeatures;
}