2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
28 #include "blackbox/blackbox.h"
30 #include "build/debug.h"
32 #include "config/config_eeprom.h"
33 #include "config/feature.h"
35 #include "drivers/system.h"
37 #include "fc/config.h"
38 #include "fc/controlrate_profile.h"
41 #include "fc/rc_adjustments.h"
42 #include "fc/rc_controls.h"
44 #include "flight/failsafe.h"
45 #include "flight/imu.h"
46 #include "flight/mixer.h"
47 #include "flight/pid.h"
48 #include "flight/servos.h"
50 #include "io/beeper.h"
51 #include "io/ledstrip.h"
52 #include "io/serial.h"
55 #include "pg/beeper.h"
56 #include "pg/beeper_dev.h"
59 #include "pg/pg_ids.h"
63 #include "sensors/acceleration.h"
64 #include "sensors/battery.h"
65 #include "sensors/gyro.h"
67 #include "scheduler/scheduler.h"
69 pidProfile_t
*currentPidProfile
;
71 #ifndef RX_SPI_DEFAULT_PROTOCOL
72 #define RX_SPI_DEFAULT_PROTOCOL 0
75 #define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(2000)
77 PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t
, pilotConfig
, PG_PILOT_CONFIG
, 1);
79 PG_RESET_TEMPLATE(pilotConfig_t
, pilotConfig
,
84 PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t
, systemConfig
, PG_SYSTEM_CONFIG
, 2);
86 PG_RESET_TEMPLATE(systemConfig_t
, systemConfig
,
88 .activeRateProfile
= 0,
89 .debug_mode
= DEBUG_MODE
,
90 .task_statistics
= true,
92 .powerOnArmingGraceTime
= 5,
93 .boardIdentifier
= TARGET_BOARD_IDENTIFIER
,
94 .hseMhz
= SYSTEM_HSE_VALUE
, // Not used for non-F4 targets
96 .schedulerOptimizeRate
= false,
99 uint8_t getCurrentPidProfileIndex(void)
101 return systemConfig()->pidProfileIndex
;
104 static void loadPidProfile(void)
106 currentPidProfile
= pidProfilesMutable(systemConfig()->pidProfileIndex
);
109 uint8_t getCurrentControlRateProfileIndex(void)
111 return systemConfig()->activeRateProfile
;
114 uint16_t getCurrentMinthrottle(void)
116 return motorConfig()->minthrottle
;
119 void resetConfigs(void)
123 #if defined(USE_TARGET_CONFIG)
124 targetConfiguration();
128 static void activateConfig(void)
130 schedulerOptimizeRate(systemConfig()->schedulerOptimizeRate
);
132 loadControlRateProfile();
136 resetAdjustmentStates();
138 pidInit(currentPidProfile
);
143 setAccelerationTrims(&accelerometerConfigMutable()->accZero
);
146 imuConfigure(throttleCorrectionConfig()->throttle_correction_angle
, throttleCorrectionConfig()->throttle_correction_value
);
149 reevaluateLedConfig();
153 static void validateAndFixConfig(void)
155 #if !defined(USE_QUAD_MIXER_ONLY)
156 // Reset unsupported mixer mode to default.
157 // This check will be gone when motor/servo mixers are loaded dynamically
158 // by configurator as a part of configuration procedure.
160 mixerMode_e mixerMode
= mixerConfigMutable()->mixerMode
;
162 if (!(mixerMode
== MIXER_CUSTOM
|| mixerMode
== MIXER_CUSTOM_AIRPLANE
|| mixerMode
== MIXER_CUSTOM_TRI
)) {
163 if (mixers
[mixerMode
].motorCount
&& mixers
[mixerMode
].motor
== NULL
)
164 mixerConfigMutable()->mixerMode
= MIXER_CUSTOM
;
166 if (mixers
[mixerMode
].useServo
&& servoMixers
[mixerMode
].servoRuleCount
== 0)
167 mixerConfigMutable()->mixerMode
= MIXER_CUSTOM_AIRPLANE
;
172 if (!isSerialConfigValid(serialConfig())) {
173 pgResetFn_serialConfig(serialConfigMutable());
177 serialPortConfig_t
*gpsSerial
= findSerialPortConfig(FUNCTION_GPS
);
178 if (gpsConfig()->provider
== GPS_MSP
&& gpsSerial
) {
179 serialRemovePort(gpsSerial
->identifier
);
184 gpsConfig()->provider
!= GPS_MSP
&& !gpsSerial
&&
187 featureDisable(FEATURE_GPS
);
190 if (systemConfig()->activeRateProfile
>= CONTROL_RATE_PROFILE_COUNT
) {
191 systemConfigMutable()->activeRateProfile
= 0;
193 loadControlRateProfile();
195 if (systemConfig()->pidProfileIndex
>= MAX_PROFILE_COUNT
) {
196 systemConfigMutable()->pidProfileIndex
= 0;
200 // Prevent invalid notch cutoff
201 if (currentPidProfile
->dterm_notch_cutoff
>= currentPidProfile
->dterm_notch_hz
) {
202 currentPidProfile
->dterm_notch_hz
= 0;
206 //PRevent invalid dynamic lowpass
207 if (currentPidProfile
->dyn_lpf_dterm_min_hz
> currentPidProfile
->dyn_lpf_dterm_max_hz
) {
208 currentPidProfile
->dyn_lpf_dterm_min_hz
= 0;
212 if (motorConfig()->dev
.motorPwmProtocol
== PWM_TYPE_BRUSHED
) {
213 featureDisable(FEATURE_3D
);
215 if (motorConfig()->mincommand
< 1000) {
216 motorConfigMutable()->mincommand
= 1000;
220 if ((motorConfig()->dev
.motorPwmProtocol
== PWM_TYPE_STANDARD
) && (motorConfig()->dev
.motorPwmRate
> BRUSHLESS_MOTORS_PWM_RATE
)) {
221 motorConfigMutable()->dev
.motorPwmRate
= BRUSHLESS_MOTORS_PWM_RATE
;
224 validateAndFixGyroConfig();
226 if (!(featureIsEnabled(FEATURE_RX_PARALLEL_PWM
) || featureIsEnabled(FEATURE_RX_PPM
) || featureIsEnabled(FEATURE_RX_SERIAL
) || featureIsEnabled(FEATURE_RX_MSP
) || featureIsEnabled(FEATURE_RX_SPI
))) {
227 featureEnable(DEFAULT_RX_FEATURE
);
230 if (featureIsEnabled(FEATURE_RX_PPM
)) {
231 featureDisable(FEATURE_RX_SERIAL
| FEATURE_RX_PARALLEL_PWM
| FEATURE_RX_MSP
| FEATURE_RX_SPI
);
234 if (featureIsEnabled(FEATURE_RX_MSP
)) {
235 featureDisable(FEATURE_RX_SERIAL
| FEATURE_RX_PARALLEL_PWM
| FEATURE_RX_PPM
| FEATURE_RX_SPI
);
238 if (featureIsEnabled(FEATURE_RX_SERIAL
)) {
239 featureDisable(FEATURE_RX_PARALLEL_PWM
| FEATURE_RX_MSP
| FEATURE_RX_PPM
| FEATURE_RX_SPI
);
243 if (featureIsEnabled(FEATURE_RX_SPI
)) {
244 featureDisable(FEATURE_RX_SERIAL
| FEATURE_RX_PARALLEL_PWM
| FEATURE_RX_PPM
| FEATURE_RX_MSP
);
248 if (featureIsEnabled(FEATURE_RX_PARALLEL_PWM
)) {
249 featureDisable(FEATURE_RX_SERIAL
| FEATURE_RX_MSP
| FEATURE_RX_PPM
| FEATURE_RX_SPI
);
253 if (featureIsEnabled(FEATURE_SOFTSPI
)) {
254 featureDisable(FEATURE_RX_PPM
| FEATURE_RX_PARALLEL_PWM
| FEATURE_SOFTSERIAL
);
255 batteryConfigMutable()->voltageMeterSource
= VOLTAGE_METER_NONE
;
256 #if defined(STM32F10X)
257 featureDisable(FEATURE_LED_STRIP
);
258 // rssi adc needs the same ports
259 featureDisable(FEATURE_RSSI_ADC
);
260 // current meter needs the same ports
261 if (batteryConfig()->currentMeterSource
== CURRENT_METER_ADC
) {
262 batteryConfigMutable()->currentMeterSource
= CURRENT_METER_NONE
;
266 #endif // USE_SOFTSPI
269 if (featureIsEnabled(FEATURE_RSSI_ADC
)) {
270 rxConfigMutable()->rssi_channel
= 0;
271 rxConfigMutable()->rssi_src_frame_errors
= false;
274 if (rxConfigMutable()->rssi_channel
275 #if defined(USE_PWM) || defined(USE_PPM)
276 || featureIsEnabled(FEATURE_RX_PPM
) || featureIsEnabled(FEATURE_RX_PARALLEL_PWM
)
279 rxConfigMutable()->rssi_src_frame_errors
= false;
282 if (!rcSmoothingIsEnabled() || rxConfig()->rcInterpolationChannels
== INTERPOLATION_CHANNELS_T
) {
283 for (unsigned i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
284 pidProfilesMutable(i
)->pid
[PID_ROLL
].F
= 0;
285 pidProfilesMutable(i
)->pid
[PID_PITCH
].F
= 0;
289 if (!rcSmoothingIsEnabled() ||
290 (rxConfig()->rcInterpolationChannels
!= INTERPOLATION_CHANNELS_RPY
&&
291 rxConfig()->rcInterpolationChannels
!= INTERPOLATION_CHANNELS_RPYT
)) {
293 for (unsigned i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
294 pidProfilesMutable(i
)->pid
[PID_YAW
].F
= 0;
298 #if defined(USE_THROTTLE_BOOST)
299 if (!rcSmoothingIsEnabled() ||
300 !(rxConfig()->rcInterpolationChannels
== INTERPOLATION_CHANNELS_RPYT
301 || rxConfig()->rcInterpolationChannels
== INTERPOLATION_CHANNELS_T
302 || rxConfig()->rcInterpolationChannels
== INTERPOLATION_CHANNELS_RPT
)) {
303 for (unsigned i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
304 pidProfilesMutable(i
)->throttle_boost
= 0;
310 featureIsEnabled(FEATURE_3D
) || !featureIsEnabled(FEATURE_GPS
)
311 #if !defined(USE_GPS) || !defined(USE_GPS_RESCUE)
315 if (failsafeConfig()->failsafe_procedure
== FAILSAFE_PROCEDURE_GPS_RESCUE
) {
316 failsafeConfigMutable()->failsafe_procedure
= FAILSAFE_PROCEDURE_DROP_IT
;
319 if (isModeActivationConditionPresent(BOXGPSRESCUE
)) {
320 removeModeActivationCondition(BOXGPSRESCUE
);
324 #if defined(USE_ESC_SENSOR)
325 if (!findSerialPortConfig(FUNCTION_ESC_SENSOR
)) {
326 featureDisable(FEATURE_ESC_SENSOR
);
330 // clear features that are not supported.
331 // I have kept them all here in one place, some could be moved to sections of code above.
334 featureDisable(FEATURE_RX_PPM
);
337 #ifndef USE_SERIAL_RX
338 featureDisable(FEATURE_RX_SERIAL
);
341 #if !defined(USE_SOFTSERIAL1) && !defined(USE_SOFTSERIAL2)
342 featureDisable(FEATURE_SOFTSERIAL
);
345 #ifndef USE_RANGEFINDER
346 featureDisable(FEATURE_RANGEFINDER
);
349 #ifndef USE_TELEMETRY
350 featureDisable(FEATURE_TELEMETRY
);
354 featureDisable(FEATURE_RX_PARALLEL_PWM
);
358 featureDisable(FEATURE_RX_MSP
);
361 #ifndef USE_LED_STRIP
362 featureDisable(FEATURE_LED_STRIP
);
365 #ifndef USE_DASHBOARD
366 featureDisable(FEATURE_DASHBOARD
);
370 featureDisable(FEATURE_OSD
);
374 featureDisable(FEATURE_SERVO_TILT
| FEATURE_CHANNEL_FORWARDING
);
377 #ifndef USE_TRANSPONDER
378 featureDisable(FEATURE_TRANSPONDER
);
382 featureDisable(FEATURE_RX_SPI
);
386 featureDisable(FEATURE_SOFTSPI
);
389 #ifndef USE_ESC_SENSOR
390 featureDisable(FEATURE_ESC_SENSOR
);
393 #ifndef USE_GYRO_DATA_ANALYSE
394 featureDisable(FEATURE_DYNAMIC_FILTER
);
397 #if !defined(USE_ADC)
398 featureDisable(FEATURE_RSSI_ADC
);
401 #if defined(USE_BEEPER)
402 if (beeperDevConfig()->frequency
&& !timerGetByTag(beeperDevConfig()->ioTag
)) {
403 beeperDevConfigMutable()->frequency
= 0;
406 if (beeperConfig()->beeper_off_flags
& ~BEEPER_ALLOWED_MODES
) {
407 beeperConfigMutable()->beeper_off_flags
= 0;
411 if (beeperConfig()->dshotBeaconOffFlags
& ~DSHOT_BEACON_ALLOWED_MODES
) {
412 beeperConfigMutable()->dshotBeaconOffFlags
= 0;
415 if (beeperConfig()->dshotBeaconTone
< DSHOT_CMD_BEACON1
416 || beeperConfig()->dshotBeaconTone
> DSHOT_CMD_BEACON5
) {
417 beeperConfigMutable()->dshotBeaconTone
= DSHOT_CMD_BEACON1
;
422 #if defined(USE_DSHOT_TELEMETRY)
423 if (motorConfig()->dev
.useBurstDshot
&& motorConfig()->dev
.useDshotTelemetry
) {
424 motorConfigMutable()->dev
.useDshotTelemetry
= false;
428 #if defined(TARGET_VALIDATECONFIG)
429 targetValidateConfiguration();
433 void validateAndFixGyroConfig(void)
435 #ifdef USE_GYRO_DATA_ANALYSE
436 // Disable dynamic filter if gyro loop is less than 2KHz
437 if (gyro
.targetLooptime
> DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME
) {
438 featureDisable(FEATURE_DYNAMIC_FILTER
);
442 // Prevent invalid notch cutoff
443 if (gyroConfig()->gyro_soft_notch_cutoff_1
>= gyroConfig()->gyro_soft_notch_hz_1
) {
444 gyroConfigMutable()->gyro_soft_notch_hz_1
= 0;
446 if (gyroConfig()->gyro_soft_notch_cutoff_2
>= gyroConfig()->gyro_soft_notch_hz_2
) {
447 gyroConfigMutable()->gyro_soft_notch_hz_2
= 0;
450 //Prevent invalid dynamic lowpass filter
451 if (gyroConfig()->dyn_lpf_gyro_min_hz
> gyroConfig()->dyn_lpf_gyro_max_hz
) {
452 gyroConfigMutable()->dyn_lpf_gyro_min_hz
= 0;
456 if (gyroConfig()->gyro_hardware_lpf
== GYRO_HARDWARE_LPF_1KHZ_SAMPLE
) {
457 pidConfigMutable()->pid_process_denom
= 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
458 gyroConfigMutable()->gyro_sync_denom
= 1;
459 gyroConfigMutable()->gyro_use_32khz
= false;
462 if (gyroConfig()->gyro_use_32khz
) {
463 // F1 and F3 can't handle high sample speed.
465 gyroConfigMutable()->gyro_sync_denom
= MAX(gyroConfig()->gyro_sync_denom
, 16);
466 #elif defined(STM32F3)
467 gyroConfigMutable()->gyro_sync_denom
= MAX(gyroConfig()->gyro_sync_denom
, 4);
471 gyroConfigMutable()->gyro_sync_denom
= MAX(gyroConfig()->gyro_sync_denom
, 3);
476 switch (gyroMpuDetectionResult()->sensor
) {
478 samplingTime
= 1.0f
/ 9000.0f
;
481 samplingTime
= 0.0003125f
;
484 samplingTime
= 0.000125f
;
487 if (gyroConfig()->gyro_hardware_lpf
== GYRO_HARDWARE_LPF_1KHZ_SAMPLE
) {
488 switch (gyroMpuDetectionResult()->sensor
) {
490 samplingTime
= 1.0f
/ 1100.0f
;
493 samplingTime
= 0.001f
;
497 if (gyroConfig()->gyro_use_32khz
) {
498 samplingTime
= 0.00003125;
501 // check for looptime restrictions based on motor protocol. Motor times have safety margin
502 float motorUpdateRestriction
;
503 switch (motorConfig()->dev
.motorPwmProtocol
) {
504 case PWM_TYPE_STANDARD
:
505 motorUpdateRestriction
= 1.0f
/ BRUSHLESS_MOTORS_PWM_RATE
;
507 case PWM_TYPE_ONESHOT125
:
508 motorUpdateRestriction
= 0.0005f
;
510 case PWM_TYPE_ONESHOT42
:
511 motorUpdateRestriction
= 0.0001f
;
514 case PWM_TYPE_DSHOT150
:
515 motorUpdateRestriction
= 0.000250f
;
517 case PWM_TYPE_DSHOT300
:
518 motorUpdateRestriction
= 0.0001f
;
522 motorUpdateRestriction
= 0.00003125f
;
526 if (motorConfig()->dev
.useUnsyncedPwm
) {
527 // Prevent overriding the max rate of motors
528 if ((motorConfig()->dev
.motorPwmProtocol
<= PWM_TYPE_BRUSHED
) && (motorConfig()->dev
.motorPwmProtocol
!= PWM_TYPE_STANDARD
)) {
529 const uint32_t maxEscRate
= lrintf(1.0f
/ motorUpdateRestriction
);
530 motorConfigMutable()->dev
.motorPwmRate
= MIN(motorConfig()->dev
.motorPwmRate
, maxEscRate
);
533 const float pidLooptime
= samplingTime
* gyroConfig()->gyro_sync_denom
* pidConfig()->pid_process_denom
;
534 if (pidLooptime
< motorUpdateRestriction
) {
535 const uint8_t minPidProcessDenom
= constrain(motorUpdateRestriction
/ (samplingTime
* gyroConfig()->gyro_sync_denom
), 1, MAX_PID_PROCESS_DENOM
);
536 pidConfigMutable()->pid_process_denom
= MAX(pidConfigMutable()->pid_process_denom
, minPidProcessDenom
);
542 if (blackboxConfig()->device
== 1) { // BLACKBOX_DEVICE_FLASH (but not defined)
543 blackboxConfigMutable()->device
= BLACKBOX_DEVICE_NONE
;
545 #endif // USE_FLASHFS
548 if (blackboxConfig()->device
== 2) { // BLACKBOX_DEVICE_SDCARD (but not defined)
549 blackboxConfigMutable()->device
= BLACKBOX_DEVICE_NONE
;
552 #endif // USE_BLACKBOX
555 bool readEEPROM(void)
557 suspendRxPwmPpmSignal();
559 // Sanity check, read flash
560 bool success
= loadEEPROM();
562 validateAndFixConfig();
566 resumeRxPwmPpmSignal();
571 static void ValidateAndWriteConfigToEEPROM(bool setConfigured
)
573 validateAndFixConfig();
575 suspendRxPwmPpmSignal();
577 #ifdef USE_CONFIGURATION_STATE
579 systemConfigMutable()->configured
= true;
582 UNUSED(setConfigured
);
585 writeConfigToEEPROM();
587 resumeRxPwmPpmSignal();
590 void writeEEPROM(void)
592 ValidateAndWriteConfigToEEPROM(true);
595 void writeEEPROMWithFeatures(uint32_t features
)
598 featureEnable(features
);
600 ValidateAndWriteConfigToEEPROM(true);
603 void resetEEPROM(void)
607 ValidateAndWriteConfigToEEPROM(false);
612 void ensureEEPROMStructureIsValid(void)
614 if (isEEPROMStructureValid()) {
620 void saveConfigAndNotify(void)
622 ValidateAndWriteConfigToEEPROM(true);
624 beeperConfirmationBeeps(1);
627 void changePidProfile(uint8_t pidProfileIndex
)
629 if (pidProfileIndex
< MAX_PROFILE_COUNT
) {
630 systemConfigMutable()->pidProfileIndex
= pidProfileIndex
;
633 pidInit(currentPidProfile
);
636 beeperConfirmationBeeps(pidProfileIndex
+ 1);
639 bool isSystemConfigured(void)
641 #ifdef USE_CONFIGURATION_STATE
642 return systemConfig()->configured
;