基于RT-Thread的IMU姿态解算源代码

/** ****************************************************************************** * @file EEPROM_Emulation/src/eeprom.c * @author MCD Application Team * @version V3.1.0 * @date 07/27/2009 * @brief This file provides all the EEPROM emulation firmware functions. ****************************************************************************** * @copy * * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. * * <h2><center>&copy; COPYRIGHT 2009 STMicroelectronics</center></h2> */ /** @addtogroup EEPROM_Emulation * @{ */ /* Includes ------------------------------------------------------------------*/ #include "eeprom.h" #include "rtthread.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ /* Global variable used to store variable value in read sequence */ uint16_t DataVar = 0; uint32_t CurWrAddress; /* Virtual address defined by the user: 0xFFFF value is prohibited */ extern uint16_t VirtAddVarTab[]; extern uint16_t NumbOfVar; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ static FLASH_Status EE_Format(void); static uint16_t EE_FindValidPage(uint8_t Operation); static uint16_t EE_VerifyPageFullWriteVariable(uint16_t VirtAddress, uint16_t Data); static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data); //初始化写地址，减少每次读写时查询时间 uint16_t InitCurrWrAddress(void) { FLASH_Status FlashStatus = FLASH_COMPLETE; uint16_t ValidPage = PAGE0; //uint32_t Address; uint32_t PageEndAddress; /* Get valid Page for write operation */ ValidPage = EE_FindValidPage(WRITE_IN_VALID_PAGE); /* Check if there is no valid page */ if (ValidPage == NO_VALID_PAGE) { CurWrAddress = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(ValidPage * PAGE_SIZE)); return NO_VALID_PAGE; } /* Get the valid Page start Address */ //Address = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(ValidPage * PAGE_SIZE)); CurWrAddress = (uint32_t)(EEPROM_START_ADDRESS + (uint32_t)(ValidPage * PAGE_SIZE)); /* Get the valid Page end Address */ PageEndAddress = (uint32_t)((EEPROM_START_ADDRESS - 2) + (uint32_t)((1 + ValidPage) * PAGE_SIZE)); /* Check each active page address starting from begining */ while (CurWrAddress < PageEndAddress) { /* Verify if Address and Address+2 contents are 0xFFFFFFFF */ if ((*(__IO uint32_t*)CurWrAddress) == 0xFFFFFFFF) { /* Set variable virtual address */ FlashStatus = FLASH_COMPLETE; /* Return program operation status */ return FlashStatus; } else { /* Next address location */ CurWrAddress = CurWrAddress + 4; } } /* Return PAGE_FULL in case the valid page is full */ return PAGE_FULL; } /** * @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]) { 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 -