![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
pure sensor fusion
main.cpp
- Committer:
- roger5641
- Date:
- 2017-06-27
- Revision:
- 1:81a146dffd7a
- Parent:
- 0:56bfa7bd6f71
File content as of revision 1:81a146dffd7a:
#include "mbed.h" #include "LSM9DS0.h" #include <math.h> #define pi 3.1415926 #define g 9.81 // timer 1 #define LOOPTIME 1000 // 1 ms unsigned long lastMilli = 0; // sampling time float T = 0.001; Timer t; Serial uart_1(D10,D2); // TX : D10 RX : D2 // serial 1 void init_uart(void); void separate(void); void uart_send(void); float lpf(float input, float output_old, float frequency); // uart send data int display[6] = {0,0,0,0,0,0}; char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte void init_sensor(void); void real_sensor_value(void); void sensor_fusion(void); LSM9DS0 sensor(SPI_MODE, D9, D6); // SPI_CS1 : D7 , SPI_CS2 : D6 int sensor_flag = 0; // sensor initial flag int sensor_count = 0; /*******************************Sensor***************************************/ #define Gyro_gain_x 0.00113356568421052631578947368421 #define Gyro_gain_y 0.00113356568421052631578947368421 #define Gyro_gain_z 0.00113356568421052631578947368421 #define Acce_gain_x -0.00165873343713498511154419826254 //-9.81/(max-zero) #define Acce_gain_y -0.00217691902504394899561081096519 #define Acce_gain_z -0.00236798520156680310444239205579 /*******************************Sensor***************************************/ #define Gyro_gain_x 0.00113356568421052631578947368421 #define Gyro_gain_y 0.00113356568421052631578947368421 #define Gyro_gain_z 0.00113356568421052631578947368421 #define Acce_gain_x -0.00165873343713498511154419826254 //-9.81/(max-zero) #define Acce_gain_y -0.00217691902504394899561081096519 #define Acce_gain_z -0.00236798520156680310444239205579 // 宣告從sensor讀到的值要存入處理的變數 float Gyro_axis_data[3]; // X, Y, Z axis float Gyro_axis_data_f[3]; float Gyro_axis_data_f_old[3]; float Gyro_scale[3]; // scale = (data - zero) * gain float Gyro_axis_zero[3] = {-3.7153254648333,-8.013402897667,-57.26524894}; float Acce_axis_data[3]; // X, Y, Z axis float Acce_axis_data_f[3]; float Acce_axis_data_f_old[3]; float Acce_scale[3]; // scale = (data - zero) * gain float Acce_axis_zero[3] = {-840.873762375,-314.556930675,0.50990098975}; //-840.45049505 -395.32178215 4143.272277 //-841.2970297 4191.811881 1.3762376235 //5073.277228 -233.7920792 -0.356435644 // final sensor output value float axm = 0; float aym = 0; float azm = 0; float u1 = 0; float u2 = 0; float u3 = 0; // new defined direction float ax = 0; float ay = 0; float az = 0; float w1 = 0; float w2 = 0; float w3 = 0; // estimated state variables float gs1_hat = 0; float gs1_hat_old = 0; float gs2_hat = 0; float gs2_hat_old = 0; float gs3_hat = 0; float gs3_hat_old = 0; // normalized variables float n = 0; float gs1_hat_n = 0; float gs2_hat_n = 0; float gs3_hat_n = 0; //general variables float gs1_hat_g = 0; float gs2_hat_g = 0; float gs3_hat_g = 0; // bandwidth float alpha = 6.28; // 1Hz float psi = 0.0; // pitch angle //Sensor Fusion //****************theta Angle****************************// float theta = 0.0; // theta angle float theta_old = 0.0; float Itheta = 0.0; float Itheta_old = 0.0; float dtheta = 0.0; float dtheta_old = 0.0; float dtheta_f = 0.0; float dtheta_f_old = 0.0; //****************phi Angle****************************// float phi = 0.0; // theta angle float phi_old = 0.0; float Iphi = 0.0; float Iphi_old = 0.0; float dphi = 0.0; float dphi_old = 0.0; float dphi_f = 0.0; float dphi_f_old = 0.0; //****************roll Angle****************************// float roll = 0.0; float roll_old = 0.0; float roll_f = 0.0; float roll_f_old = 0.0; float Iroll = 0.0; // float Iroll_f = 0.0; float Iroll_f_old = 0.0; float droll = 0.0; //roll Angle float droll_old = 0.0; float droll_f = 0.0; float droll_f_old = 0.0; //float roll_1 = 0.0; //****************roll Angle without theta ****************************// float gama = 0.0; //Roll Angle float gama_old = 0.0; float gama_f = 0.0; float gama_f_old = 0.0; float Igama = 0.0; float Igama_f = 0.0; float Igama_f_old = 0.0; float dgama = 0.0; float dgama_old = 0.0; float dgama_f = 0.0; float dgama_f_old = 0.0; //****************pitch Angle****************************// float pitch = 0.0; //pitch Angle float pitch_old = 0.0; float pitch_f = 0.0; float pitch_f_old = 0.0; float Ipitch = 0.0; // float Ipitch_f = 0.0; float Ipitch_f_old = 0.0; float dpitch = 0.0; //pitch Angle float dpitch_old = 0.0; float dpitch_f = 0.0; float dpitch_f_old = 0.0; float pitch_0 = 0.0; // int main() { t.start(); init_uart(); init_sensor(); while(1) { // timer 1 if((t.read_us() - lastMilli) >= LOOPTIME) // 2000 us = 2 ms { lastMilli = t.read_us(); // sensor initial start if(sensor_flag == 0) { sensor_count++; if(sensor_count >= 50) { sensor_flag = 1; sensor_count = 0; } } else { real_sensor_value(); sensor_fusion(); uart_send(); } } } // while(1) end } int i = 0; void uart_send(void) { // uart send data display[0] = 100*pitch*180/3.1415926; display[1] = 100*roll*180/3.1415926; display[2] = 100*w3; display[3] = 100*gs1_hat_n; display[4] = 100*gs2_hat_n; display[5] = 100*gs3_hat_n; separate(); int j = 0; int k = 1; while(k) { if(uart_1.writeable()) { uart_1.putc(num[i]); i++; j++; } if(j>1) // send 2 bytes { k = 0; j = 0; } } if(i>13) i = 0; } void real_sensor_value(void) { // sensor raw data Gyro_axis_data[0] = sensor.readRawGyroX(); Gyro_axis_data[1] = sensor.readRawGyroY(); Gyro_axis_data[2] = sensor.readRawGyroZ(); Acce_axis_data[0] = sensor.readRawAccelX(); Acce_axis_data[1] = sensor.readRawAccelY(); Acce_axis_data[2] = sensor.readRawAccelZ(); // gyro filter Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18); Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0]; Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18); Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1]; Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18); Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2]; // acce filter Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18); Acce_axis_data_f_old[0] = Acce_axis_data_f[0]; Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18); Acce_axis_data_f_old[1] = Acce_axis_data_f[1]; Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18); Acce_axis_data_f_old[2] = Acce_axis_data_f[2]; Gyro_scale[0] = (Gyro_axis_data_f[0]-Gyro_axis_zero[0])*Gyro_gain_x; Gyro_scale[1] = (Gyro_axis_data_f[1]-Gyro_axis_zero[1])*Gyro_gain_y; Gyro_scale[2] = (Gyro_axis_data_f[2]-Gyro_axis_zero[2])*Gyro_gain_z; Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x; Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y; Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z; // final 6 axis data axm = Acce_scale[0]; aym = Acce_scale[1]; azm = Acce_scale[2]; u1 = Gyro_scale[0]; u2 = Gyro_scale[1]; u3 = Gyro_scale[2]; ax = axm; ay = aym; az = azm; w1 = u1; w2 = u2; w3 = u3; } void init_uart() { uart_1.baud(115200); // 設定baudrate } void separate(void) { num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列 num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 num[4] = display[1] >> 8; num[5] = display[1] & 0x00ff; num[6] = display[2] >> 8; num[7] = display[2] & 0x00ff; num[8] = display[3] >> 8; num[9] = display[3] & 0x00ff; num[10] = display[4] >> 8; num[11] = display[4] & 0x00ff; num[12] = display[5] >> 8; num[13] = display[5] & 0x00ff; } void init_sensor(void) { sensor.begin(); // sensor.begin() setting : // uint16_t begin(gyro_scale gScl = G_SCALE_2000DPS, // accel_scale aScl = A_SCALE_8G, // mag_scale mScl = M_SCALE_8GS, // gyro_odr gODR = G_ODR_760_BW_100, // accel_odr aODR = A_ODR_800, // mag_odr mODR = M_ODR_100); } void sensor_fusion(void) { gs1_hat = lpf(ax + w3*ay/alpha - w2*az/alpha , gs1_hat_old , alpha); gs1_hat_old = gs1_hat; gs2_hat = lpf(-w3*ax/alpha + ay + w1*az/alpha , gs2_hat_old , alpha); gs2_hat_old = gs2_hat; gs3_hat = lpf(w2*ax/alpha - w1*ay/alpha + az , gs3_hat_old , alpha); gs3_hat_old = gs3_hat; n = sqrt(gs1_hat*gs1_hat + gs2_hat*gs2_hat + gs3_hat*gs3_hat); gs1_hat_n = (gs1_hat / n) * g; gs2_hat_n = (gs2_hat / n) * g; gs3_hat_n = (gs3_hat / n) * g; gs1_hat_g = cos(theta)*gs1_hat_n-sin(theta)*gs2_hat_n; gs2_hat_g = cos(phi)*sin(theta)*gs1_hat_n + cos(phi)*cos(theta)*gs2_hat_n - sin(phi)*gs3_hat_n; gs3_hat_g = sin(phi)*sin(theta)*gs1_hat_n + sin(phi)*cos(theta)*gs2_hat_n - cos(phi)*gs3_hat_n; pitch = asin(gs1_hat_g/g); roll = atan(gs2_hat_g/gs3_hat_g); gama = atan(gs2_hat_n / gs3_hat_n);//+0.0157; pitch_0 = asin(-gs1_hat_n/g); pitch_f = lpf(pitch, pitch_f_old, 1.0); pitch_f_old = pitch_f; Ipitch = Ipitch + pitch_f*T; Ipitch_f = lpf(Ipitch, Ipitch_f_old, 18.0); Ipitch_f_old = Ipitch_f; dpitch = (pitch - dpitch_old)/T; dpitch_old = pitch; dpitch_f = lpf(dpitch, dpitch_f_old, 1.0); dpitch_f_old = dpitch_f; roll_f = lpf(roll, roll_f_old, 1.0); roll_f_old = roll_f; Iroll = Iroll + roll_f*T; Iroll_f = lpf(Iroll, Iroll_f_old, 18.0); Iroll_f_old = Iroll_f; droll = (roll - droll_old)/T; droll_old = roll; droll_f = lpf(droll, droll_f_old, 1.0); droll_f_old = droll_f; gama_f = lpf(gama, gama_f_old, 1.0); gama_f_old = gama_f; Igama = Igama + gama_f*T; Igama_f = lpf(Igama, Igama_f_old, 18.0); Igama_f_old = Igama_f; dgama = (gama - dgama_old)/T; dgama_old = gama; dgama_f = lpf(dgama, dgama_f_old, 1.0); dgama_f_old = dgama_f; } float lpf(float input, float output_old, float frequency) { float output = 0; output = (output_old + frequency*T*input) / (1 + frequency*T); return output; }