2 * This file is part of Cleanflight.
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.
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.
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/>.
24 #include "build/build_config.h"
25 #include "build/debug.h"
27 #include "blackbox/blackbox_io.h"
29 #include "common/color.h"
30 #include "common/axis.h"
31 #include "common/maths.h"
32 #include "common/filter.h"
34 #include "config/config_eeprom.h"
35 #include "config/feature.h"
36 #include "config/parameter_group.h"
37 #include "config/parameter_group_ids.h"
39 #include "drivers/system.h"
40 #include "drivers/pwm_mapping.h"
41 #include "drivers/pwm_output.h"
42 #include "drivers/serial.h"
43 #include "drivers/timer.h"
44 #include "drivers/bus_i2c.h"
46 #include "sensors/sensors.h"
47 #include "sensors/gyro.h"
48 #include "sensors/compass.h"
49 #include "sensors/acceleration.h"
50 #include "sensors/battery.h"
51 #include "sensors/boardalignment.h"
52 #include "sensors/rangefinder.h"
54 #include "io/beeper.h"
55 #include "io/serial.h"
56 #include "io/ledstrip.h"
62 #include "flight/mixer.h"
63 #include "flight/servos.h"
64 #include "flight/pid.h"
65 #include "flight/imu.h"
66 #include "flight/failsafe.h"
67 #include "flight/ez_tune.h"
69 #include "fc/config.h"
70 #include "fc/controlrate_profile.h"
71 #include "fc/rc_adjustments.h"
72 #include "fc/rc_controls.h"
73 #include "fc/rc_curves.h"
74 #include "fc/rc_modes.h"
75 #include "fc/runtime_config.h"
76 #include "fc/settings.h"
78 #include "navigation/navigation.h"
80 #ifndef DEFAULT_FEATURES
81 #define DEFAULT_FEATURES 0
84 #define BRUSHED_MOTORS_PWM_RATE 16000
85 #define BRUSHLESS_MOTORS_PWM_RATE 400
87 #if !defined(VBAT_ADC_CHANNEL)
88 #define VBAT_ADC_CHANNEL ADC_CHN_NONE
90 #if !defined(RSSI_ADC_CHANNEL)
91 #define RSSI_ADC_CHANNEL ADC_CHN_NONE
93 #if !defined(CURRENT_METER_ADC_CHANNEL)
94 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_NONE
96 #if !defined(AIRSPEED_ADC_CHANNEL)
97 #define AIRSPEED_ADC_CHANNEL ADC_CHN_NONE
100 PG_REGISTER_WITH_RESET_TEMPLATE(featureConfig_t
, featureConfig
, PG_FEATURE_CONFIG
, 0);
102 PG_RESET_TEMPLATE(featureConfig_t
, featureConfig
,
103 .enabledFeatures
= DEFAULT_FEATURES
| COMMON_DEFAULT_FEATURES
106 PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t
, systemConfig
, PG_SYSTEM_CONFIG
, 7);
108 PG_RESET_TEMPLATE(systemConfig_t
, systemConfig
,
109 .current_profile_index
= 0,
110 .current_battery_profile_index
= 0,
111 .current_mixer_profile_index
= 0,
112 .debug_mode
= SETTING_DEBUG_MODE_DEFAULT
,
114 .groundTestMode
= SETTING_GROUND_TEST_MODE_DEFAULT
, // disables motors, set heading trusted for FW (for dev use)
117 .i2c_speed
= SETTING_I2C_SPEED_DEFAULT
,
119 .throttle_tilt_compensation_strength
= SETTING_THROTTLE_TILT_COMP_STR_DEFAULT
, // 0-100, 0 - disabled
120 .craftName
= SETTING_NAME_DEFAULT
,
121 .pilotName
= SETTING_NAME_DEFAULT
124 PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t
, beeperConfig
, PG_BEEPER_CONFIG
, 2);
126 PG_RESET_TEMPLATE(beeperConfig_t
, beeperConfig
,
127 .beeper_off_flags
= 0,
128 .preferred_beeper_off_flags
= 0,
129 .dshot_beeper_enabled
= SETTING_DSHOT_BEEPER_ENABLED_DEFAULT
,
130 .dshot_beeper_tone
= SETTING_DSHOT_BEEPER_TONE_DEFAULT
,
131 .pwmMode
= SETTING_BEEPER_PWM_MODE_DEFAULT
,
134 PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t
, adcChannelConfig
, PG_ADC_CHANNEL_CONFIG
, 0);
136 PG_RESET_TEMPLATE(adcChannelConfig_t
, adcChannelConfig
,
137 .adcFunctionChannel
= {
138 [ADC_BATTERY
] = VBAT_ADC_CHANNEL
,
139 [ADC_RSSI
] = RSSI_ADC_CHANNEL
,
140 [ADC_CURRENT
] = CURRENT_METER_ADC_CHANNEL
,
141 [ADC_AIRSPEED
] = AIRSPEED_ADC_CHANNEL
,
145 #define SAVESTATE_NONE 0
146 #define SAVESTATE_SAVEONLY 1
147 #define SAVESTATE_SAVEANDNOTIFY 2
149 static uint8_t saveState
= SAVESTATE_NONE
;
151 void validateNavConfig(void)
153 // Make sure minAlt is not more than maxAlt, maxAlt cannot be set lower than 500.
154 navConfigMutable()->general
.land_slowdown_minalt
= MIN(navConfig()->general
.land_slowdown_minalt
, navConfig()->general
.land_slowdown_maxalt
- 100);
158 // Stubs to handle target-specific configs
159 __attribute__((weak
)) void validateAndFixTargetConfig(void)
161 #if !defined(SITL_BUILD)
166 __attribute__((weak
)) void targetConfiguration(void)
168 #if !defined(SITL_BUILD)
173 #ifdef SWAP_SERIAL_PORT_0_AND_1_DEFAULTS
174 #define FIRST_PORT_INDEX 1
175 #define SECOND_PORT_INDEX 0
177 #define FIRST_PORT_INDEX 0
178 #define SECOND_PORT_INDEX 1
181 uint32_t getLooptime(void)
183 return gyroConfig()->looptime
;
186 uint32_t getGyroLooptime(void)
188 return gyro
.targetLooptime
;
191 void validateAndFixConfig(void)
194 #ifdef USE_ADAPTIVE_FILTER
195 // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz
196 if (gyroConfig()->adaptiveFilterMinHz
+ 5 > gyroConfig()->gyro_main_lpf_hz
) {
197 gyroConfigMutable()->adaptiveFilterMinHz
= gyroConfig()->gyro_main_lpf_hz
- 5;
199 //gyroConfig()->adaptiveFilterMaxHz has to be at least 5 units higher than gyroConfig()->gyro_main_lpf_hz
200 if (gyroConfig()->adaptiveFilterMaxHz
- 5 < gyroConfig()->gyro_main_lpf_hz
) {
201 gyroConfigMutable()->adaptiveFilterMaxHz
= gyroConfig()->gyro_main_lpf_hz
+ 5;
205 if (accelerometerConfig()->acc_notch_cutoff
>= accelerometerConfig()->acc_notch_hz
) {
206 accelerometerConfigMutable()->acc_notch_hz
= 0;
209 // Disable unused features
210 featureClear(FEATURE_UNUSED_1
| FEATURE_UNUSED_3
| FEATURE_UNUSED_4
| FEATURE_UNUSED_5
| FEATURE_UNUSED_6
| FEATURE_UNUSED_7
| FEATURE_UNUSED_8
| FEATURE_UNUSED_9
| FEATURE_UNUSED_10
);
212 #if defined(USE_LED_STRIP) && (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
213 if (featureConfigured(FEATURE_SOFTSERIAL
) && featureConfigured(FEATURE_LED_STRIP
)) {
214 const timerHardware_t
*ledTimerHardware
= timerGetByTag(IO_TAG(WS2811_PIN
), TIM_USE_ANY
);
215 if (ledTimerHardware
!= NULL
) {
216 bool sameTimerUsed
= false;
218 #if defined(USE_SOFTSERIAL1)
219 const timerHardware_t
*ss1TimerHardware
= timerGetByTag(IO_TAG(SOFTSERIAL_1_RX_PIN
), TIM_USE_ANY
);
220 if (ss1TimerHardware
!= NULL
&& ss1TimerHardware
->tim
== ledTimerHardware
->tim
) {
221 sameTimerUsed
= true;
224 #if defined(USE_SOFTSERIAL2)
225 const timerHardware_t
*ss2TimerHardware
= timerGetByTag(IO_TAG(SOFTSERIAL_2_RX_PIN
), TIM_USE_ANY
);
226 if (ss2TimerHardware
!= NULL
&& ss2TimerHardware
->tim
== ledTimerHardware
->tim
) {
227 sameTimerUsed
= true;
231 // led strip needs the same timer as softserial
232 featureClear(FEATURE_LED_STRIP
);
238 #ifndef USE_SERVO_SBUS
239 if (servoConfig()->servo_protocol
== SERVO_TYPE_SBUS
|| servoConfig()->servo_protocol
== SERVO_TYPE_SBUS_PWM
) {
240 servoConfigMutable()->servo_protocol
= SERVO_TYPE_PWM
;
244 if (!isSerialConfigValid(serialConfigMutable())) {
245 pgResetCopy(serialConfigMutable(), PG_SERIAL_CONFIG
);
248 // Ensure sane values of navConfig settings
251 // Limitations of different protocols
252 #if !defined(USE_DSHOT)
253 if (motorConfig()->motorPwmProtocol
> PWM_TYPE_BRUSHED
) {
254 motorConfigMutable()->motorPwmProtocol
= PWM_TYPE_MULTISHOT
;
258 // Call target-specific validation function
259 validateAndFixTargetConfig();
262 if (compassConfig()->mag_align
== ALIGN_DEFAULT
) {
263 compassConfigMutable()->mag_align
= CW270_DEG_FLIP
;
267 if (settingsValidate(NULL
)) {
268 DISABLE_ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING
);
270 ENABLE_ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING
);
274 void applyAndSaveBoardAlignmentDelta(int16_t roll
, int16_t pitch
)
276 updateBoardAlignment(roll
, pitch
);
277 saveConfigAndNotify();
281 void createDefaultConfig(void)
284 #ifdef RX_CHANNELS_TAER
285 parseRcChannels("TAER1234");
287 parseRcChannels("AETR1234");
291 #ifdef ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
292 featureSet(FEATURE_BLACKBOX
);
296 featureSet(FEATURE_AIRMODE
);
298 targetConfiguration();
301 int port
= findSerialPortIndexByIdentifier(MSP_UART
);
303 serialConfigMutable()->portConfigs
[port
].functionMask
= FUNCTION_MSP
;
304 serialConfigMutable()->portConfigs
[port
].msp_baudrateIndex
= BAUD_115200
;
309 void resetConfigs(void)
311 pgResetAll(MAX_PROFILE_COUNT
);
312 pgActivateProfile(0);
314 createDefaultConfig();
316 setConfigProfile(getConfigProfile());
318 reevaluateLedConfig();
322 static void activateConfig(void)
324 activateControlRateConfig();
325 activateBatteryProfile();
326 activateMixerConfig();
328 resetAdjustmentStates();
330 updateUsedModeActivationConditionFlags();
334 accSetCalibrationValues();
344 void readEEPROM(void)
346 // Sanity check, read flash
348 failureMode(FAILURE_INVALID_EEPROM_CONTENTS
);
351 setConfigProfile(getConfigProfile());
352 setConfigBatteryProfile(getConfigBatteryProfile());
353 setConfigMixerProfile(getConfigMixerProfile());
355 validateAndFixConfig();
359 void processSaveConfigAndNotify(void)
365 beeperConfirmationBeeps(1);
367 osdShowEEPROMSavedNotification();
371 void writeEEPROM(void)
373 writeConfigToEEPROM();
376 void resetEEPROM(void)
381 void ensureEEPROMContainsValidData(void)
383 if (isEEPROMContentValid()) {
393 * Used to save the EEPROM and notify the user with beeps and OSD notifications.
394 * This consolidates all save calls in the loop in to a single save operation. This save is actioned in the next loop, if the model is disarmed.
396 void saveConfigAndNotify(void)
399 osdStartedSaveProcess();
401 saveState
= SAVESTATE_SAVEANDNOTIFY
;
405 * Used to save the EEPROM without notifications. Can be used instead of writeEEPROM() if no reboot is called after the write.
406 * This consolidates all save calls in the loop in to a single save operation. This save is actioned in the next loop, if the model is disarmed.
407 * If any save with notifications are requested, notifications are shown.
409 void saveConfig(void)
411 if (saveState
!= SAVESTATE_SAVEANDNOTIFY
) {
412 saveState
= SAVESTATE_SAVEONLY
;
416 void processDelayedSave(void)
418 if (saveState
== SAVESTATE_SAVEANDNOTIFY
) {
419 processSaveConfigAndNotify();
420 saveState
= SAVESTATE_NONE
;
421 } else if (saveState
== SAVESTATE_SAVEONLY
) {
425 saveState
= SAVESTATE_NONE
;
429 uint8_t getConfigProfile(void)
431 return systemConfig()->current_profile_index
;
434 bool setConfigProfile(uint8_t profileIndex
)
436 bool ret
= true; // return true if current_profile_index has changed
437 if (systemConfig()->current_profile_index
== profileIndex
) {
440 if (profileIndex
>= MAX_PROFILE_COUNT
) {// sanity check
443 pgActivateProfile(profileIndex
);
444 systemConfigMutable()->current_profile_index
= profileIndex
;
445 // set the control rate profile to match
446 setControlRateProfile(profileIndex
);
453 void setConfigProfileAndWriteEEPROM(uint8_t profileIndex
)
455 if (setConfigProfile(profileIndex
)) {
456 // profile has changed, so ensure current values saved before new profile is loaded
462 beeperConfirmationBeeps(profileIndex
+ 1);
465 uint8_t getConfigBatteryProfile(void)
467 return systemConfig()->current_battery_profile_index
;
470 bool setConfigBatteryProfile(uint8_t profileIndex
)
472 bool ret
= true; // return true if current_battery_profile_index has changed
473 if (systemConfig()->current_battery_profile_index
== profileIndex
) {
476 if (profileIndex
>= MAX_BATTERY_PROFILE_COUNT
) {// sanity check
479 systemConfigMutable()->current_battery_profile_index
= profileIndex
;
480 setBatteryProfile(profileIndex
);
484 void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex
)
486 if (setConfigBatteryProfile(profileIndex
)) {
487 // profile has changed, so ensure current values saved before new profile is loaded
493 beeperConfirmationBeeps(profileIndex
+ 1);
496 uint8_t getConfigMixerProfile(void)
498 return systemConfig()->current_mixer_profile_index
;
501 bool setConfigMixerProfile(uint8_t profileIndex
)
503 bool ret
= true; // return true if current_mixer_profile_index has changed
504 if (systemConfig()->current_mixer_profile_index
== profileIndex
) {
507 if (profileIndex
>= MAX_MIXER_PROFILE_COUNT
) {// sanity check
510 systemConfigMutable()->current_mixer_profile_index
= profileIndex
;
514 void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex
)
516 if (setConfigMixerProfile(profileIndex
)) {
517 // profile has changed, so ensure current values saved before new profile is loaded
523 beeperConfirmationBeeps(profileIndex
+ 1);
526 void setGyroCalibration(float getGyroZero
[XYZ_AXIS_COUNT
])
528 gyroConfigMutable()->gyro_zero_cal
[X
] = (int16_t) getGyroZero
[X
];
529 gyroConfigMutable()->gyro_zero_cal
[Y
] = (int16_t) getGyroZero
[Y
];
530 gyroConfigMutable()->gyro_zero_cal
[Z
] = (int16_t) getGyroZero
[Z
];
533 void setGravityCalibration(float getGravity
)
535 gyroConfigMutable()->gravity_cmss_cal
= getGravity
;
538 void beeperOffSet(uint32_t mask
)
540 beeperConfigMutable()->beeper_off_flags
|= mask
;
543 void beeperOffSetAll(uint8_t beeperCount
)
545 beeperConfigMutable()->beeper_off_flags
= (1 << beeperCount
) -1;
548 void beeperOffClear(uint32_t mask
)
550 beeperConfigMutable()->beeper_off_flags
&= ~(mask
);
553 void beeperOffClearAll(void)
555 beeperConfigMutable()->beeper_off_flags
= 0;
558 uint32_t getBeeperOffMask(void)
560 return beeperConfig()->beeper_off_flags
;
563 void setBeeperOffMask(uint32_t mask
)
565 beeperConfigMutable()->beeper_off_flags
= mask
;
568 uint32_t getPreferredBeeperOffMask(void)
570 return beeperConfig()->preferred_beeper_off_flags
;
573 void setPreferredBeeperOffMask(uint32_t mask
)
575 beeperConfigMutable()->preferred_beeper_off_flags
= mask
;