最近在写linux 下的定时器编程实验,测试发现 usleep函数在 x86 架构下的定时还是比较准确的,在arm9下 就不太准了.
今天用linux 下的setitimer()函数进行了定时 器的测试,代码如下:
#include <stdio.h>
#include <time.h>
#include <sys/time.h>
#include <stdlib.h>
#include <signal.h>
#include <math.h>
#define pi 3.1415926 /*四元数的元素，代表估计方向 */
float q0 = , q1 = , q2 = , q3 = ;
float q0_inc,q1_inc,q2_inc,q3_inc;
/*用于对四元进行更新,角增量,不是真实的欧拉角*/
float Roll_inc,Pitch_inc,Yaw_inc;
float Roll,Pitch,Yaw; /*真实欧拉角*/ void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add) ;
void ToEulerAngle();
void Quaternion_nor();
void Multiply(float q0_n,float q1_n,float q2_n,float q3_n); char flag=;
int count = ;
void set_timer()
{
struct itimerval itv, oldtv;
itv.it_interval.tv_sec = ;
itv.it_interval.tv_usec =;
itv.it_value.tv_sec = ;
itv.it_value.tv_usec = ; setitimer(ITIMER_REAL, &itv, &oldtv);
} void sigalrm_handler(int sig)
{
flag=; //printf("timer signal.. %d\n", count);
} int main()
{ float time_use=;
struct timeval start;
struct timeval end; signal(SIGALRM, sigalrm_handler);
set_timer();
while (count < )
{
if(flag)
{
Roll_inc=0.01;
Pitch_inc=0.01;
Yaw_inc=0.01; gettimeofday(&start,NULL); FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ;
/*更新四元 */
Multiply(q0_inc,q1_inc,q2_inc,q3_inc);
ToEulerAngle(); gettimeofday(&end,NULL);
time_use+=(end.tv_sec-start.tv_sec)*+(end.tv_usec-start.tv_usec);//微秒 printf("The count is %i\n",count);
printf("yaw=%f\n",Yaw*57.3);
printf("pitch=%f\n",Pitch*57.3);
printf("roll=%f\n",Roll*57.3);
flag=;
count++;
} }
printf("time_use is %f\n",time_use); exit(); } /*欧拉角转四元,其它坐标系 */
/*这里是否采用小角近似???*/ void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add)/*这里只是机体转角近似成欧拉*/
{ /*在其他人的解算中,用的是小角近似 q=[1,Ω*t/2,Ω*t/2,Ω*t/2]T*/ float fCosHRoll = (float)cos(Roll_add * .5f);
float fSinHRoll = (float)sin(Roll_add * .5f);
float fCosHPitch = (float)cos(Pitch_add * .5f);
float fSinHPitch = (float)sin(Pitch_add * .5f);
float fCosHYaw = (float)cos(Yaw_add * .5f);
float fSinHYaw = (float)sin(Yaw_add * .5f); /*回来看看这三角函数运算用了多长时间*/
/*下面这个运算要根据坐标第进行修改*/
q0_inc = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
q1_inc = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
q2_inc = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
q3_inc = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw; }
/*四元数转欧拉角*/
void ToEulerAngle()
{
Roll=atan2( * (q0 * q1 + q2 * q3) , - * (q1 * q1 + q2 * q2));
Yaw = atan2( * (q0 * q3 + q1 * q2) , - * (q2 * q2 + q3 * q3));
Pitch = asin(*(q0*q2-q3*q1)) ;
} /*更新四元数*/
/* q_n 这里代表新来的四元数,这里指的是增量 */
void Multiply(float q0_n,float q1_n,float q2_n,float q3_n)
{
float q0_temp=q0*q0_n -q1*q1_n -q2*q2_n -q3*q3_n;
float q1_temp=q0*q1_n +q1*q0_n +q2*q3_n -q3*q2_n;
float q2_temp=q0*q2_n -q1*q3_n +q2*q0_n +q3*q1_n;
float q3_temp=q0*q3_n +q1*q2_n -q2*q1_n +q3*q0_n;
q0=q0_temp;
q1=q1_temp;
q2=q2_temp;
q3=q3_temp;
float s=sqrt(q0*q0+q1*q1+q2*q2+q3*q3); //这里重新进行规范化,避免的累积误差
q0=q0/s;
q1=q1/s;
q2=q2/s;
q3=q3/s; }
代码简介,我这里用的之前写的姿态解算的代码.这里进行100HZ的定时 ,在PC 上测试 10s ,运行结果如下图
其中姿态解算部分占用 3042us ,也就是说每次解算用时 3.042us .
我在am9平台下测试,结果如下:
在freescale cortex-a9 双核 测试结果
全志 cortex-a7 双核 测试结果
这里测试结果差别还是比较大的,姿态解算用了 188213us, 平均为 0.188ms ,相对与 10ms 的解算周期,占用还是比较小的.
补充一点,在arm9下linux 到200HZ还是可以的,但是就不太准了.这里我也同时测试了usleep函数和利用select()这个系统调用,延时都不理想.
下面介紹一下是利用RTC进行定时，下面的程序来自这里，http://www.linuxidc.com/Linux/2007-01/1821p2.htm
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h> #include <linux/rtc.h>
#include <sys/ioctl.h>
int main(int argc, char* argv[])
{
unsigned long i = ;
unsigned long data = ;
int retval = ;
int fd = open ("/dev/rtc", O_RDONLY);
if(fd < )
{
perror("open");
exit(errno);
}
/*Set the freq as 4Hz*/
if(ioctl(fd, RTC_IRQP_SET, ) < )
{
perror("ioctl(RTC_IRQP_SET)");
close(fd);
exit(errno);
}
/* Enable periodic interrupts */
if(ioctl(fd, RTC_PIE_ON, ) < )
{
perror("ioctl(RTC_PIE_ON)");
close(fd);
exit(errno);
}
for(i = ; i < ; i++)
{ /*Blocking read*/
if(read(fd, &data, sizeof(unsigned long)) < )
{
perror("read");
close(fd);
exit(errno);
}
printf("timer\n");
}
/* Disable periodic interrupts */
ioctl(fd, RTC_PIE_OFF, );
close(fd);
return ;
}
程序说明：代码第 15行处的open()函数，非root下会被拒绝访问。RTC定时有一定的局限性，频率只能为2幂。
2013.7.28,进行 posix timer 接口的编程测试,测试代码如下:
#include <stdio.h>
#include <time.h>
#include <signal.h>
#include <pthread.h>
#include <string.h>
#include <unistd.h>
char flag;
void handle(union sigval v)
{
flag=;
return;
} int create (int ms,int id)
{
timer_t tid;
struct sigevent se;
struct itimerspec ts,ots;
memset (&se,,sizeof(se));
se.sigev_notify = SIGEV_THREAD;
se.sigev_notify_function = handle;
se.sigev_value.sival_int = id; //作为handle()的参数
if(timer_create(CLOCK_REALTIME,&se,&tid)<) //create the timer
{
perror("timer_creat");
return -;
}
puts("timer_create successfully.");
ts.it_value.tv_sec = ;
ts.it_value.tv_nsec =**ms ;
ts.it_interval.tv_sec = ;
ts.it_interval.tv_nsec = **ms;
if(timer_settime (tid,TIMER_ABSTIME,&ts,&ots) < ) //start/stop the timer
{
perror("timer_settime");
return -;
}
return ;
} int main(void)
{
//create(3,1);
int num=;
create(,);
while(num<)
{
if(flag)
{
flag=;
printf("the num is %i\n",num);
num++;
} }
printf("2013.7.28.11\n");
return ;
}
代码参考百度空间,在PC 环境 下运行的时间为 10s,在arm linux下定时 200hz ,实际运行的时间为 20S,所以是极为不准确的.这期间我现时进行了 gettimeofday()这个方法的测试,结果都不理想.总结这面这些代码 ,要想进行 ms 级的精确定时 ,只有到驱动层面进行编写相关程序.
博文为本人所写,转载请表明出处:博客园:梦工厂2012.