自己写的mma7260加速度计侧倾角程序不是很准，没有高深的算法，望高手指点 (amobbs.com 阿莫电子技术论坛) -

liguole 发表于 2011-5-26 12:04:53

自己写的mma7260加速度计侧倾角程序不是很准，没有高深的算法，望高手指点

/////////////////////////////////////
//Generated Initialization File//
/////////////////////////////////////

/*                            RC_PPM波形示意图
   <----------------------------------------------------------------------->
      ____ ______ _____ ________             ______ sync gap    ____
   | | |    | | | |    |          |    |             |
   | | |    | | | |    |          |    |             |
___| |_|    |_| |_|    |_.............|    |________________|
   <-----><-------><------><-------->          <------>             <---
      t0    t1    t2    t4                   tn                   t0

The PPM-Frame length is 22.5 ms.
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
The minimum duration of all channels at minimum value is8 * 1 ms = 8 ms.
The maximum duration of all channels at maximum value is8 * 2 ms = 16 ms.
The remaining time of (22.5 - 8 ms) ms = 14.5 msto (22.5 - 16 ms) ms = 6.5 ms is
the syncronization gap.

这个PPM波形长度为22.5ms，各通道的高电平宽度为0.7ms~1.7ms，低电平宽度为固定的0.3ms。
经解码后各通道的最小脉宽为0.7+0.3=1ms，最大脉宽为1.7+0.3=2ms。
所有通道的最小脉宽加在一起是：8 * 1 ms = 8 ms.
所有通道的最大脉宽加在一起是：8 * 2 ms = 16 ms.
所以剩下的时间(同步时间)是：(22.5 - 8 ms) ms = 14.5 ms~ (22.5 - 16 ms) ms = 6.5 ms
*/

#include "C8051F340.h"

#include "string.h"
#include "intrins.h"
#include "stdio.h"
//#include "ubx.h"

sfr16 TMR2RL = 0xCA; // Timer2 reload value 地址

sfr16 TMR2 = 0xCC; // Timer2 counter地址

sfr16 ADC0 = 0xBD; // 10-bit ADC0 result地址

#define SYSCLK 11059200 // SYSCLK frequency in Hz

#define BAUDRATE 9600 // Baud rate of UART in bps

#define MaxCTNO 8

#define ANALOG_INPUTS 6 // Number of AIN pins to measure,

unsigned char TXD,RXD;

int X_Angle = 0; // X轴与水平面夹角

int Y_Angle = 0; // Y轴与水平面夹角

int Vx = 0; // x 当前电压

int Vy = 0; // y 当前电压

unsigned int X0 = 1650; // x 零度电压

unsigned int Y0 = 1320; // y 零度电压

unsigned int Time0Cnt = 0; // Counter

long RESULT; // 结果大批ADC0,每一个值 ADC0 decimated value, one for each

//unsigned char PIN_TABLE = {0,1,2,3,5};
//48pin f340 P2.4不能作为模拟输入
unsigned char PIN_MUX_TABLE = {0x02,0x03,0x04,0x06,0x07,0x1E};
//p2.2,p2.3,p2.5,p3.0,p3.1,Temp;
//0 ,1 ,2 ,3 ,4 ,5 ;
//MX,MY,MZ,EX,EY,温度
char idata UAV_1 =
{
0x0A ,0xFF ,0x14 ,0x00 ,0x46 ,
0x00 ,0x03 ,0x0E ,0x01 ,0x06 ,
0x0B ,0x01 ,0x4E ,0x45 ,0x48 ,
0x4D ,0xB9 ,0x17 ,0x31 ,0x9F ,
0x00 ,0x00 ,0x00 ,0x00 ,0x00 ,
0x00 ,0x1C ,0xDA ,0x21 ,0x00 ,
0xFD ,0x7A ,0xFF ,0xFF ,0x04 ,
0x80 ,0x01 ,0x04 ,0x19 ,0x00 ,
0x2C ,0x00 ,0x00 ,0x08 ,0x6C ,
0x04 ,0xF1 ,0x9A ,0x00 ,0x00 ,
0x00 ,0x00 ,0x27 ,0x55 ,0xA0 ,
0x03 ,0x2E ,0x02 ,0x00 ,0xF4 ,
0x00 ,0x0C ,0x1C ,0x8F ,0x11 ,
0x57 ,0xE0 ,0x57 ,0xAC ,0x70 ,
0x6A ,0x57 ,0x79 ,0x03 ,0x25 ,
0x00
};

// Peripheral specific initialization functions,
// Called from the Init_Device() function

void Delay (unsigned int count)//24.5M下延时1ms
{
unsigned int i,j;
for(i=0;i<count;i++)
{
for(j=0;j<1361;j++)
{
;
}
}
}

void Reset_Sources_Init()
{
/* //以下代码将VDD监视器设置为复位源-----------防止程序丢失得重要措施
int i = 0;
VDM0CN = 0x80; //使能VDD监视器
for (i = 0; i < 350; i++); // Wait 100us for initialization
RSTSRC |= 0x02; //设置VDD监视器为复位源,代码中所有的对rstsrc的写操作均用直接赋值方式完成（如rstsrc = 0x02

*/
RSTSRC |= 0x04; // 使能时钟丢失检测器
}

void PCA_Init()
{
PCA0MD &= ~0x40;
PCA0MD = 0x00;
}

void Timer_Init()
{
TCON    = 0x40;
TMOD    = 0x20;

/*******************************************************************************/

// TH1    = 0x40; // 2400 bps
// TH1    = 0xA0; // 4800 bps

/*******************************************************************************/

// CKCON = 0x01; // 9600\19200 bps
// TH1    = 0x70; // 9600 bps
// TH1    = 0xB8; // 19200 bps

/*******************************************************************************/

// CKCON = 0x08; // 38400~115200 bps
// TH1    = 0x70; // 38400 bps
// TH1    = 0xA0; // 57600 bps
// TH1    = 0xD0; // 115200 bps

/*******************************************************************************/

if(BAUDRATE == 2400)
{
TH1    = 0x40; // 2400 bps
}
else
{
if(BAUDRATE == 4800)
{
TH1    = 0xA0; // 4800 bps
}
else
{
if(BAUDRATE == 9600)
{
CKCON |= 0x01; // 9600\19200 bps
TH1    = 0x70; // 9600 bps
}
else
{
if(BAUDRATE == 19200)
{
CKCON |= 0x01; // 9600\19200 bps
TH1    = 0xB8; // 19200 bps
}
else
{
if(BAUDRATE == 38400)
{
CKCON |= 0x08; // 38400~115200 bps
TH1    = 0x70; // 38400 bps
}
else
{
if(BAUDRATE == 57600)
{
CKCON |= 0x08; // 38400~115200 bps
TH1    = 0xA0; // 57600 bps
}
else
{
if(BAUDRATE == 115200)
{
CKCON |= 0x08; // 38400~115200 bps
TH1    = 0xD0; // 115200 bps
}
else
{
TH1    = 0x40; // 2400 bps
// break; // 可以在这里添加一个报错的语句！！！
}
}
}
}
}
}
}

TMR2CN = 0x04;
CKCON |= 0x10;

}

void UART_Init()
{
SCON0 = 0x10;
TI0    = 1;
}

void ADC_Init()
{
AMX0P    = PIN_MUX_TABLE; // ADC0 initial positive input = P2.0
AMX0N = 0x1F;
ADC0CN = 0x83;
AD0EN    = 1; // Enable ADC0
}

void Voltage_Reference_Init()
{
REF0CN = 0x03;
}

void Port_IO_Init()
{
// P0.0-Skipped, Open-Drain, Digital
// P0.1-CEX0(PCA), Open-Drain, Digital
// P0.2-CEX1(PCA), Open-Drain, Digital
// P0.3-CEX2(PCA), Open-Drain, Digital
// P0.4-TX0 (UART0), Push-Pull,Digital
// P0.5-RX0 (UART0), Open-Drain, Digital
// P0.6-Skipped, Open-Drain, Analog
// P0.7-Skipped, Open-Drain, Analog

// P1.0-Skipped, Open-Drain, Digital
// P1.1-Skipped, Open-Drain, Digital
// P1.2-TX1 (UART1), Push-Pull,Digital
// P1.3-RX1 (UART1), Open-Drain, Digital
// P1.4-Unassigned,Open-Drain, Digital
// P1.5-Skipped, Push-Pull,Analog
// P1.6-Unassigned,Open-Drain, Digital
// P1.7-Unassigned,Open-Drain, Digital

// P2.0-Skipped, Open-Drain, Analog
// P2.1-Skipped, Open-Drain, Analog
// P2.2-Skipped, Open-Drain, Analog
// P2.3-Skipped, Open-Drain, Analog
// P2.4-Unassigned,Open-Drain, Digital
// P2.5-Skipped, Open-Drain, Analog
// P2.6-Skipped, Open-Drain, Analog
// P2.7-Skipped, Open-Drain, Analog

// P3.0-Unassigned,Open-Drain, Digital
// P3.1-Unassigned,Open-Drain, Digital
// P3.2-Unassigned,Open-Drain, Digital
// P3.3-Unassigned,Open-Drain, Digital
// P3.4-Unassigned,Open-Drain, Digital
// P3.5-Unassigned,Open-Drain, Digital
// P3.6-Unassigned,Open-Drain, Digital
// P3.7-Unassigned,Open-Drain, Digital

P0MDIN = 0x3F;
P1MDIN = 0xDF;
P2MDIN = 0x10;
P0MDOUT = 0x10;
P1MDOUT = 0x24;
P0SKIP = 0xC1;
P1SKIP = 0x23;
P2SKIP = 0xEF;
XBR0    = 0x01;
XBR1    = 0x43;
XBR2    = 0x01;
}

void Oscillator_Init()
{
int i = 0;
OSCXCN = 0x67;
for (i = 0; i < 3000; i++);// Wait 1ms for initialization
while ((OSCXCN & 0x80) == 0);
for (i = 0; i < 3000; i++);// Wait 1ms for initialization
OSCICN = 0x83;
CLKMUL = 0x02;
CLKSEL = 0x01;
}

void Interrupts_Init()
{
EIE1    = 0x08;
IE    = 0x90;
}

void Time0_Out(void) interrupt 1    //定时器0溢出中断
{
if(++Time0Cnt > 10000)
Time0Cnt = 0;
}

void UATR0_ISR(void)interrupt 4
{
//Rx、Tx共用中断
//接收中断
if(!TI0)
{
   RI0=0 ;
   RXD=SBUF0 ;

X0=Vx;
Y0=Vy;
}
//发送中断
else TI0=0 ;
}

void Timer2_ISR (void) interrupt 5
{
TF2H = 0; // Clear Timer2 interrupt flag
}

void ADC0_ISR (void) interrupt 10
{

static unsigned char i = 0;

if(i < ANALOG_INPUTS)
{
AMX0P = PIN_MUX_TABLE;
RESULT = ADC0;
i++;
}
else
{
i = 0;
}

AD0INT = 0; // 清除ADC0转换完成标志 Clear ADC0 conv. complete flag
}

void Uart0_Transmit_8bit(unsigned char TXD)
{
EA = 0 ; //关总中断,防止发送过程中有中断插入

SBUF0 = TXD;
while(TI0==0); //TI0=0 ;等待串口发送不忙以后在发送数据

EA = 1 ; //发送结束，开总中断

Delay(2); // 延时一点时间为的是让他有个反映的时间
}

void UAV_TX ()
{
unsigned char i;

unsigned int TADD;

for (i=0;i<76;i++)
{
if(i > 1)
{
TADD += UAV_1;
if(i==74)
{
UAV_1 = TADD%256;
}
}

         //校验和发送
Uart0_Transmit_8bit(UAV_1);
if(i==75)
{
TADD = 0;
}

}
}

// Initialization function for device,
// Call Init_Device() from your main program
void Init_Device(void)
{
Reset_Sources_Init();
PCA_Init();
Timer_Init();
UART_Init();
ADC_Init();
Voltage_Reference_Init();
Port_IO_Init();
Oscillator_Init();
Interrupts_Init();
}

//主函数
void main()
{
// P2.2 = MX;
// P2.3 = MY;
// P2.5 = MZ;
// P3.0 = EX;
// P3.1 = EY;
// P3.2 = EZ;
Init_Device();

while(1)
{
// AD0BUSY = 1;

Vx = RESULT * 2430 / 1023;
Vy = RESULT * 2430 / 1023;

X_Angle = (Vx-X0)/90;
Y_Angle = (Vy-Y0);

// X_Angle = -697;//负值转换错误,已解决
// Y_Angle = -900;

if(X_Angle >= 0)
{
UAV_1 = X_Angle / 256;
UAV_1 = X_Angle % 256;
}

else
{
UAV_1 = (X_Angle / 256)-1;
UAV_1 = X_Angle % 256;
}

if(Y_Angle >= 0)
{
UAV_1 = Y_Angle / 256;
UAV_1 = Y_Angle % 256;
}

else
{
UAV_1 = (Y_Angle / 256)-1;
UAV_1 = Y_Angle % 256;
}

// RI0=1;

UAV_TX ();

Delay (50);

}
}

Clinging 发表于 2011-5-26 16:20:12

http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fsrch=1&sr=1

第一次看到这个是在 "g921002" 的回帖里，原帖没找到

loongsuns 发表于 2011-5-26 16:25:35

怎么个不准？

g921002 发表于 2011-5-27 01:21:24

ADC解析度不足吧?
還有用在飛機上，外力這樣多，當然不準。

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

自己写的mma7260加速度计侧倾角程序 不是很准，没有高深的算法，望高手指点

自己写的mma7260加速度计侧倾角程序不是很准，没有高深的算法，望高手指点