STM32算法

1.通过编码器对返回的错误速度进行滤波 #define MOTOR_BUFF_CIRCLE_SIZE 4 #define STATIC_ENCODER_VALUE 6 int32_t LMotor_Encoder_buff[MOTOR_BUFF_CIRCLE_SIZE] = {0}; uint8_t LEindex = 0; int32_t LMotor_Encoder_last = 0; int32_t L_Encoder_change = 0; int32_t RMotor_Encoder_buff[MOTOR_BUFF_CIRCLE_SIZE] = {0}; uint8_t REindex = 0; int32_t RMotor_Encoder_last = 0; int32_t R_Encoder_change = 0; uint8_t Robot_static_flag = 0; uint8_t Robot_dynamic_flag = 0; int32_t Encoder_buff_change_value(int32_t *buff,uint8_t size) { int i = 0; int32_t value = 0; for(i = 0; i < size;i++) { value += buff[i]; } return value; } u8 Speed_Filter_send(int32_t L_speed,int32_t R_speed) { CAN1Sedbuf[0]=L_speed; CAN1Sedbuf[1]=L_speed>>8; CAN1Sedbuf[2]=L_speed>>16; CAN1Sedbuf[3]=L_speed>>24; CAN1Sedbuf[4]=R_speed; CAN1Sedbuf[5]=R_speed>>8; CAN1Sedbuf[6]=R_speed>>16; CAN1Sedbuf[7]=R_speed>>24; CAN1_Send(0x183,8); Delay_Us(200); } void Updata_Motor_Speed(void) { // if((Motor_Vel_receive[0] < 10) && (Motor_Vel_receive[0] > -10)) // { // Motor_Vel_receive[0] = 0; // } // if((Motor_Vel_receive[1] < 10) && (Motor_Vel_receive[1] > -10)) // { // Motor_Vel_receive[1] = 0; // } LEindex = LEindex % MOTOR_BUFF_CIRCLE_SIZE; LMotor_Encoder_buff[LEindex] = Motor_Encoder_receive[0] - LMotor_Encoder_last; LEindex++; LMotor_Encoder_last = Motor_Encoder_receive[0]; L_Encoder_change = Encoder_buff_change_value(LMotor_Encoder_buff,MOTOR_BUFF_CIRCLE_SIZE); REindex = REindex % MOTOR_BUFF_CIRCLE_SIZE; RMotor_Encoder_buff[REindex] = Motor_Encoder_receive[1] - RMotor_Encoder_last; REindex++; RMotor_Encoder_last = Motor_Encoder_receive[1]; R_Encoder_change = Encoder_buff_change_value(RMotor_Encoder_buff,MOTOR_BUFF_CIRCLE_SIZE); if( (abs(L_Encoder_change) < STATIC_ENCODER_VALUE) && (abs(R_Encoder_change) < STATIC_ENCODER_VALUE) ) { //static Robot_static_flag = 1; Robot_dynamic_flag = 0; } else { //dynamic Robot_static_flag = 0; Robot_dynamic_flag = 1; } if( (Robot_static_flag == 1) && (Robot_dynamic_flag == 0) ) { Motor_Speed[0] = 0; Motor_Speed[1] = 0; Motor_Odom = 0; Motor_gyro_z = 0; Speed_Filter_send(0,0); } else { Motor_Speed[0] = rpm2vel(Motor_Vel_receive[0]); Motor_Speed[1] = -rpm2vel(Motor_Vel_receive[1]); Motor_Odom = (Motor_Speed[0] + Motor_Speed[1])/2.0f; Motor_gyro_z = ((Motor_Speed[1] - Motor_Speed[0])/WHRRL_L); Speed_Filter_send(Motor_Vel_receive[0],Motor_Vel_receive[1]); } Motor_L_Encoder = Encoder2Distance(Motor_Encoder_receive[0]); Motor_R_Encoder = -Encoder2Distance(Motor_Encoder_receive[1]); } 2.中位值平均滤波 /***note input:Speed_input_value; outout:Speed_output_value;**/ #define FILTER_BUFFER_SIZE 4 uint8 speed_filter[FILTER_BUFFER_SIZE] ={0}; void TMSpeed_filter(void) { static unit8 ad_save_location=0; speed_filter[ad_save_location] = Speed_input_value; /*sample value get input value*/ if (ad_save_location > (FILTER_BUFFER_SIZE-1)) { ad_save_location = 0; TM_filter(); //中位值平均滤波 } else { ad_save_location++; } } /*************************************************************************** * Function: void compositor(u8 channel) * Description: 中位值平均滤波 * Parameters: None * Returns: * Author: Teana **************************************************************************/ void compositor(void) { unit8 exchange; unit8 i,j; u16 change_value; for (j=FILTER_BUFFER_SIZE;j>1;j--) { for (i=0;i<j-1;i++) { exchange = 0; if (speed_filter[i]>speed_filter[i+1]) { change_value = speed_filter[i]; speed_filter[i] = speed_filter[i+1]; speed_filter[i+1] = change_value; exchange = 1; } } if (exchange == 0) return; } } void TM_filter(void) { unit8 index; unit8 count; u16 sum_data = 0; compositor(); //filter up-down for (count=1;count<FILTER_BUFFER_SIZE-1;count++) { sum_data +=speed_filter[count]; } Speed_output_value= sum_data / (FILTER_BUFFER_SIZE - 2); sum_data = 0; } 3.PID算法

pid.h

// protection against multiple includes #ifndef SAXBOPHONE_PID_H #define SAXBOPHONE_PID_H #ifdef __cplusplus extern "C"{ #endif typedef struct pid_calibration { /* * struct PID_Calibration * * Struct storing calibrated PID constants for a PID Controller * These are used for tuning the algorithm and the values they take are * dependent upon the application - (in other words, YMMV...) */ double kp; // Proportional gain double ki; // Integral gain double kd; // Derivative gain } PID_Calibration; typedef struct pid_state { /* * struct PID_State * * Struct storing the current state of a PID Controller. * This is used as the input value to the PID algorithm function, which also * returns a PID_State struct reflecting the adjustments suggested by the algorithm. * * NOTE: The output field in this struct is set by the PID algorithm function, and * is ignored in the actual calculations. */ double actual; // The actual reading as measured double target; // The desired reading double time_delta; // Time since last sample/calculation - should be set when updating state // The previously calculated error between actual and target (zero initially) double previous_error; double integral; // Sum of integral error over time double output; // the modified output value calculated by the algorithm, to compensate for error } PID_State; /* * PID Controller Algorithm implementation * * Given a PID calibration for the P, I and D values and a PID_State for the current * state of the PID controller, calculate the new state for the PID Controller and set * the output state to compensate for any error as defined by the algorithm */ PID_State pid_iterate(PID_Calibration calibration, PID_State state); #ifdef __cplusplus } // extern "C" #endif // end of header #endif

pid.c

#include "pid.h" PID_State pid_iterate(PID_Calibration calibration, PID_State state) { // calculate difference between desired and actual values (the error) double error = state.target - state.actual; // calculate and update integral state.integral += (error * state.time_delta); // calculate derivative double derivative = (error - state.previous_error) / state.time_delta; // calculate output value according to algorithm state.output = ( (calibration.kp * error) + (calibration.ki * state.integral) + (calibration.kd * derivative) ); // update state.previous_error to the error value calculated on this iteration state.previous_error = error; // return the state struct reflecting the calculations return state; }

test.c

#include "pid.h" // initialise blank PID_Calibration struct and blank PID_State struct PID_Calibration calibration; PID_State state; void setup() { // configure the calibration and state structs // dummy gain values calibration.kp = 1.0; calibration.ki = 1.0; calibration.kd = 1.0; // an initial blank starting state state.actual = 0.0; state.target = 0.0; state.time_delta = 1.0; // assume an arbitrary time interval of 1.0 state.previous_error = 0.0; state.integral = 0.0; // start the serial line at 9600 baud! Serial.begin(9600); } void loop() { // read in two bytes from serial, assume first is the target value and // second is the actual value. Output calculated result on serial if (Serial.available() >= 2) { // retrieve one byte as target value, cast to double and store in state struct state.target = (double) Serial.read(); // same as above for actual value state.actual = (double) Serial.read(); // now do PID calculation and assign output back to state state = pid_iterate(calibration, state); // print results back on serial Serial.print("Target:\t"); Serial.println(state.target); Serial.print("Actual:\t"); Serial.println(state.actual); Serial.print("Output:\t"); Serial.println(state.output); } } 4.STM32 滤波算法 #include "led.h" #include "delay.h" #include "sys.h" #include "usart.h" #include "lcd.h" #include "adc.h" #include "usartbo.h" u16 ftable[255] = { 2048, 2098, 2148, 2198, 2248, 2298, 2348, 2398, 2447, 2496, 2545, 2594, 2642, 2690, 2737, 2785, 2831, 2877, 2923, 2968, 3013, 3057, 3100, 3143, 3185, 3227, 3267, 3307, 3347, 3385, 3423, 3460, 3496, 3531, 3565, 3598, 3631, 3662, 3692, 3722, 3750, 3778, 3804, 3829, 3854, 3877, 3899, 3920, 3940, 3958, 3976, 3992, 4007, 4021, 4034, 4046, 4056, 4065, 4073, 4080, 4086, 4090, 4093, 4095, 4095, 4095, 4093, 4090, 4086, 4080, 4073, 4065, 4056, 4046, 4034, 4021, 4007, 3992, 3976, 3958, 3940, 3920, 3899, 3877, 3854, 3829, 3804, 3778, 3750, 3722, 3692, 3662, 3631, 3598, 3565, 3531, 3496, 3460, 3423, 3385, 3347, 3307, 3267, 3227, 3185, 3143, 3100, 3057, 3013, 2968, 2923, 2877, 2831, 2785, 2737, 2690, 2642, 2594, 2545, 2496, 2447, 2398, 2348, 2298, 2248, 2198, 2148, 2098, 2047, 1997, 1947, 1897, 1847, 1797, 1747, 1697, 1648, 1599, 1550, 1501, 1453, 1405, 1358, 1310, 1264, 1218, 1172, 1127, 1082, 1038, 995, 952, 910, 868, 828, 788, 748, 710, 672, 635, 599, 564, 530, 497, 464, 433, 403, 373, 345, 317, 291, 266, 241, 218, 196, 175, 155, 137, 119, 103, 88, 74, 61, 49, 39, 30, 22, 15, 9, 5, 2, 0, 0, 0, 2, 5, 9, 15, 22, 30, 39, 49, 61, 74, 88, 103, 119, 137, 155, 175, 196, 218, 241, 266, 291, 317, 345, 373, 403, 433, 464, 497, 530, 564, 599, 635, 672, 710, 748, 788, 828, 868, 910, 952, 995, 1038, 1082, 1127, 1172, 1218, 1264, 1310, 1358, 1405, 1453, 1501, 1550, 1599, 1648, 1697, 1747, 1797, 1847, 1897, 1947 }; int a=0; int b=1; /*// 方法一：限幅滤波法 方法：根据经验判断，确定两次采样允许的最大偏差值（设为A），每次检测到新值时判断： 如果本次值与上次值之差<=A，则本次值有效， 如果本次值与上次值之差>A，则本次值无效，放弃本次值，用上次值代替本次值。 优点：能克服偶然因素引起的脉冲干扰 缺点：无法抑制周期性的干扰，平滑度差 //*/ #define A 51 u16 Value1; u16 filter1() { u16 NewValue; Value1 = ftable[b-1]; NewValue = ftable[b]; b++; a++; if(a==255) a=0; if(b==255) b=1; if(((NewValue - Value1) > A) || ((Value1 - NewValue) > A)) { print_host(ftable[a],NewValue); return NewValue; } else { print_host(ftable[a],Value1); return Value1; } } /*// 方法二：中位值滤波法 方法： 连续采样N次（N取奇数），把N次采样值按大小排列，取中间值为本次有效值。 优点：克服偶然因素（对温度、液位的变化缓慢的被测参数有良好的滤波效果） 缺点：对流量、速度等快速变化的参数不宜 //*/ //#define N 3 //u16 value_buf[N]; //u16 filter2() //{ // u16 count,i,j,temp; // for(count=0;count<N;count++) // { // value_buf[count] = ftable[a]; // a++; // if(a==255) a=0; // } // for (j=0;j<N-1;j++) // { // for (i=0;i<N-j;i++) // { // if ( value_buf[i] > value_buf[i+1] ) // { // temp = value_buf[i]; // value_buf [i]= value_buf[i+1]; // value_buf[i+1] = temp; // } // } // } printf("%d\n",value_buf[(N-1)/2]); // return value_buf[(N-1)/2]; //} //void pros2() //{ // print_host(4,filter2()); //} /*// 方法三：算术平均滤波法 方法：连续取N个采样值进行算术平均运算：（ N值的选取：一般流量，N=12；压力：N=4。） N值较大时：信号平滑度较高，但灵敏度较低； N值较小时：信号平滑度较低，但灵敏度较高； 优点：适用于对一般具有随机干扰的信号进行滤波；这种信号的特点是有一个平均值，信号在某一数值范围附近上下波动 缺点：对于测量速度较慢或要求数据计算速度较快的实时控制不适用，比较浪费RAM。 //*/ //#define N 5 //u16 filter3() //{ // u16 sum = 0,count; // for ( count=0;count<N;count++) // { // sum = sum+ ftable[a]; // a++; // if(a==255) a=0; // } // print_host(4,sum/N); printf("%d\n",sum/N); // return (sum/N); //} /*// 方法四：递推平均滤波法（又称滑动平均滤波法） 方法： 把连续取得的N个采样值看成一个队列，队列的长度固定为N， 每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出原则）， 把队列中的N个数据进行算术平均运算，获得新的滤波结果。 N值的选取：流量，N=12；压力，N=4；液面，N=4-12；温度，N=1-4。 优点：对周期性干扰有良好的抑制作用，平滑度高； 适用于高频振荡的系统。 缺点：灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差； 不易消除由于脉冲干扰所引起的采样值偏差； 不适用于脉冲干扰比较严重的场合； 比较浪费RAM。 //*/ //#define FILTER4_N 3 //u16 filter_buf[FILTER4_N + 1]; //u16 filter4() //{ // int i; // int filter_sum = 0; // filter_buf[FILTER4_N] = ftable[a]; // a++; // if(a==255) a=0; // for(i = 0; i < FILTER4_N; i++) // { // filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉 // filter_sum += filter_buf[i]; // } printf("%d\n",filter_sum / FILTER4_N); // return (int)(filter_sum / FILTER4_N); //} //void pros4(void) //{ // u16 i=0; // print_host(4,filter4()); //} /*// 方法五：中位值平均滤波法（又称防脉冲干扰平均滤波法） 方法： 采一组队列去掉最大值和最小值后取平均值， （N值的选取：3-14）。 相当于“中位值滤波法”+“算术平均滤波法”。 连续采样N个数据，去掉一个最大值和一个最小值， 然后计算N-2个数据的算术平均值。 优点： 融合了“中位值滤波法”+“算术平均滤波法”两种滤波法的优点。 对于偶然出现的脉冲性干扰，可消除由其所引起的采样值偏差。 对周期干扰有良好的抑制作用。 平滑度高，适于高频振荡的系统。 缺点：对于测量速度较慢或要求数据计算速度较快的实时控制不适用，比较浪费RAM。 //*/ //#define N 3 //int filter5() //{ // int i, j; // int filter_temp, filter_sum = 0; // int filter_buf[N]; // for(i = 0; i < N; i++) // { // filter_buf[i] = ftable[a]; // a++; // if(a==255) a=0; // delay_us(10); // } // // 采样值从小到大排列（冒泡法） // for(j = 0; j < N - 1; j++) // { // for(i = 0; i < N - 1 - j; i++) // { // if(filter_buf[i] > filter_buf[i + 1]) // { // filter_temp = filter_buf[i]; // filter_buf[i] = filter_buf[i + 1]; // filter_buf[i + 1] = filter_temp; // } // } // } // // 去除最大最小极值后求平均 // for(i = 1; i < N - 1; i++) filter_sum += filter_buf[i]; printf("%d\n",filter_sum / ( N - 2)); // return filter_sum / (N - 2); //} //void pros5(void) //{ // u16 i=0; // for(i=0;i<255;i++) // { // print_host(ftable[i],filter5()); // } //} /*// 方法六：限幅平均滤波法 方法： 相当于“限幅滤波法”+“递推平均滤波法”； 每次采样到的新数据先进行限幅处理， 再送入队列进行递推平均滤波处理。 优点： 融合了两种滤波法的优点； 对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差。 缺点：比较浪费RAM。 //*/ //#define FILTER6_N 3 //#define FILTER6_A 51 //int filter_buf[FILTER6_N]; //int filter6() //{ // int i; // int filter_sum = 0; // filter_buf[FILTER6_N - 1] = ftable[a]; // a++; // if(a==255) a=0; // if(((filter_buf[FILTER6_N - 1] - filter_buf[FILTER6_N - 2]) > FILTER6_A) || ((filter_buf[FILTER6_N - 2] - filter_buf[FILTER6_N - 1]) > FILTER6_A)) // filter_buf[FILTER6_N - 1] = filter_buf[FILTER6_N - 2]; // for(i = 0; i < FILTER6_N - 1; i++) // { // filter_buf[i] = filter_buf[i + 1]; // filter_sum += filter_buf[i]; // } printf("%d\n",filter_sum / ( FILTER6_N - 1)); // return filter_sum / (FILTER6_N - 1); //} //void pros6(void) //{ // print_host(4,filter6()); //} /*// 方法七：一阶滞后滤波法 方法： 取a=0-1，本次滤波结果=(1-a)*本次采样值+a*上次滤波结果。 优点： 对周期性干扰具有良好的抑制作用； 适用于波动频率较高的场合。 平滑度高，适于高频振荡的系统。 缺点： 相位滞后，灵敏度低； 滞后程度取决于a值大小； 不能消除滤波频率高于采样频率1/2的干扰信号。 //*/ //#define FILTER7_A 0.01 //u16 Value; //u16 filter7() //{ // int NewValue; // Value = ftable[b-1]; // NewValue = ftable[b]; // b++; // if(b==255) b=1; // Value = (int)((float)NewValue * FILTER7_A + (1.0 - FILTER7_A) * (float)Value); printf("%d\n",Value); // return Value; //} //void pros7(void) //{ // u16 i=0; // for(i=0;i<255;i++) // { // print_host(ftable[i],filter7()); // } //} /*// 方法八：加权递推平均滤波法 方法： 是对递推平均滤波法的改进，即不同时刻的数据加以不同的权； 通常是，越接近现时刻的数据，权取得越大。 给予新采样值的权系数越大，则灵敏度越高，但信号平滑度越低。 优点： 适用于有较大纯滞后时间常数的对象，和采样周期较短的系统。 缺点： 对于纯滞后时间常数较小、采样周期较长、变化缓慢的信号； 不能迅速反应系统当前所受干扰的严重程度，滤波效果差。 //*/ //#define FILTER8_N 12 //int coe[FILTER8_N] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; // 加权系数表 //int sum_coe = 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12; // 加权系数和 //int filter_buf[FILTER8_N + 1]; //int filter8() //{ // int i; // int filter_sum = 0; // filter_buf[FILTER8_N] = ftable[a]; // a++; // if(a==255) a=0; // for(i = 0; i < FILTER8_N; i++) // { // filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉 // filter_sum += filter_buf[i] * coe[i]; // } // filter_sum /= sum_coe; printf("%d\n",filter_sum); // return filter_sum; //} //void pros8(void) //{ // u16 i=0; // for(i=0;i<255;i++) // { // print_host(ftable[i],filter8()); // } //} /*// 方法九： 消抖滤波法 方法： 设置一个滤波计数器，将每次采样值与当前有效值比较： 如果采样值=当前有效值，则计数器清零； 如果采样值<>当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）； 如果计数器溢出，则将本次值替换当前有效值，并清计数器。 优点： 对于变化缓慢的被测参数有较好的滤波效果； 可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。 缺点： 对于快速变化的参数不宜； 如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。 //*/ //#define FILTER9_N 51 //u16 i = 0; //u16 Value; //u16 filter9() //{ // int new_value; // Value = ftable[b-1]; // new_value = ftable[b]; // b++; // if(b==255) b=1; // if(Value != new_value) // { // i++; // if(i > FILTER9_N) // { // i = 0; // Value = new_value; // } // } // else i = 0; // return Value; //} //void pros9(void) //{ // u16 i=0; // for(i=0;i<255;i++) // { // print_host(ftable[i],filter9()); // } //} /*// 方法十：限幅消抖滤波法 方法： 相当于“限幅滤波法”+“消抖滤波法”； 先限幅，后消抖。 优点： 继承了“限幅”和“消抖”的优点； 改进了“消抖滤波法”中的某些缺陷，避免将干扰值导入系统。 缺点： 对于快速变化的参数不宜。 //*/ //#define FILTER10_A 51 //#define FILTER10_N 5 //u16 i = 0; //u16 Value; //u16 filter10() //{ // u16 NewValue; // u16 new_value; // Value = ftable[b-1]; // NewValue = ftable[b]; // b++; // if(b==255) b=1; // if(((NewValue - Value) > FILTER10_A) || ((Value - NewValue) > FILTER10_A)) // new_value = Value; // else // new_value = NewValue; // if(Value != new_value) // { // i++; // if(i > FILTER10_N) // { // i = 0; // Value = new_value; // } // } // else i = 0; // return Value; //} //void pros10(void) //{ // u16 i=0; // for(i=0;i<255;i++) // { // print_host(ftable[i],filter10()); // } //} /*// 主函数 //*/ int main(void) { delay_init(); //延时函数初始化 uart_init(256000); //串口初始化为9600 LED_Init(); //初始化与LED连接的硬件接口 while(1) { filter1(); // filter2(); // pros2(); // filter3(); // filter4(); // pros4(); // filter5(); // pros5(); // filter6(); // pros6(); // filter7(); // pros7(); // filter8(); // pros8(); // filter9(); // pros9(); // filter10(); // pros10(); delay_ms(20); } } 5.互补滤波器 对IMU解算的roll和pitch进行线性叠加，给出更准确的估计。

Kalman Filter 更适合 9 轴的传感器，也就是在 6 轴的基础上（3-axis Accel + 3-axis Gyro）融合 3 轴的磁力计。对于一个只有 6 轴 IMU 的 MCU，轻量级的 互补滤波器 (Complementary Filter) 更加合适，利用 3 轴陀螺仪和 3 轴加速度计来估计开发板的姿态 (Pitch, Roll, Yaw)。

#include <math.h> #include <icm20608.h> #include <stdio.h> // Sample Frequency 100 Hz const rt_int32_t TIME_STEP_MS = 10; // For 250 deg/s range, check the datasheet double gSensitivity = 131; // For 2g range, check the datasheet double aSensitivity = 16384; // Raw data from the IMU rt_int16_t accel_x, accel_y, accel_z; rt_int16_t gyro_x, gyro_y, gyro_z; // Predicted Orientation (Gyro) double gx = 0, gy = 0, gz = 0; // Predicted Orientation (Acc) double ax = 0, ay = 0; double accelX = 0, accelY = 0, accelZ = 0; double gyrX = 0, gyrY = 0, gyrZ = 0; int main(void) { icm20608_device_t imu = icm20608_init("i2c3"); if(imu != RT_NULL) { rt_kprintf("Initialized IMU\n"); } icm20608_calib_level(imu, 500); while (1) { rt_tick_t prevTime = rt_tick_get(); if (icm20608_get_accel(imu, &accel_x, &accel_y, &accel_z) == RT_EOK) { if(icm20608_get_gyro(imu, &gyro_x, &gyro_y, &gyro_z) == RT_EOK) { accelX = accel_x / aSensitivity; accelY = accel_y / aSensitivity; accelZ = accel_z / aSensitivity; gyrX = gyro_x / gSensitivity; gyrY = gyro_y / gSensitivity; gyrZ = gyro_z / gSensitivity; // angles based on accelerometer ax = atan2(accelY, accelZ) * 180 / M_PI; // roll ay = atan2(-accelX, sqrt( pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI; // pitch // This is incorrect, many tutorials make this mistake // ax = atan2(accelY, sqrt( pow(accelX, 2) + pow(accelZ, 2))) * 180 / M_PI; // roll // angles based on gyro (deg/s) gx = gx + gyrX * TIME_STEP_MS / 1000; gy = gy + gyrY * TIME_STEP_MS / 1000; gz = gz + gyrZ * TIME_STEP_MS / 1000; // complementary filter gx = gx * 0.96 + ax * 0.04; gy = gy * 0.96 + ay * 0.04; printf("%d %d %d %d %d %d %.4f %.4f %.4f \n", accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, gx, gy, gz); } }; while( (rt_tick_get() - prevTime) < rt_tick_from_millisecond(TIME_STEP_MS)); } }