四軸飛控-STC8A8K64S4A12-LQFP44

ID:683143 · 發(fā)表于 2020-1-7 19:45

#include "config.h"
#include "EEPROM.h"

#define EE_ADDRESS1 0x0200

#define CMD_IDLE 0
#define CMD_READ 1
#define CMD_PROGRAM 2
#define CMD_ERASE 3

#define ENABLE_IAP 0x80          //if SYSCLK<30MHz
extern int xdata g_x,g_y,g_z; //陀螺儀矯正參數(shù)
extern float xdata a_x,a_y; //角度矯正參數(shù)
extern float data AngleX,AngleY;

u8 xdata tmp[20];

//========================================================================
// 函數(shù): void DisableEEPROM(void)
// 描述: 禁止訪問ISP/IAP.
// 參數(shù): non.
// 返回: non.
// 版本: V1.0, 2012-10-22
//========================================================================
void DisableEEPROM(void)
{
IAP_CONTR = 0; //禁止ISP/IAP操作
IAP_CMD = 0; //去除ISP/IAP命令
IAP_TRIG  = 0; //防止ISP/IAP命令誤觸發(fā)
IAP_ADDRH = 0xff; //清0地址高字節(jié)
IAP_ADDRL = 0xff; //清0地址低字節(jié)，指向非EEPROM區(qū)，防止誤操作
}

//========================================================================
// 函數(shù): void EEPROM_read_n(u16 EE_address,u8 *DataAddress,u16 number)
// 描述: 從指定EEPROM首地址讀出n個字節(jié)放指定的緩沖.
// 參數(shù): EE_address:  讀出EEPROM的首地址.
//    DataAddress: 讀出數(shù)據(jù)放緩沖的首地址.
//    number:    讀出的字節(jié)長度.
// 返回: non.
// 版本: V1.0, 2012-10-22
//========================================================================

void EEPROM_read_n(u16 EE_address,u8 *DataAddress,u16 number)
{
F0 = EA;
EA = 0; //禁止中斷
IAP_CONTR = ENABLE_IAP;
IAP_CMD = CMD_READ;
do
{
IAP_ADDRH = EE_address / 256; //送地址高字節(jié)（地址需要改變時才需重新送地址）
IAP_ADDRL = EE_address % 256; //送地址低字節(jié)
IAP_TRIG  = 0x5a; IAP_TRIG  = 0xa5;
_nop_();
*DataAddress = IAP_DATA; //讀出的數(shù)據(jù)送往
EE_address++;
DataAddress++;
}while(--number);

DisableEEPROM();
EA = F0; //重新允許中斷
}

/******************** 扇區(qū)擦除函數(shù) *****************/
//========================================================================
// 函數(shù): void EEPROM_SectorErase(u16 EE_address)
// 描述: 把指定地址的EEPROM扇區(qū)擦除.
// 參數(shù): EE_address:  要擦除的扇區(qū)EEPROM的地址.
// 返回: non.
// 版本: V1.0, 2013-5-10
//========================================================================
void EEPROM_SectorErase(u16 EE_address)
{
F0 = EA;
EA = 0; //禁止中斷
IAP_ADDRH = EE_address / 256; //送扇區(qū)地址高字節(jié)（地址需要改變時才需重新送地址）
IAP_ADDRL = EE_address % 256; //送扇區(qū)地址低字節(jié)
IAP_CONTR = ENABLE_IAP;
IAP_CMD = CMD_ERASE;
IAP_TRIG  = 0x5a; IAP_TRIG  = 0xa5;
_nop_();
DisableEEPROM();
EA = F0; //重新允許中斷
}

//========================================================================
// 函數(shù): void EEPROM_write_n(u16 EE_address,u8 *DataAddress,u16 number)
// 描述: 把緩沖的n個字節(jié)寫入指定首地址的EEPROM.
// 參數(shù): EE_address:  寫入EEPROM的首地址.
//    DataAddress: 寫入源數(shù)據(jù)的緩沖的首地址.
//    number:    寫入的字節(jié)長度.
// 返回: non.
// 版本: V1.0, 2012-10-22
//========================================================================
void EEPROM_write_n(u16 EE_address,u8 *DataAddress,u16 number)
{
F0 = EA;
EA = 0; //禁止中斷
IAP_CONTR = ENABLE_IAP;
IAP_CMD = CMD_PROGRAM;
do
{
IAP_ADDRH = EE_address / 256; //送地址高字節(jié)（地址需要改變時才需重新送地址）
IAP_ADDRL = EE_address % 256; //送地址低字節(jié)
IAP_DATA  = *DataAddress; //送數(shù)據(jù)到IAP_DATA，只有數(shù)據(jù)改變時才需重新送
IAP_TRIG  = 0x5a; IAP_TRIG  = 0xa5;
_nop_();
EE_address++;
DataAddress++;
}while(--number);

DisableEEPROM();
EA = F0; //重新允許中斷
}

void IAP_Gyro(void) //陀螺儀校準
{
u8 i,j;
((int *)&tmp)[0] = g_x; //讀取陀螺儀數(shù)據(jù)
((int *)&tmp)[1] = g_y;
((int *)&tmp)[2] = g_z;
((int *)&tmp)[3] = 0x55aa;

((float *)&tmp)[2] = a_x;
((float *)&tmp)[3] = a_y;

for(i=0,j=0; i<16; i++) j += tmp[i];
j = ~j + 1;
tmp[i] = j;

EEPROM_SectorErase(EE_ADDRESS1); //扇區(qū)擦除
EEPROM_write_n(EE_ADDRESS1, tmp, 17);
}

void IAPRead(void) //讀取陀螺儀靜差
{
u8 i,j;

EEPROM_read_n(EE_ADDRESS1, tmp, 17);
for(i=0,j=0; i<17; i++) j += tmp[i];

if((j == 0) && (((int *)&tmp)[3] == 0x55aa)) //判斷數(shù)據(jù)是否已經(jīng)更新過，更新過才讀取數(shù)據(jù)
{
g_x = ((int *)&tmp)[0];
g_y = ((int *)&tmp)[1];
g_z = ((int *)&tmp)[2];
a_x = ((float *)&tmp)[2];
a_y = ((float *)&tmp)[3];
}
else
{
g_x = 0;
g_y = 0;
g_z = 0;
a_x = 0;
a_y = 0;
}

}

帳號		自動登錄	找回密碼
密碼			立即注冊