SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
Diff: main.cpp
- Revision:
- 4:b9dd320947ff
- Parent:
- 3:502b83f7761c
- Child:
- 5:2f0633d8fc20
diff -r 502b83f7761c -r b9dd320947ff main.cpp --- a/main.cpp Mon Sep 05 14:43:43 2016 +0000 +++ b/main.cpp Wed Sep 14 05:57:55 2016 +0000 @@ -1,12 +1,15 @@ #include "mbed.h" #include "LSM9DS0_SH.h" -#define pi 3.141592f + +#define pi 3.141592f #define d2r 0.01745329f -#define Rms 5000 -#define dt 0.005f + +#define Rms 5000 //TT rate +#define dt 0.003f +#define NN 200 + #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) - //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// //~~~structure~~~// DigitalOut led(D13); //detection @@ -17,17 +20,77 @@ DigitalOut SPI_CSXM(D6,1); //low for ACC/MAG enable SPI spi(D4, D5, D3); //MOSI MISO SCLK +//~~~Servo out~~~// +PwmOut Drive1(D8); //control leg +PwmOut Drive2(D9); +PwmOut Drive3(D10); +PwmOut Drive4(D11); +PwmOut Drive5(D14); +PwmOut Drive6(D15); + //~~~Serial~~~// Serial pc(D1, D0); //Serial reg(TX RX) -//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// -//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// //~~~globle~~~// Ticker TT; //call a timer int count = 0; //one second counter for extrenal led blink +//~~~PWMreference~~~// +const int pwm_mid = 1450; //+2080 for minimall lenght +const int PWM_base[1][6] = { //desired six reference command at 0 deg + {pwm_mid+70,pwm_mid+10,pwm_mid+40,pwm_mid+60,pwm_mid+10,pwm_mid+30}, +// {pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid}, //only for debugging +}; +int PWM[1][6] = { //desired six reference command by PWM_base + PWM + {0,0,0,0,0,0}, //transfer equaliyu 10us to 1 deg +}; + +//~~~RR varible~~~// +float Z0 = 2.45; //as mid point hight +float Z_dis = -0.087; //move rotation center + +//~~~stPF_lenth_uni varible~~~// +const float alpha = 2.094f; //pair angle +const float beta = 0.1309f; //couple angle +const float A[3][6] = { //base cood + {cos(0.5f*alpha-beta),cos(0.5f*alpha+beta),cos(1.5f*alpha-beta),cos(1.5f*alpha+beta),cos(2.5f*alpha-beta),cos(2.5f*alpha+beta)}, + {sin(0.5f*alpha-beta),sin(0.5f*alpha+beta),sin(1.5f*alpha-beta),sin(1.5f*alpha+beta),sin(2.5f*alpha-beta),sin(2.5f*alpha+beta)}, + {0,0,0,0,0,0} +}; +const float B[3][6] = { //top cood(static) + {cos(beta),cos(alpha-beta),cos(alpha+beta),cos(2*alpha-beta),cos(2*alpha+beta),cos(3*alpha-beta)}, + {sin(beta),sin(alpha-beta),sin(alpha+beta),sin(2*alpha-beta),sin(2*alpha+beta),sin(3*alpha-beta)}, + {Z_dis,Z_dis,Z_dis,Z_dis,Z_dis,Z_dis} +}; +float C[3][6] = { //top cood(active) + {0,0,0,0,0,0}, + {0,0,0,0,0,0}, + {0,0,0,0,0,0} +}; +float VEC[1][6] = { //desired six reference command + {0,0,2.52,0,0,0}, //X Y Z VEC[0][3] VEC[0][4] VEC[0][5] +}; +float L[1][6] = { //desired six reference command + {0,0,0,0,0,0}, +}; +float Rtot[3][3] = { //RR' + {0,0,0}, + {0,0,0}, + {0,0,0} +}; + +//~~~stPF_tracle_R varible~~~// +const float L90 = 2.422; //L under 90° (195 mm) +const float Larm = 0.4969; //arm lenth (40 mm) b +const float Llink = 0.8944; //link lenth (72 mm) +float A_R = 0; +float B_R = 0; +float C_R = 0; + //~~~IMU_SPI~~~// short low_byte = 0x00; //buffer short high_byte = 0x00; @@ -39,20 +102,35 @@ float Ay = 0.0; float Az = 0.0; float gDIR[1][3] = { //g vector's direction , required unitconstrain - {0,0,-1}, //X Y Z + {0.0,0.0,0.0}, //X Y Z }; -float gUnity = 0; +float Bet = 0.005; //confidence of Acc data +float gUnity = 0.0; +float Gdx = 0.0; +float Gdy = 0.0; +float Gdz = 0.0; +float Ele_phy = 0.0; //estimation of top plate attitide +float Til_phy = 0.0; +float Ele_phy_int = 0.0; +float Til_phy_int = 0.0; //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// -void init_TIMER(); //set TT_main() rate -void TT_main(); //timebase function rated by TT -void init_IO(); //initialize IO state -void init_IMU(); //initialize IMU -void read_IMU(); //read IMU data give raw data -void state_update(); //estimation of new attitude +void init_TIMER(); //set TT_main() rate +void TT_main(); //timebase function rated by TT +void init_IO(); //initialize IO state +void init_IMU(); //initialize IMU +void init_Gdrift(); //read Gdrift at start up +void read_IMU(); //read IMU data give raw data +void state_update(); //estimation of new attitude + +void RR(); //status generator +void stPF_lenth_uni(); //referenve generator +void stPF_travle_R(); //lenth to pwm pulse width + +float lpf(float input, float output_old, float frequency); //lpf discrete //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// @@ -60,13 +138,18 @@ //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// int main() { - init_IO(); //initialized value - init_IMU(); //initialize IMU - init_TIMER(); //start TT_main - pc.baud(115200); //set baud rate + pc.baud(115200); //set baud rate + wait_ms(1000); - while(1) { //main() loop - if(count >= 200) { //check if main working + init_IO(); //initialized value + init_IMU(); //initialize IMU + init_Gdrift(); //read Gdrift at start up + wait_ms(1000); + + init_TIMER(); //start TT_main + + while(1) { //main() loop + if(count >= NN) { //check if main working count=0; led = !led; } @@ -90,25 +173,43 @@ read_IMU(); //read IMU data give raw data state_update(); //estimation of new attitude - pc.printf("%.2f\t%.2f\t%.2f\n", gDIR[0][0], gDIR[0][1], gDIR[0][2]); -// pc.printf("%.2f\t%.2f\n", Sele, Srol); -// pc.printf("%.2f\n", Sx); + RR(); //VEC generated + stPF_lenth_uni(); //L generated + stPF_travle_R(); //PWM generated + + for(int i=0; i<6; i++) { //safty constrain + PWM[0][i] = constrain(PWM[0][i],725,2025); + } + + Drive1.pulsewidth_us(PWM[0][0]); //drive command + Drive2.pulsewidth_us(PWM[0][1]); + Drive3.pulsewidth_us(PWM[0][2]); + Drive4.pulsewidth_us(PWM[0][3]); + Drive5.pulsewidth_us(PWM[0][4]); + Drive6.pulsewidth_us(PWM[0][5]); + +//for Serial-Oscilloscope +// pc.printf("%.3f\r", Bet); + pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy); +// pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]); +// pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az); +// pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]); } -//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// -//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void init_IO(void) //initialize { TT_ext = 0; led = 1; } -//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// -//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void init_IMU(void) //initialize { //gloable config @@ -139,11 +240,33 @@ spi.write(0xC8); //cut off 50 Hz/ Scale +-4g SPI_CSXM = 1; //end spi talking } -//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// -//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_Gdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +void init_Gdrift(void) //initialize +{ + for(int i=0; i<1000; i++) { + read_IMU(); //note Gdrift = 0 at this moment + gDIR[0][0] = gDIR[0][0] - Wx; + gDIR[0][1] = gDIR[0][1] - Wy; + gDIR[0][2] = gDIR[0][2] - Wz; + wait_ms(2); + } + Gdx = gDIR[0][0] /1000.0f; + Gdy = gDIR[0][1] /1000.0f; + Gdz = gDIR[0][2] /1000.0f; +// pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz); + gDIR[0][0] = 0; + gDIR[0][1] = 0; + gDIR[0][2] = -1; +} +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_Gdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// + + + +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// void read_IMU(void) //read IMU data give raw data { //Wx @@ -153,7 +276,8 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking - Wx = Buff * 2.663e-4; //1.526e-2 * d2r = +// Wx = Buff * Gpx + Gdx; + Wx = lpf(Buff * Gpx + Gdx, Wx, 18.0f); //Wy SPI_CSG = 0; //start spi talking Wx spi.write(0xEA); //read B11101010 read/multi/address @@ -161,7 +285,8 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking - Wy = Buff * 2.663e-4; //1.526e-2 * d2r = +// Wy = Buff * Gpy + Gdy; + Wy = lpf(Buff * Gpy + Gdy, Wy, 18.0f); //Wz SPI_CSG = 0; //start spi talking Wx spi.write(0xEC); //read B11101100 read/multi/address @@ -169,7 +294,8 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSG = 1; //end spi talking - Wz = Buff * 2.663e-4; //1.526e-2 * d2r = +// Wz = Buff * Gpz + Gdz; + Wz = lpf(Buff * Gpz + Gdz, Wz, 18.0f); //Ax SPI_CSXM = 0; //start spi talking Ax spi.write(0xE8); //read B11101000 read/multi/address @@ -177,7 +303,7 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking - Ax = Buff * 1.22e-4; + Ax = lpf(Buff*Apx + Adx, Ax, 15.0f); //Ay SPI_CSXM = 0; //start spi talking Ax spi.write(0xEA); //read B11101010 read/multi/address @@ -185,7 +311,7 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking - Ay = Buff * 1.22e-4; + Ay = lpf(Buff*Apy + Ady, Ay, 15.0f); //Az SPI_CSXM = 0; //start spi talking Ax spi.write(0xEC); //read B11101100 read/multi/address @@ -193,7 +319,7 @@ high_byte = spi.write(0); Buff = high_byte << 8 |low_byte; SPI_CSXM = 1; //end spi talking - Az = Buff * 1.22e-4; + Az = lpf(Buff*Apz + Adz, Az, 15.0f); } //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// @@ -206,14 +332,110 @@ gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt; gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt; gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt; - + //update - - + gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax; + gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay; + gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az; + //nutralizing - gUnity = gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2]; + gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] ); for(int i=0; i<3; i++) { gDIR[0][i] = gDIR[0][i] / gUnity; } + + //transfer + Ele_phy = asin(gDIR[0][0]); + Til_phy = asin(-gDIR[0][1] / cos(Ele_phy)); +// //test +// gDIR[0][0] = gDIR[0][0] + Wx*dt; +// gDIR[0][1] = gDIR[0][1] + Wy*dt; +// gDIR[0][2] = gDIR[0][2] + Wz*dt; } -//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// \ No newline at end of file +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// + + + +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓RR funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +void RR() //status generator +{ + Ele_phy_int = Ele_phy_int + Ele_phy*dt; + Til_phy_int = Til_phy_int + Til_phy*dt; + Ele_phy_int = constrain(Ele_phy_int,-0.01f,0.01f); + Til_phy_int = constrain(Til_phy_int,-0.01f,0.01f); + + VEC[0][4] = -9.5f*Ele_phy - 0.17f*Wy - 60.0f*Ele_phy_int; + VEC[0][5] = -9.5f*Til_phy - 0.17f*Wx - 60.0f*Til_phy_int; + + VEC[0][4] = constrain(VEC[0][4],-0.30f,0.30f); + VEC[0][5] = constrain(VEC[0][5],-0.30f,0.30f); +} +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// + + + +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_lenth_uni funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +void stPF_lenth_uni() //referenve generator +{ + Rtot[0][0] = cos(VEC[0][4])*cos(VEC[0][3]); + Rtot[0][1] = - cos(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][3])*sin(VEC[0][4])*sin(VEC[0][5]); + Rtot[0][2] = sin(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][5])*cos(VEC[0][3])*sin(VEC[0][4]); + Rtot[1][0] = cos(VEC[0][4])*sin(VEC[0][3]); + Rtot[1][1] = cos(VEC[0][5])*cos(VEC[0][3]) + sin(VEC[0][4])*sin(VEC[0][5])*sin(VEC[0][3]); + Rtot[1][2] = - cos(VEC[0][3])*sin(VEC[0][5]) + cos(VEC[0][5])*sin(VEC[0][4])*sin(VEC[0][3]); + Rtot[2][0] = - sin(VEC[0][4]); + Rtot[2][1] = cos(VEC[0][4])*sin(VEC[0][5]); + Rtot[2][2] = cos(VEC[0][4])*cos(VEC[0][5]); + + for(int j=0; j<6; j++) { //reset C + for(int i=0; i<3; i++) { + C[i][j] = 0; + } + } + + for(int i=0; i<6; i++) { //(RR.' * B) + for(int j=0; j<3; j++) { + for(int k=0; k<3; k++) { + C[j][i] = Rtot[j][k] * B[k][i] + C[j][i]; + } + } + } + + for(int j=0; j<6; j++) { //+ [X,Y,Z] + for(int i=0; i<3; i++) { + C[i][j] = C[i][j] + VEC[0][i]; + } + } + for(int i=0; i<6; i++) { + float X = C[0][i]-A[0][i]; + float Y = C[1][i]-A[1][i]; + float Z = C[2][i]-A[2][i]; + L[0][i] = sqrt(X*X+Y*Y+Z*Z); + } +} +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_lenth_uni funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// + + + +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_travle_R funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +void stPF_travle_R() //lenth to pwm pulse width +{ + for(int i=0; i<6; i++) { + A_R = ( L[0][i] - L90 + sqrt( Llink*Llink - Larm*Larm ) ) /Larm; + B_R = Llink/Larm; + C_R = ( A_R*A_R - B_R*B_R + 1 ) /(A_R*2); + PWM[0][i] = -asin(C_R)*573 + PWM_base[0][i]; + } +} +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_travle_R funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// + + + +//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓// +float lpf(float input, float output_old, float frequency) +{ + float output = 0; + output = (output_old + frequency*dt*input) / (1 + frequency*dt); + return output; +} +//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of lpf funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑// \ No newline at end of file