void ANO_Param::READ_GYRO_OFFSET(void)
{
uint16_t temp[3];
EE_ReadVariable(VirtAddVarTab[EE_6050_GYRO_X_OFFSET_ADDR], &temp[0]);
EE_ReadVariable(VirtAddVarTab[EE_6050_GYRO_Y_OFFSET_ADDR], &temp[1]);
EE_ReadVariable(VirtAddVarTab[EE_6050_GYRO_Z_OFFSET_ADDR], &temp[2]);
mpu6050.Gyro_Offset.x = temp[0];
mpu6050.Gyro_Offset.y = temp[1];
mpu6050.Gyro_Offset.z = temp[2];
}
/*====================================================================================================*/
static void EE_READ_Attitude_PID(void)
{
int16_t _temp;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_PITCH_P],&_temp);
// 参数为0 说明已经清空flash 则使用默认参数
if(_temp==0) return;
ctrl.pitch.core.kp =(float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_PITCH_I],&_temp);
ctrl.pitch.core.ki =(float)_temp / 1000;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_PITCH_D],&_temp);
ctrl.pitch.core.kd = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_ROLL_P],&_temp);
ctrl.roll.core.kp =(float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_ROLL_I],&_temp);
ctrl.roll.core.ki = (float)_temp / 1000;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_ROLL_D],&_temp);
ctrl.roll.core.kd = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_YAW_P],&_temp);
ctrl.yaw.core.kp =(float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_YAW_I],&_temp);
ctrl.yaw.core.ki =(float)_temp / 1000;
EE_ReadVariable(VirtAddVarTab[EE_PID_CORE_YAW_D], &_temp);
ctrl.yaw.core.kd =(float)_temp / 100;
}
/* read from flash and set --------------------------------------------------*/
u16 readFlashParameter(u16 para)
{
u16 val;
if (para == PARA_SET)
{
EE_ReadVariable(VirtAddVarTab[PARA_SET], &val);
parameter[PARA_SET] = val;
}
else
{
EE_ReadVariable(VirtAddVarTab[para+parameter[PARA_SET]], &val);
parameter[para+parameter[PARA_SET]] = val;
}
return val;
}
/*
* Pobranie informacji o ilosci aktualnie zapisanych wcisniec
* klawiszy
*/
void getHowMany(void)
{
EE_ReadVariable(VirtAddVarTab[63], &howMany);
if (howMany < 0x00 || howMany > 0x3F)
{
resetEEPROM();
howMany = 0x00;
}
}
/**
* Read app_configuration from EEPROM. If this fails, default values will be used.
*
* @param conf
* A pointer to a app_configuration struct to write the read configuration to.
*/
void conf_general_read_app_configuration(app_configuration *conf) {
bool is_ok = true;
uint8_t *conf_addr = (uint8_t*)conf;
uint16_t var;
for (unsigned int i = 0;i < (sizeof(app_configuration) / 2);i++) {
if (EE_ReadVariable(EEPROM_BASE_APPCONF + i, &var) == 0) {
conf_addr[2 * i] = (var >> 8) & 0xFF;
conf_addr[2 * i + 1] = var & 0xFF;
} else {
/* read from flash and set --------------------------------------------------*/
u16 readFlashParameter(u16 para)
{
u16 val;
if (para == PARA_SET)
{
EE_ReadVariable(VirtAddVarTab[PARA_SET], &val);
parameter[PARA_SET] = val;
}
else
{
EE_ReadVariable(VirtAddVarTab[para+parameter[PARA_SET]], &val);
parameter[para+parameter[PARA_SET]] = val;
// if we change the SW Parameter we have to init the filter
if (para == PARA_SW) // = 4
{
initFilter(gyroAngle, copterAngle);
}
}
return val;
}
void eeprom_read_block(void *__dst, const void *__src, size_t __n) {
uint16_t data = 0xFF;
uint16_t eeprom_address = (unsigned) __src;
eeprom_init();
for (uint8_t c = 0; c < __n; c++) {
EE_ReadVariable(eeprom_address+c, &data);
*((uint8_t*)__dst + c) = data;
}
}
uint8_t eeprom_read_byte(uint8_t *pos) {
uint16_t data = 0xFF;
uint16_t eeprom_address = (unsigned)pos;
eeprom_init();
if (EE_ReadVariable(eeprom_address, &data) != EE_OK) {
return (unsigned char)data;
}
return (unsigned char)data;
}
/* eeprom: 01,23,45 -> Read results: (10)(32)(54) -> buffer: 012345 */
uint8_t eeprom_restore(uint8_t *buf, uint8_t virt_addr)
{
uint8_t i, retVal = 0;
for(i=0; i<3; i++) {
if (EE_ReadVariable(virt_addr + i, (uint16_t *) &buf[2*i])) {
/* the variable was not found or no valid page was found */
*((uint16_t *) &buf[2*i]) = 0xFFFF;
retVal = 1;
}
}
return retVal;
}
void setup_FLASH()
{
uint16_t tmp_eeprom_data_structure_version;
FLASH_Unlock();
EE_Init();
EE_ReadVariable(VirtAddVarTab[index_eeprom_data_structure_version],&tmp_eeprom_data_structure_version); //eeprom_data_structure_version lesen
if (tmp_eeprom_data_structure_version != eeprom_data_structure_version)
{
EE_Format();
EE_WriteVariable(VirtAddVarTab[index_eeprom_data_structure_version],eeprom_data_structure_version);
}
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
/* STM32F103xB HAL library initialization:
- Configure the Flash prefetch
- Systick timer is configured by default as source of time base, but user
can eventually implement his proper time base source (a general purpose
timer for example or other time source), keeping in mind that Time base
duration should be kept 1ms since PPP_TIMEOUT_VALUEs are defined and
handled in milliseconds basis.
- Set NVIC Group Priority to 4
- Low Level Initialization
*/
HAL_Init();
/* Configure the system clock to 64 MHz */
SystemClock_Config();
/* Unlock the Flash Program Erase controller */
HAL_FLASH_Unlock();
/* EEPROM Init */
EE_Init();
/* --- Store successively many values of the three variables in the EEPROM ---*/
/* Store 0x1000 values of Variable1 in EEPROM */
for (VarValue = 1; VarValue <= 0x1000; VarValue++)
{
EE_WriteVariable(VirtAddVarTab[0], VarValue);
}
/* read the last stored variables data*/
EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0]);
/* Store 0x2000 values of Variable2 in EEPROM */
for (VarValue = 1; VarValue <= 0x2000; VarValue++)
{
EE_WriteVariable(VirtAddVarTab[1], VarValue);
}
/* read the last stored variables data*/
EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0]);
EE_ReadVariable(VirtAddVarTab[1], &VarDataTab[1]);
/* Store 0x3000 values of Variable3 in EEPROM */
for (VarValue = 1; VarValue <= 0x3000; VarValue++)
{
EE_WriteVariable(VirtAddVarTab[2], VarValue);
}
/* read the last stored variables data*/
EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0]);
EE_ReadVariable(VirtAddVarTab[1], &VarDataTab[1]);
EE_ReadVariable(VirtAddVarTab[2], &VarDataTab[2]);
while (1)
{
};
}
/* load Parameter from virtual EEPROM ---------------------------------------*/
void loadParameter()
{
u16 val;
u16 i;
// test if flash is clear
EE_ReadVariable(401, &val);
if (val != 99)
{
loadDefault();
}
EE_WriteVariable(401, 99);
for (i = 0;i <= USED_PARAMETER; i++)
{
EE_ReadVariable(VirtAddVarTab[i], &val);
parameter[i] = val;
// for test
print_para(i);
Delay(2); // prevent txbuffer overvlow
}
}
void display_info_get(uint8_t *distortion_type, uint8_t *buf)
{
bool found_display_info = 1;
uint16_t temp_distortion = default_distortion_type;
EE_ReadVariable(EE_DISPLAY_INFO_DISTORTION_TYPE, &temp_distortion);
*distortion_type = temp_distortion;
uint16_t *readbuf = (uint16_t *)buf;
for (uint8_t i = 0; i < EE_DISPLAY_INFO_SIZE; i++) {
if (EE_ReadVariable(EE_DISPLAY_INFO_START+i, &readbuf[i])) {
found_display_info = 0;
break;
}
}
// if display info stuff wasn't found in EEPROM, load the defaults
if (!found_display_info) {
// offset back to the start of the buffer
generate_default_display_info(buf-4);
}
}
void getConfig()
{
FLASH_Unlock();
uint16_t i;
uint16_t *ptr;
EE_Init();
EE_ReadVariable(EADDR_IS_INITIALIZED,&i);
if(i != 0x5253)
{
//empty or corrupted EEPROM detected: write default config
//EE_Format();
EE_WriteVariable(EADDR_IS_INITIALIZED, 0x5253);
s.commutationMethod = commutationMethod_HALL;
s.inputMethod = inputMethod_stepDir;
s.encoder_PPR = 4000;
s.encoder_poles = 4;
s.encoder_counts_per_step = 10;
s.pid_Kp = 10;
s.pid_Ki = 0;
s.pid_Kd = 0;
s.usart_baud = 1152;
writeConfig(s);
return;
}
ptr = (uint16_t *)&s;
for(i=0; i<sizeof(servoConfig)/2;i++)
{
if(EE_ReadVariable(EADDR_CONFIG_START+i, ptr)!=0)
usart_sendStr("Flash Error\n\r");
ptr++;
}
return;
}
/* eeprom: 01,23,45 -> Read results: (10)(32)(54) -> memcpy 01|23|45 -> buffer: 012345 */
uint8_t eeprom_restore(uint8_t *buf, uint8_t virt_addr)
{
uint8_t i, retVal = 0;
uint16_t EE_Data;
for(i=0; i<3; i++) {
if (EE_ReadVariable(virt_addr + i, &EE_Data)) {
/* the variable was not found or no valid page was found */
EE_Data = 0xFFFF;
retVal = 1;
}
memcpy(&buf[2*i], &EE_Data, 2);
}
return retVal;
}
/*====================================================================================================*/
static void EE_READ_MAG_OFFSET(void)
{
EE_ReadVariable(VirtAddVarTab[3], &HMC58X3_limit[0]);
EE_ReadVariable(VirtAddVarTab[4], &HMC58X3_limit[1]);
EE_ReadVariable(VirtAddVarTab[5], &HMC58X3_limit[2]);
EE_ReadVariable(VirtAddVarTab[6], &HMC58X3_limit[3]);
EE_ReadVariable(VirtAddVarTab[7], &HMC58X3_limit[4]);
EE_ReadVariable(VirtAddVarTab[8], &HMC58X3_limit[5]);
}
void EE_Read_ACC_GYRO_Offset(void)
{
EE_ReadVariable(VirtAddVarTab[EE_ACC_X_OFFSET], (u16 *)(&mpu_info.offset_acc.x));
EE_ReadVariable(VirtAddVarTab[EE_ACC_Y_OFFSET], (u16 *)(&mpu_info.offset_acc.y));
EE_ReadVariable(VirtAddVarTab[EE_ACC_Z_OFFSET], (u16 *)(&mpu_info.offset_acc.z));
EE_ReadVariable(VirtAddVarTab[EE_GYRO_X_OFFSET], (u16 *)(&mpu_info.offset_gyro.x));
EE_ReadVariable(VirtAddVarTab[EE_GYRO_Y_OFFSET], (u16 *)(&mpu_info.offset_gyro.y));
EE_ReadVariable(VirtAddVarTab[EE_GYRO_Z_OFFSET], (u16 *)(&mpu_info.offset_gyro.z));
}
/* load Parameter from virtual EEPROM ---------------------------------------*/
void loadParameter()
{
u16 val;
u16 i;
for (i = 0;i <= USED_PARAMETER; i++)
{
EE_ReadVariable(VirtAddVarTab[i], &val);
parameter[i] = val;
// for test
print_para(i);
Delay(2); // prevent txbuffer overvlow
}
}
static uint16_t EE_TransferVariable(uint16_t VirtAddress, uint32_t BaseAddress)
{
if (( *(__IO uint16_t*)(BaseAddress + 6)) != VirtAddress)
{
/* Read the last variables' updates */
uint16_t ReadStatus = EE_ReadVariable(VirtAddress, &DataVar);
/* In case variable corresponding to the virtual address was found */
if (ReadStatus != 0x1)
{
/* Transfer the variable to the new page */
return EE_VerifyPageFullWriteVariable(VirtAddress, DataVar);
}
}
return FLASH_COMPLETE;
}
void EE_READ_PID(void)
{
u16 _temp;
EE_ReadVariable(VirtAddVarTab[EE_PID_ROL_P],&_temp);
PID_ROL.P = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_ROL_I],&_temp);
PID_ROL.I = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_ROL_D],&_temp);
PID_ROL.D = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_PIT_P],&_temp);
PID_PIT.P = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_PIT_I],&_temp);
PID_PIT.I = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_PIT_D],&_temp);
PID_PIT.D = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_YAW_P],&_temp);
PID_YAW.P = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_YAW_I],&_temp);
PID_YAW.I = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_PID_YAW_D],&_temp);
PID_YAW.D = (float)_temp / 100;
}
void EE_Read_Rate_PID(void)
{
u16 _temp;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_ROL_P],&_temp);
pidRollRate.kp = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_ROL_I],&_temp);
pidRollRate.ki = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_ROL_D],&_temp);
pidRollRate.kd = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_PIT_P],&_temp);
pidPitchRate.kp = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_PIT_I],&_temp);
pidPitchRate.ki = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_PIT_D],&_temp);
pidPitchRate.kd = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_YAW_P],&_temp);
pidYawRate.kp = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_YAW_I],&_temp);
pidYawRate.ki = (float)_temp / 100;
EE_ReadVariable(VirtAddVarTab[EE_RATE_PID_YAW_D],&_temp);
pidYawRate.kd = (float)_temp / 100;
}
/* load default parameter to flash ------------------------------------------*/
void loadDefault()
{
//#########################################################################
// store default set if flash - only set 1
//#########################################################################
EE_WriteVariable(VirtAddVarTab[PARA_SET], 0x00);
EE_WriteVariable(VirtAddVarTab[PARA_DEBUG], 0x00);
EE_WriteVariable(VirtAddVarTab[PARA_HW], 0x01);
EE_WriteVariable(VirtAddVarTab[PARA_VOLT], 0x8BC); //2236 = 10 Volt
EE_WriteVariable(VirtAddVarTab[PARA_SW], 0x01);
EE_WriteVariable(VirtAddVarTab[PARA_ACC_X_ZERO], 0x7FF);
EE_WriteVariable(VirtAddVarTab[PARA_ACC_Y_ZERO], 0x7F1);
EE_WriteVariable(VirtAddVarTab[PARA_ACC_Z_ZERO], 0x7F8);
EE_WriteVariable(VirtAddVarTab[PARA_SMOOTH_ACC], 0x00);
EE_WriteVariable(VirtAddVarTab[PARA_SMOOTH_GYRO], 0x00);
EE_WriteVariable(VirtAddVarTab[PARA_SMOOTH_RC], 0x00);
EE_WriteVariable(VirtAddVarTab[PARA_GYRO_X_90], 0x898);
EE_WriteVariable(VirtAddVarTab[PARA_GYRO_Y_90], 0x898);
EE_WriteVariable(VirtAddVarTab[PARA_GYRO_Z_90], 0x898);
EE_WriteVariable(VirtAddVarTab[PARA_ACC_FORCE], 0x1F4);
EE_WriteVariable(VirtAddVarTab[PARA_GYRO_CORR], 0x1F4);
EE_WriteVariable(VirtAddVarTab[PARA_SERVO0_CH], 0x5);
EE_WriteVariable(VirtAddVarTab[PARA_SERVO1_CH], 0x6);
EE_WriteVariable(VirtAddVarTab[PARA_SERVO2_CH], 0x7);
EE_WriteVariable(VirtAddVarTab[PARA_SERVO3_CH], 0x8);
EE_WriteVariable(VirtAddVarTab[PARA_CAM_X], 0x5014); //servo 2 , 0, wert 500
EE_WriteVariable(VirtAddVarTab[PARA_CAM_Y], 0x7724); //servo 3 , 0, wert 500
EE_WriteVariable(VirtAddVarTab[PARA_MIN_GAS], 0x20);
//#########################################################################
u16 val;
u16 i;
for (i = 0;i <= USED_PARAMETER; i++)
{
EE_ReadVariable(VirtAddVarTab[i], &val);
parameter[i] = val;
// for test
print_para(i);
Delay(2); // prevent txbuffer overvlow
}
// info user over uart1
send(DEFAULT);
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
/*!< At this stage the microcontroller clock setting is already configured,
this is done through SystemInit() function which is called from startup
file (startup_stm32f4xx.s) before to branch to application main.
To reconfigure the default setting of SystemInit() function, refer to
system_stm32f4xx.c file
*/
/* Unlock the Flash Program Erase controller */
FLASH_Unlock();
/* EEPROM Init */
EE_Init();
/* --- Store successively many values of the three variables in the EEPROM ---*/
/* Store 0x1000 values of Variable1 in EEPROM */
for (VarValue = 1; VarValue <= 0x1000; VarValue++)
{
EE_WriteVariable(VirtAddVarTab[0], VarValue);
}
/* read the last stored variables data*/
EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0]);
/* Store 0x2000 values of Variable2 in EEPROM */
for (VarValue = 1; VarValue <= 0x2000; VarValue++)
{
EE_WriteVariable(VirtAddVarTab[1], VarValue);
}
/* read the last stored variables data*/
EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0]);
EE_ReadVariable(VirtAddVarTab[1], &VarDataTab[1]);
/* Store 0x3000 values of Variable3 in EEPROM */
for (VarValue = 1; VarValue <= 0x3000; VarValue++)
{
EE_WriteVariable(VirtAddVarTab[2], VarValue);
}
/* read the last stored variables data*/
EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0]);
EE_ReadVariable(VirtAddVarTab[1], &VarDataTab[1]);
EE_ReadVariable(VirtAddVarTab[2], &VarDataTab[2]);
while (1);
}
// Load old calibrated accel scale parameters from before cross axis matrix
static void init_legacy_acc(void)
{
float_pack_t pack;
memset(g_cal.acc_scale, 0, sizeof(g_cal.acc_scale));
if (EE_ReadVariable(EE_ACC_SC_X_L, &pack.s[0]) || EE_ReadVariable(EE_ACC_SC_X_H, &pack.s[1]))
g_cal.acc_scale[0][0] = 1.0;
else
g_cal.acc_scale[0][0] = pack.f;
if (EE_ReadVariable(EE_ACC_SC_Y_L, &pack.s[0]) || EE_ReadVariable(EE_ACC_SC_Y_H, &pack.s[1]))
g_cal.acc_scale[1][1] = 1.0;
else
g_cal.acc_scale[1][1] = pack.f;
if (EE_ReadVariable(EE_ACC_SC_Z_L, &pack.s[0]) || EE_ReadVariable(EE_ACC_SC_Z_H, &pack.s[1]))
g_cal.acc_scale[2][2] = 1.0;
else
g_cal.acc_scale[2][2] = pack.f;
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
/* Enable the CPU Cache */
CPU_CACHE_Enable();
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* Configure the system clock to have a system clock = 180 Mhz */
SystemClock_Config();
/* Unlock the Flash Program Erase controller */
HAL_FLASH_Unlock();
/* Configure LED1 */
BSP_LED_Init(LED1);
/* EEPROM Init */
if( EE_Init() != EE_OK)
{
Error_Handler();
}
/* --- Store successively many values of the three variables in the EEPROM ---*/
/* Store 0x1000 values of Variable1 in EEPROM */
for (VarValue = 1; VarValue <= 0x1000; VarValue++)
{
/* Sequence 1 */
if((EE_WriteVariable(VirtAddVarTab[0], VarValue)) != HAL_OK)
{
Error_Handler();
}
if((EE_ReadVariable(VirtAddVarTab[0], &VarDataTab[0])) != HAL_OK)
{
Error_Handler();
}
if (VarValue != VarDataTab[0])
{
Error_Handler();
}
/* Sequence 2 */
if(EE_WriteVariable(VirtAddVarTab[1], ~VarValue) != HAL_OK)
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[1], &VarDataTab[1]) != HAL_OK)
{
Error_Handler();
}
if(((uint16_t)~VarValue) != VarDataTab[1])
{
Error_Handler();
}
/* Sequence 3 */
if(EE_WriteVariable(VirtAddVarTab[2], VarValue << 1) != HAL_OK)
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[2], &VarDataTab[2]) != HAL_OK)
{
Error_Handler();
}
if ((VarValue << 1) != VarDataTab[2])
{
Error_Handler();
}
}
/* Store 0x2000 values of Variable2 in EEPROM */
for (VarValue = 1; VarValue <= 0x2000; VarValue++)
{
if(EE_WriteVariable(VirtAddVarTab[1], VarValue) != HAL_OK)
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[1], &VarDataTab[1]) != HAL_OK)
{
Error_Handler();
}
if(VarValue != VarDataTab[1])
{
Error_Handler();
}
}
/* read the last stored variables data*/
if(EE_ReadVariable(VirtAddVarTab[0], &VarDataTmp) != HAL_OK)
{
Error_Handler();
}
if (VarDataTmp != VarDataTab[0])
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[1], &VarDataTmp) != HAL_OK)
{
Error_Handler();
}
if (VarDataTmp != VarDataTab[1])
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[2], &VarDataTmp) != HAL_OK)
{
Error_Handler();
}
if (VarDataTmp != VarDataTab[2])
{
Error_Handler();
}
/* Store 0x3000 values of Variable3 in EEPROM */
for (VarValue = 1; VarValue <= 0x3000; VarValue++)
{
if(EE_WriteVariable(VirtAddVarTab[2], VarValue) != HAL_OK)
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[2], &VarDataTab[2]) != HAL_OK)
{
Error_Handler();
}
if(VarValue != VarDataTab[2])
{
Error_Handler();
}
}
/* read the last stored variables data*/
if(EE_ReadVariable(VirtAddVarTab[0], &VarDataTmp) != HAL_OK)
{
Error_Handler();
}
if (VarDataTmp != VarDataTab[0])
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[1], &VarDataTmp) != HAL_OK)
{
Error_Handler();
}
if (VarDataTmp != VarDataTab[1])
{
Error_Handler();
}
if(EE_ReadVariable(VirtAddVarTab[2], &VarDataTmp) != HAL_OK)
{
Error_Handler();
}
if (VarDataTmp != VarDataTab[2])
{
Error_Handler();
}
while (1)
{
/* Turn LED1 On */
BSP_LED_On(LED1);
}
}
void EE_READ_GYRO_OFFSET(void)
{
EE_ReadVariable(VirtAddVarTab[EE_6050_GYRO_X_OFFSET_ADDR], &GYRO_OFFSET.X);
EE_ReadVariable(VirtAddVarTab[EE_6050_GYRO_Y_OFFSET_ADDR], &GYRO_OFFSET.Y);
EE_ReadVariable(VirtAddVarTab[EE_6050_GYRO_Z_OFFSET_ADDR], &GYRO_OFFSET.Z);
}
void EE_READ_ACC_OFFSET(void)
{
EE_ReadVariable(VirtAddVarTab[EE_6050_ACC_X_OFFSET_ADDR], &ACC_OFFSET.X);
EE_ReadVariable(VirtAddVarTab[EE_6050_ACC_Y_OFFSET_ADDR], &ACC_OFFSET.Y);
EE_ReadVariable(VirtAddVarTab[EE_6050_ACC_Z_OFFSET_ADDR], &ACC_OFFSET.Z);
}
/**
* @brief Restore the pages to a known good state in case of page's status
* corruption after a power loss.
* @param None.
* @retval - Flash error code: on write Flash error
* - FLASH_COMPLETE: on success
*/
uint16_t EE_Init(void)
{
uint16_t PageStatus0 = 6, PageStatus1 = 6;
uint16_t VarIdx = 0;
uint16_t EepromStatus = 0, ReadStatus = 0;
int16_t x = -1;
uint16_t FlashStatus;
/* Get Page0 status */
PageStatus0 = (*(__IO uint16_t*)PAGE0_BASE_ADDRESS);
/* Get Page1 status */
PageStatus1 = (*(__IO uint16_t*)PAGE1_BASE_ADDRESS);
/* Check for invalid header states and repair if necessary */
switch(PageStatus0) {
case ERASED:
if(PageStatus1 == VALID_PAGE) { /* Page0 erased, Page1 valid */
/* Erase Page0 */
FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
} else if(PageStatus1 == RECEIVE_DATA) { /* Page0 erased, Page1 receive */
/* Erase Page0 */
FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Mark Page1 as valid */
FlashStatus = FLASH_ProgramHalfWord(PAGE1_BASE_ADDRESS, VALID_PAGE);
/* If program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
} else { /* First EEPROM access (Page0&1 are erased) or invalid state -> format EEPROM */
/* Erase both Page0 and Page1 and set Page0 as valid page */
FlashStatus = EE_Format();
/* If erase/program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
}
break;
case RECEIVE_DATA:
if(PageStatus1 == VALID_PAGE) { /* Page0 receive, Page1 valid */
/* Transfer data from Page1 to Page0 */
for(VarIdx = 0; VarIdx < NumbOfVar; VarIdx++) {
if((*(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == VirtAddVarTab[VarIdx]) {
//前两个字节存PageStatus，随后每4字节为一组，先存数据值（前2字节），再存虚拟地址（后2字节），所以+6，是判断第一个组的虚拟地址:jkl
x = VarIdx;
}
if(VarIdx != x) {
/* Read the last variables' updates */
ReadStatus = EE_ReadVariable(VirtAddVarTab[VarIdx], &DataVar);
/* In case variable corresponding to the virtual address was found */
if(ReadStatus != 0x1) {
/* Transfer the variable to the Page0 */
EepromStatus = EE_VerifyPageFullWriteVariable(VirtAddVarTab[VarIdx], DataVar);
/* If program operation was failed, a Flash error code is returned */
if(EepromStatus != FLASH_COMPLETE) {
return EepromStatus;
}
}
}
}
/* Mark Page0 as valid */
FlashStatus = FLASH_ProgramHalfWord(PAGE0_BASE_ADDRESS, VALID_PAGE);
/* If program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Erase Page1 */
FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
} else if(PageStatus1 == ERASED) { /* Page0 receive, Page1 erased */
/* Erase Page1 */
FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Mark Page0 as valid */
FlashStatus = FLASH_ProgramHalfWord(PAGE0_BASE_ADDRESS, VALID_PAGE);
/* If program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
} else { /* Invalid state -> format eeprom */
/* Erase both Page0 and Page1 and set Page0 as valid page */
FlashStatus = EE_Format();
/* If erase/program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
}
break;
case VALID_PAGE:
if(PageStatus1 == VALID_PAGE) { /* Invalid state -> format eeprom */
/* Erase both Page0 and Page1 and set Page0 as valid page */
FlashStatus = EE_Format();
/* If erase/program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
} else if(PageStatus1 == ERASED) { /* Page0 valid, Page1 erased */
/* Erase Page1 */
FlashStatus = FLASH_ErasePage(PAGE1_BASE_ADDRESS);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
} else { /* Page0 valid, Page1 receive */
/* Transfer data from Page0 to Page1 */
for(VarIdx = 0; VarIdx < NumbOfVar; VarIdx++) {
if((*(__IO uint16_t*)(PAGE1_BASE_ADDRESS + 6)) == VirtAddVarTab[VarIdx]) {
x = VarIdx;
}
if(VarIdx != x) {
/* Read the last variables' updates */
ReadStatus = EE_ReadVariable(VirtAddVarTab[VarIdx], &DataVar);
/* In case variable corresponding to the virtual address was found */
if(ReadStatus != 0x1) {
/* Transfer the variable to the Page1 */
EepromStatus = EE_VerifyPageFullWriteVariable(VirtAddVarTab[VarIdx], DataVar);
/* If program operation was failed, a Flash error code is returned */
if(EepromStatus != FLASH_COMPLETE) {
return EepromStatus;
}
}
}
}
/* Mark Page1 as valid */
FlashStatus = FLASH_ProgramHalfWord(PAGE1_BASE_ADDRESS, VALID_PAGE);
/* If program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Erase Page0 */
FlashStatus = FLASH_ErasePage(PAGE0_BASE_ADDRESS);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
}
break;
default: /* Any other state -> format eeprom */
/* Erase both Page0 and Page1 and set Page0 as valid page */
FlashStatus = EE_Format();
/* If erase/program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
break;
}
return FLASH_COMPLETE;
}
/**
* @brief Transfers last updated variables data from the full Page to
* an empty one.
* @param VirtAddress: 16 bit virtual address of the variable
* @param Data: 16 bit data to be written as variable value
* @retval Success or error status:
* - FLASH_COMPLETE: on success
* - PAGE_FULL: if valid page is full
* - NO_VALID_PAGE: if no valid page was found
* - Flash error code: on write Flash error
*/
static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
{
FLASH_Status FlashStatus = FLASH_COMPLETE;
uint32_t NewPageAddress = 0x00000000, OldPageAddress = 0x00000000;
uint16_t ValidPage = PAGE0, VarIdx = 0;
uint16_t EepromStatus = 0, ReadStatus = 0;
/* Get active Page for read operation */
ValidPage = EE_FindValidPage(READ_FROM_VALID_PAGE);
if(ValidPage == PAGE1) { /* Page1 valid */
/* New page address where variable will be moved to */
NewPageAddress = PAGE0_BASE_ADDRESS;
/* Old page address where variable will be taken from */
OldPageAddress = PAGE1_BASE_ADDRESS;
} else if(ValidPage == PAGE0) { /* Page0 valid */
/* New page address where variable will be moved to */
NewPageAddress = PAGE1_BASE_ADDRESS;
/* Old page address where variable will be taken from */
OldPageAddress = PAGE0_BASE_ADDRESS;
} else {
return NO_VALID_PAGE; /* No valid Page */
}
/* Set the new Page status to RECEIVE_DATA status */
FlashStatus = FLASH_ProgramHalfWord(NewPageAddress, RECEIVE_DATA);
/* If program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Write the variable passed as parameter in the new active page */
EepromStatus = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
/* If program operation was failed, a Flash error code is returned */
if(EepromStatus != FLASH_COMPLETE) {
return EepromStatus;
}
/* Transfer process: transfer variables from old to the new active page */
for(VarIdx = 0; VarIdx < NumbOfVar; VarIdx++) {
if(VirtAddVarTab[VarIdx] != VirtAddress) { /* Check each variable except the one passed as parameter */
/* Read the other last variable updates */
ReadStatus = EE_ReadVariable(VirtAddVarTab[VarIdx], &DataVar);
/* In case variable corresponding to the virtual address was found */
if(ReadStatus != 0x1) {
/* Transfer the variable to the new active page */
EepromStatus = EE_VerifyPageFullWriteVariable(VirtAddVarTab[VarIdx], DataVar);
/* If program operation was failed, a Flash error code is returned */
if(EepromStatus != FLASH_COMPLETE) {
return EepromStatus;
}
}
}
}
/* Erase the old Page: Set old Page status to ERASED status */
FlashStatus = FLASH_ErasePage(OldPageAddress);
/* If erase operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Set new Page status to VALID_PAGE status */
FlashStatus = FLASH_ProgramHalfWord(NewPageAddress, VALID_PAGE);
/* If program operation was failed, a Flash error code is returned */
if(FlashStatus != FLASH_COMPLETE) {
return FlashStatus;
}
/* Return last operation flash status */
return FlashStatus;
}
int EEPROMFull (void)
{
uint16_t tmp;
EE_ReadVariable(VirtAddVarTab[63], &tmp);
return (tmp == 0x3F ? 1 : 0);
}
