diff --git a/src/config.c b/src/config.c index cddedcda7e..2fa6c49b11 100755 --- a/src/config.c +++ b/src/config.c @@ -90,7 +90,13 @@ void readEEPROM(void) setPIDController(cfg.pidController); gpsSetPIDs(); useFailsafeConfig(&cfg.failsafeConfig); +} +void readEEPROMAndNotify(void) +{ + // re-read written data + readEEPROM(); + blinkLedAndSoundBeeper(15, 20, 1); } void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex) @@ -99,7 +105,7 @@ void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex) memcpy(&mcfg.profile[profileSlotIndex], &cfg, sizeof(config_t)); } -void writeEEPROM(uint8_t b) +void writeEEPROM(void) { FLASH_Status status; uint32_t i; @@ -143,11 +149,6 @@ retry: if (tries == 3 || !validEEPROM()) { failureMode(10); } - - // re-read written data - readEEPROM(); - if (b) - blinkLedAndSoundBeeper(15, 20, 1); } void checkFirstTime(bool reset) @@ -155,7 +156,8 @@ void checkFirstTime(bool reset) // check the EEPROM integrity before resetting values if (!validEEPROM() || reset) { resetConf(); - writeEEPROM(0); + writeEEPROM(); + readEEPROM(); } } diff --git a/src/config.h b/src/config.h index f9d839e5a8..74d8c992b9 100644 --- a/src/config.h +++ b/src/config.h @@ -41,5 +41,6 @@ uint32_t featureMask(void); void copyCurrentProfileToProfileSlot(uint8_t profileSlotIndex); void readEEPROM(void); -void writeEEPROM(uint8_t b); +void readEEPROMAndNotify(void); +void writeEEPROM(); void checkFirstTime(bool reset); diff --git a/src/mw.c b/src/mw.c index 40dda9c5bb..23b6f8f546 100755 --- a/src/mw.c +++ b/src/mw.c @@ -496,7 +496,8 @@ void loop(void) i = 3; if (i) { mcfg.current_profile = i - 1; - writeEEPROM(0); + writeEEPROM(); + readEEPROM(); blinkLedAndSoundBeeper(2, 40, i); // TODO alarmArray[0] = i; } @@ -530,7 +531,8 @@ void loop(void) } if (i) { copyCurrentProfileToProfileSlot(mcfg.current_profile); - writeEEPROM(1); + writeEEPROM(); + readEEPROMAndNotify(); rcDelayCommand = 0; // allow autorepetition } } diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index 9c3b866049..d1087386a6 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -41,7 +41,8 @@ void ACC_Common(void) cfg.angleTrim[AI_ROLL] = 0; cfg.angleTrim[AI_PITCH] = 0; copyCurrentProfileToProfileSlot(mcfg.current_profile); - writeEEPROM(1); // write accZero in EEPROM + writeEEPROM(); // write accZero in EEPROM + readEEPROMAndNotify(); } calibratingA--; } @@ -92,7 +93,8 @@ void ACC_Common(void) cfg.angleTrim[AI_ROLL] = 0; cfg.angleTrim[AI_PITCH] = 0; copyCurrentProfileToProfileSlot(mcfg.current_profile); - writeEEPROM(1); // write accZero in EEPROM + writeEEPROM(); // write accZero in EEPROM + readEEPROMAndNotify(); } } diff --git a/src/sensors_compass.c b/src/sensors_compass.c index 799e57a3ee..84ffb15896 100644 --- a/src/sensors_compass.c +++ b/src/sensors_compass.c @@ -62,7 +62,8 @@ int Mag_getADC(void) mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets } copyCurrentProfileToProfileSlot(mcfg.current_profile); - writeEEPROM(1); + writeEEPROM(); + readEEPROMAndNotify(); } } diff --git a/src/serial_cli.c b/src/serial_cli.c index 0a52e42d72..73b3d14aec 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -835,7 +835,8 @@ static void cliProfile(char *cmdline) i = atoi(cmdline); if (i >= 0 && i <= 2) { mcfg.current_profile = i; - writeEEPROM(0); + writeEEPROM(); + readEEPROM(); cliProfile(""); } } @@ -845,7 +846,7 @@ static void cliSave(char *cmdline) { cliPrint("Saving..."); copyCurrentProfileToProfileSlot(mcfg.current_profile); - writeEEPROM(0); + writeEEPROM(); cliPrint("\r\nRebooting..."); delay(10); systemReset(false); diff --git a/src/serial_msp.c b/src/serial_msp.c index 64c3878f8f..0c8b7375c4 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -354,10 +354,11 @@ static void evaluateCommand(void) case MSP_SELECT_SETTING: if (!f.ARMED) { mcfg.current_profile = read8(); - if (mcfg.current_profile > 2) + if (mcfg.current_profile > 2) { mcfg.current_profile = 0; - // this writes new profile index and re-reads it - writeEEPROM(0); + } + writeEEPROM(); + readEEPROM(); } headSerialReply(0); break; @@ -619,7 +620,8 @@ static void evaluateCommand(void) headSerialError(0); } else { copyCurrentProfileToProfileSlot(mcfg.current_profile); - writeEEPROM(0); + writeEEPROM(); + readEEPROM(); headSerialReply(0); } break;