代码收藏家 深入理解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));
}
}
