最近在写linux 下的定时器编程实验,测试发现 usleep函数在 x86 架构下的定时还是比较准确的,在arm9下 就不太准了.
今天用linux 下的setitimer()函数进行了定时 器的测试,代码如下:
1 #include <stdio.h> 2 #include <time.h> 3 #include <sys/time.h> 4 #include <stdlib.h> 5 #include <signal.h> 6 #include <math.h> 7 #define pi 3.1415926 8 9 /*四元数的元素，代表估计方向 */ 10 float q0 = 1, q1 = 0, q2 = 0, q3 = 0; 11 float q0_inc,q1_inc,q2_inc,q3_inc; 12 /*用于对四元进行更新,角增量,不是真实的欧拉角*/ 13 float Roll_inc,Pitch_inc,Yaw_inc; 14 float Roll,Pitch,Yaw; /*真实欧拉角*/ 15 16 void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add) ; 17 void ToEulerAngle(); 18 void Quaternion_nor(); 19 void Multiply(float q0_n,float q1_n,float q2_n,float q3_n); 20 21 char flag=0; 22 int count = 0; 23 void set_timer() 24 { 25 struct itimerval itv, oldtv; 26 itv.it_interval.tv_sec = 0; 27 itv.it_interval.tv_usec =10000; 28 itv.it_value.tv_sec = 0; 29 itv.it_value.tv_usec = 10000; 30 31 setitimer(ITIMER_REAL, &itv, &oldtv); 32 } 33 34 void sigalrm_handler(int sig) 35 { 36 flag=1; 37 38 //printf("timer signal.. %d ", count); 39 } 40 41 int main() 42 { 43 44 float time_use=0; 45 struct timeval start; 46 struct timeval end; 47 48 signal(SIGALRM, sigalrm_handler); 49 set_timer(); 50 while (count < 1000) 51 { 52 if(flag) 53 { 54 Roll_inc=0.01; 55 Pitch_inc=0.01; 56 Yaw_inc=0.01; 57 58 gettimeofday(&start,NULL); 59 60 FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ; 61 /*更新四元 */ 62 Multiply(q0_inc,q1_inc,q2_inc,q3_inc); 63 ToEulerAngle(); 64 65 gettimeofday(&end,NULL); 66 time_use+=(end.tv_sec-start.tv_sec)*1000000+(end.tv_usec-start.tv_usec);//微秒 67 68 printf("The count is %i ",count); 69 printf("yaw=%f ",Yaw*57.3); 70 printf("pitch=%f ",Pitch*57.3); 71 printf("roll=%f ",Roll*57.3); 72 flag=0; 73 count++; 74 } 75 76 } 77 printf("time_use is %f ",time_use); 78 79 exit(0); 80 81 } 82 83 /*欧拉角转四元,其它坐标系 */ 84 /*这里是否采用小角近似???*/ 85 86 void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add)/*这里只是机体转角近似成欧拉*/ 87 { 88 89 /*在其他人的解算中,用的是小角近似 q=[1,Ω*t/2,Ω*t/2,Ω*t/2]T*/ 90 91 float fCosHRoll = (float)cos(Roll_add * .5f); 92 float fSinHRoll = (float)sin(Roll_add * .5f); 93 float fCosHPitch = (float)cos(Pitch_add * .5f); 94 float fSinHPitch = (float)sin(Pitch_add * .5f); 95 float fCosHYaw = (float)cos(Yaw_add * .5f); 96 float fSinHYaw = (float)sin(Yaw_add * .5f); /*回来看看这三角函数运算用了多长时间*/ 97 /*下面这个运算要根据坐标第进行修改*/ 98 q0_inc = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw; 99 q1_inc = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw; 100 q2_inc = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw; 101 q3_inc = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw; 102 103 } 104 /*四元数转欧拉角*/ 105 void ToEulerAngle() 106 { 107 Roll=atan2(2 * (q0 * q1 + q2 * q3) , 1 - 2 * (q1 * q1 + q2 * q2)); 108 Yaw = atan2(2 * (q0 * q3 + q1 * q2) , 1 - 2 * (q2 * q2 + q3 * q3)); 109 Pitch = asin(2*(q0*q2-q3*q1)) ; 110 } 111 112 /*更新四元数*/ 113 /* q_n 这里代表新来的四元数,这里指的是增量 */ 114 void Multiply(float q0_n,float q1_n,float q2_n,float q3_n) 115 { 116 float q0_temp=q0*q0_n -q1*q1_n -q2*q2_n -q3*q3_n; 117 float q1_temp=q0*q1_n +q1*q0_n +q2*q3_n -q3*q2_n; 118 float q2_temp=q0*q2_n -q1*q3_n +q2*q0_n +q3*q1_n; 119 float q3_temp=q0*q3_n +q1*q2_n -q2*q1_n +q3*q0_n; 120 q0=q0_temp; 121 q1=q1_temp; 122 q2=q2_temp; 123 q3=q3_temp; 124 float s=sqrt(q0*q0+q1*q1+q2*q2+q3*q3); //这里重新进行规范化,避免的累积误差 125 q0=q0/s; 126 q1=q1/s; 127 q2=q2/s; 128 q3=q3/s; 129 130 }
代码简介,我这里用的之前写的姿态解算的代码.这里进行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
1 #include <stdio.h> 2 #include <stdlib.h> 3 #include <string.h> 4 #include <unistd.h> 5 #include <errno.h> 6 #include <fcntl.h> 7 8 #include <linux/rtc.h> 9 #include <sys/ioctl.h> 10 int main(int argc, char* argv[]) 11 { 12 unsigned long i = 0; 13 unsigned long data = 0; 14 int retval = 0; 15 int fd = open ("/dev/rtc", O_RDONLY); 16 if(fd < 0) 17 { 18 perror("open"); 19 exit(errno); 20 } 21 /*Set the freq as 4Hz*/ 29 if(ioctl(fd, RTC_IRQP_SET, 4) < 0) 30 { 31 perror("ioctl(RTC_IRQP_SET)"); 32 close(fd); 33 exit(errno); 34 } 35 /* Enable periodic interrupts */ 36 if(ioctl(fd, RTC_PIE_ON, 0) < 0) 37 { 38 perror("ioctl(RTC_PIE_ON)"); 39 close(fd); 40 exit(errno); 41 } 42 for(i = 0; i < 100; i++) 43 { /*Blocking read*/ 44 if(read(fd, &data, sizeof(unsigned long)) < 0) 45 { 46 perror("read"); 47 close(fd); 48 exit(errno); 49 } 50 printf("timer "); 51 } 52 /* Disable periodic interrupts */ 53 ioctl(fd, RTC_PIE_OFF, 0); 54 close(fd); 55 return 0; 56 }
程序说明：代码第 15行处的open()函数，非root下会被拒绝访问。RTC定时有一定的局限性，频率只能为2幂。
2013.7.28,进行 posix timer 接口的编程测试,测试代码如下:
1 #include <stdio.h> 2 #include <time.h> 3 #include <signal.h> 4 #include <pthread.h> 5 #include <string.h> 6 #include <unistd.h> 7 char flag; 8 void handle(union sigval v) 9 { 10 flag=1; 11 return; 12 } 13 14 int create (int ms,int id) 15 { 16 timer_t tid; 17 struct sigevent se; 18 struct itimerspec ts,ots; 19 memset (&se,0,sizeof(se)); 20 se.sigev_notify = SIGEV_THREAD; 21 se.sigev_notify_function = handle; 22 se.sigev_value.sival_int = id; //作为handle()的参数 23 if(timer_create(CLOCK_REALTIME,&se,&tid)<0) //create the timer 24 { 25 perror("timer_creat"); 26 return -1; 27 } 28 puts("timer_create successfully."); 29 ts.it_value.tv_sec = 0; 30 ts.it_value.tv_nsec =1000*1000*ms ; 31 ts.it_interval.tv_sec = 0; 32 ts.it_interval.tv_nsec = 1000*1000*ms; 33 if(timer_settime (tid,TIMER_ABSTIME,&ts,&ots) < 0) //start/stop the timer 34 { 35 perror("timer_settime"); 36 return -1; 37 } 38 return 0; 39 } 40 41 int main(void) 42 { 43 //create(3,1); 44 int num=0; 45 create(5,2); 46 while(num<2000) 47 { 48 if(flag) 49 { 50 flag=0; 51 printf("the num is %i ",num); 52 num++; 53 } 54 55 } 56 printf("2013.7.28.11 "); 57 return 0; 58 }
代码参考百度空间,在PC 环境 下运行的时间为 10s,在arm linux下定时 200hz ,实际运行的时间为 20S,所以是极为不准确的.这期间我现时进行了 gettimeofday()这个方法的测试,结果都不理想.总结这面这些代码 ,要想进行 ms 级的精确定时 ,只有到驱动层面进行编写相关程序.
博文为本人所写,转载请表明出处:博客园:梦工厂2012.