solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 49:73fe59148dd4
- Parent:
- 48:1065506191f2
- Child:
- 52:abec61ea3cd3
diff -r 1065506191f2 -r 73fe59148dd4 main.cpp --- a/main.cpp Wed May 19 05:39:14 2021 +0000 +++ b/main.cpp Fri May 21 01:35:05 2021 +0000 @@ -24,14 +24,16 @@ #define servoPwmMin 1200.0 #define motorPwmMax 2000.0 #define motorPwmMin 1100.0 -#define pitch_align 8.0*3.141562/180.0 -#define roll_align -2.8*3.141562/180.0 + +#define N_EEPROM 8 MPU6050 accelgyro; MAG3110 mag(PB_9,PB_8); SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); DigitalIn userButton(USER_BUTTON); +DigitalIn locdef1(PG_2); +DigitalIn locdef2(PG_3); FastPWM servo(PE_9); PID pitchPID(5.0, 0,0,PID_dt); //rad PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s @@ -92,25 +94,70 @@ float accref[3] = {0, 0, -0.98}; float magref[3] = {0.65, 0, 0.75}; -int agoffset[6] = {0, 0, 0, -117, -563, 2 }; -float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610}; +int calibrationFlag = 0; +int pos_tail = 1;//1:left 2:center 3:right +int agoffset[6] = {0, 0, 0, 386, -450, 48}; +float magbias[4] = {-215.833374, 207.740616, -167.444565, 36.688690}; +float pitch_align = 0.0*3.141562/180.0 +float roll_align = 0.0*3.141562/180.0 +// eepromのread writeのためのunionを定義 +int address = 0xAE; // EEPROMの3つの入力が全て+より +int pointeraddress = 0; + +I2C i2c(PB_9,PB_8); // sda, scl +union U{ + int i[N_EEPROM]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias + char c[N_EEPROM*4]; // floatはcharの4倍 +}; void writeSdcard() { //magcal.getExtremes(&magmin[0],&magmax[0]); //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]); //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]); - pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi); + //pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi); //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi); //pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); - //pc.printf("%d \r\n",userButton.read()); - //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); - //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); + //pc.printf("%f %f \r\n",rc[1],servoOut[0]); + pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi); + //pc.printf("%f %f\r\n",roll_g*180.0f/pi,pitch_g*180.0f/pi); //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z); //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z); } +// eeprom書き込み・読み込みに必要な関数 +void writeEEPROM(int address, unsigned int eeaddress, char *data, int size) +{ + char i2cBuffer[size + 2]; + i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB + i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB + + for (int i = 0; i < size; i++) { + i2cBuffer[i + 2] = data[i]; + } + + int result = i2c.write(address, i2cBuffer, size + 2, false); + //sleep_ms(500); +} + +// this function has no read limit +void readEEPROM(int address, unsigned int eeaddress, char *data, int size) +{ + char i2cBuffer[2]; + i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB + i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB + + // Reset eeprom pointer address + int result = i2c.write(address, i2cBuffer, 2, false); + + //sleep_ms(500); + + // Read eeprom + i2c.read(address, data, size); + //sleep_ms(500); +} + float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; @@ -377,82 +424,6 @@ dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]); } -void execCalibration(){ - pc.printf("\r\nEnter to Calibration Mode\r\n"); - wait(5); - pc.printf("\r\n Acc and Gyro Calibration Start\r\n"); - int axs = 0; - int ays = 0; - int azs = 0; - int gxs = 0; - int gys = 0; - int gzs = 0; - int iter_n = 5000; - for(int i = 0;i<iter_n ;i++){ - accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); - axs += ax; - ays += ay; - azs -= az; - gxs += gx; - gys += gy; - gzs -= gz; - wait(0.001); - } - axs = axs /iter_n; - ays = ays /iter_n; - azs = azs /iter_n; - gxs = gxs /iter_n; - gys = gys /iter_n; - gzs = gzs /iter_n; - pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); - - pc.printf("\r\n Mag Calibration Start\r\n"); - float f = 0; - while(1){ - mag.getAxis(mdata); // flush the magnetmeter - magval[0] = (mdata.x - magbias[0]); - magval[1] = (mdata.y - magbias[1]); - magval[2] = (mdata.z - magbias[2]); - float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2]; - float lr = 0.00001f; - f = mag_r - magbias[3]*magbias[3]; - magbias[0] = magbias[0] + 4 * lr * f * magval[0]; - magbias[1] = magbias[1] + 4 * lr * f * magval[1]; - magbias[2] = magbias[2] + 4 * lr * f * magval[2]; - magbias[3] = magbias[3] + 4 * lr * f * magbias[3]; - - if(userButton.read() == 1){ - break; - } - wait(0.001); - } - pc.printf("magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]); - pc.printf("\r\n Refvec Calibration waiting\r\n"); - wait(5); - pc.printf("\r\n Calibration Start\r\n"); - float arefs[3] = {0.0,0.0,0.0}; - float mrefs[3] = {0.0,0.0,0.0}; - for(int i = 0;i<iter_n ;i++){ - getIMUval(); - arefs[0] += acc_x; - arefs[1] += acc_y; - arefs[2] += acc_z; - mrefs[0] += mag_x; - mrefs[1] += mag_y; - mrefs[2] += mag_z; - wait(0.001); - } - arefs[0] /= iter_n; - arefs[1] /= iter_n; - arefs[2] /= iter_n; - mrefs[0] /= iter_n; - mrefs[1] /= iter_n; - mrefs[2] /= iter_n; - pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]); - pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]); - while(1) {wait(1000);} -} - // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut(){ if(sbus.failSafe == false) { @@ -536,31 +507,62 @@ accelgyro.setDLPFMode(MPU6050_LPF); //地磁気 mag.enable(); + + qhat << 1 << 0 << 0 << 0; - if(userButton.read() == 0){ - qhat << 1 << 0 << 0 << 0; - - D.add(1,1,1.0); - D.add(2,2,1.0); - D.add(3,3,1.0); + D.add(1,1,1.0); + D.add(2,2,1.0); + D.add(3,3,1.0); + + Phat.add(1,1,0.1); + Phat.add(2,2,0.1); + Phat.add(3,3,0.1); + Phat.add(4,4,0.1); - Phat << 0.01 << 0 <<0 <<0 - << 0 << 0.01 <<0 <<0 - << 0 << 0 <<0.001 <<0 - << 0 << 0 << 0 << 0.001; + Qgyro.add(1,1,0.0224); + Qgyro.add(2,2,0.0224); + Qgyro.add(3,3,0.0224); + + Racc.add(1,1,0.0330*100); + Racc.add(2,2,0.0330*100); + Racc.add(3,3,0.0330*100); + + Rmag.add(1,1,1.0); + Rmag.add(2,2,1.0); + Rmag.add(3,3,1.0); + + calibrationFlag = userButton.read(); + if(calibrationFlag == 0){ - Qgyro << 0.0224<< 0 <<0 - << 0 <<0.0224<<0 - << 0 << 0 <<0.0224; - - Racc.add(1,1,0.0330*100); - Racc.add(2,2,0.0330*100); - Racc.add(3,3,0.0330*100); - - Rmag.add(1,1,1.0); - Rmag.add(2,2,1.0); - Rmag.add(3,3,1.0); - + pc.printf("reading calibration value\r\n"); + //キャリブレーション値を取得 + U read_calib; + readEEPROM(address, pointeraddress, read_calib.c, 32); + wait(3); + for (int i = 0 ; i < N_EEPROM; i ++){ + pos_tail = read_calib.i[0]; + agoffset[3] = float(read_calib.i[5]); + agoffset[4] = float(read_calib.i[6]); + agoffset[5] = float(read_calib.i[7]); + magbias[0] = float(read_calib.i[1])/1000.0; + magbias[1] = float(read_calib.i[2])/1000.0; + magbias[2] = float(read_calib.i[3])/1000.0; + magbias[3] = float(read_calib.i[4])/1000.0; + } + switch(pos_tail){ + case 1: + pc.printf("This MBED is Located at Left \r\n"); + break; + case 2: + pc.printf("This MBED is Located at Center \r\n"); + break; + case 3: + pc.printf("This MBED is Located at Right \r\n"); + break; + default: // error situation + pc.printf("error\r\n"); + break; + } getIMUval(); triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm); float sumLPaccnorm = 0; @@ -588,6 +590,144 @@ att_dt = (tend-tstart); } }else{ - execCalibration(); + pc.printf("\r\nEnter to Calibration Mode\r\n"); + wait(5); + pc.printf("Acc and Gyro Calibration Start\r\n"); + + int gxs = 0; + int gys = 0; + int gzs = 0; + int iter_n = 10000; + for(int i = 0;i<iter_n ;i++){ + accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); + gxs += gx; + gys += gy; + gzs -= gz; + //wait(0.01); + } + gxs = gxs /iter_n; + gys = gys /iter_n; + gzs = gzs /iter_n; + agoffset[3] = gxs; + agoffset[4] = gys; + agoffset[5] = gzs; + pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); + + pc.printf("Mag Calibration Start\r\n"); + float f = 0; + while(1){ + mag.getAxis(mdata); // flush the magnetmeter + magval[0] = (mdata.x - magbias[0]); + magval[1] = (mdata.y - magbias[1]); + magval[2] = (mdata.z - magbias[2]); + float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2]; + float lr = 0.00001f; + f = mag_r - magbias[3]*magbias[3]; + magbias[0] = magbias[0] + 4 * lr * f * magval[0]; + magbias[1] = magbias[1] + 4 * lr * f * magval[1]; + magbias[2] = magbias[2] + 4 * lr * f * magval[2]; + magbias[3] = magbias[3] + 4 * lr * f * magbias[3]; + + if(userButton.read() == 1){ + break; + } + wait(0.001); + } + pc.printf("Magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]); + + pc.printf("Determine Position of MBED\r\n"); + wait(1); + pc.printf("Press the user button\r\n"); + + while(userButton.read() == 0){wait(0.01);}; + while(1){ + pc.printf("Left\r\n"); + wait(2); + if(userButton.read() == 0){ + pos_tail=1; + break; + }; + pc.printf("Center\r\n"); + wait(2); + if(userButton.read() == 0){ + pos_tail=2; + break; + }; + pc.printf("Right\r\n"); + wait(2); + if(userButton.read() == 0){ + pos_tail=3; + break; + }; + }; + switch(pos_tail){ + case 1: + pc.printf("This MBED is Located at Left \r\n"); + break; + case 2: + pc.printf("This MBED is Located at Center \r\n"); + break; + case 3: + pc.printf("This MBED is Located at Right \r\n"); + break; + default: // error situation + pc.printf("error\r\n"); + break; + } + + + + pc.printf("Writing to EEPROM...\r\n"); + U transfer_data; + transfer_data.i[0] = pos_tail; + for (int i = 1; i < 5; i++) { + if (!isnan(magbias[i - 1])) + transfer_data.i[i] = int(magbias[i - 1]*1000); // intに丸めた値を送る。 + else { + pc.printf("Mag bias is NOT transferred\n"); + transfer_data.i[i] = 100; + } + } + // gxs,gys,gzsを送る + int gxyzs[3] = {gxs, gys, gzs}; + for (int i = 5; i < 8; i++) { + if (!isnan(gxyzs[i - 5])) + transfer_data.i[i] = gxyzs[i - 5]; + else { + pc.printf("gxyzs is NOT transferred\n"); + transfer_data.i[i] = 0; + } + } + + for (int i = 0; i < N_EEPROM; i++) { + pc.printf("transfer_data[%d]: %\d\r\n", i, transfer_data.i[i]); + } + writeEEPROM(address, pointeraddress, transfer_data.c, N_EEPROM*4); + wait(3); + + U read_test; + readEEPROM(address, pointeraddress, read_test.c, N_EEPROM*4); + wait(3); + for (int i = 0 ; i < N_EEPROM; i ++){ + pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]); + } + + float pitch_align = 0.0*3.141562/180.0 + float roll_align = 0.0*3.141562/180.0 + + for (int i =1 ; i<1000; i++){ + float tstart = t.read(); + //姿勢角を更新 + getIMUval(); + updateBetweenMeasures(); + updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); + updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); + computeAngles(); + + float tend = t.read(); + att_dt = (tend-tstart); + } + + while(1) {wait(1000);} } } \ No newline at end of file