diff --git a/src/config.c b/src/config.c index 2fa6c49b11..b23ecff712 100755 --- a/src/config.c +++ b/src/config.c @@ -45,7 +45,7 @@ config_t cfg; // profile config struct static const uint8_t EEPROM_CONF_VERSION = 64; static void resetConf(void); -static uint8_t validEEPROM(void) +static bool isEEPROMContentValid(void) { const master_t *temp = (const master_t *)FLASH_WRITE_ADDR; const uint8_t *p; @@ -53,11 +53,11 @@ static uint8_t validEEPROM(void) // check version number if (EEPROM_CONF_VERSION != temp->version) - return 0; + return false; // check size and magic numbers if (temp->size != sizeof(master_t) || temp->magic_be != 0xBE || temp->magic_ef != 0xEF) - return 0; + return false; // verify integrity of temporary copy for (p = (const uint8_t *)temp; p < ((const uint8_t *)temp + sizeof(master_t)); p++) @@ -65,16 +65,16 @@ static uint8_t validEEPROM(void) // checksum failed if (chk != 0) - return 0; + return false; // looks good, let's roll! - return 1; + return true; } void readEEPROM(void) { // Sanity check - if (!validEEPROM()) + if (!isEEPROMContentValid()) failureMode(10); // Read flash @@ -146,19 +146,24 @@ retry: FLASH_Lock(); // Flash write failed - just die now - if (tries == 3 || !validEEPROM()) { + if (tries == 3 || !isEEPROMContentValid()) { failureMode(10); } } -void checkFirstTime(bool reset) +void ensureEEPROMContainsValidData(void) { - // check the EEPROM integrity before resetting values - if (!validEEPROM() || reset) { - resetConf(); - writeEEPROM(); - readEEPROM(); + if (isEEPROMContentValid()) { + return; } + + resetEEPROM(); +} + +void resetEEPROM(void) +{ + resetConf(); + writeEEPROM(); } // Default settings diff --git a/src/config.h b/src/config.h index 74d8c992b9..bfbd64b786 100644 --- a/src/config.h +++ b/src/config.h @@ -40,7 +40,8 @@ uint32_t featureMask(void); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); +void resetEEPROM(void); void readEEPROM(void); void readEEPROMAndNotify(void); void writeEEPROM(); -void checkFirstTime(bool reset); +void ensureEEPROMContainsValidData(void); diff --git a/src/main.c b/src/main.c index 765f15292b..1bc5dcea6d 100755 --- a/src/main.c +++ b/src/main.c @@ -38,7 +38,7 @@ int main(void) systemInit(); initPrintfSupport(); - checkFirstTime(false); + ensureEEPROMContainsValidData(); readEEPROM(); // configure power ADC diff --git a/src/serial_cli.c b/src/serial_cli.c index 73b3d14aec..9178aabdb9 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -550,15 +550,6 @@ static void cliCMix(char *cmdline) } } -static void cliDefaults(char *cmdline) -{ - cliPrint("Resetting to defaults...\r\n"); - checkFirstTime(true); - cliPrint("Rebooting..."); - delay(10); - systemReset(false); -} - static void cliDump(char *cmdline) { int i; @@ -842,14 +833,25 @@ static void cliProfile(char *cmdline) } } +static void cliReboot(void) { + cliPrint("\r\nRebooting..."); + delay(10); + systemReset(false); +} + static void cliSave(char *cmdline) { cliPrint("Saving..."); copyCurrentProfileToProfileSlot(mcfg.current_profile); writeEEPROM(); - cliPrint("\r\nRebooting..."); - delay(10); - systemReset(false); + cliReboot(); +} + +static void cliDefaults(char *cmdline) +{ + cliPrint("Resetting to defaults..."); + resetEEPROM(); + cliReboot(); } static void cliPrint(const char *str) diff --git a/src/serial_msp.c b/src/serial_msp.c index 0c8b7375c4..b85bbcf8d9 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -601,8 +601,10 @@ static void evaluateCommand(void) headSerialReply(0); break; case MSP_RESET_CONF: - if (!f.ARMED) - checkFirstTime(true); + if (!f.ARMED) { + resetEEPROM(); + readEEPROM(); + } headSerialReply(0); break; case MSP_ACC_CALIBRATION: