solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: setup.cpp
- Revision:
- 56:888379912f81
- Child:
- 58:a7947322db87
diff -r b11c9fef094a -r 888379912f81 setup.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/setup.cpp Mon May 31 18:59:36 2021 +0000 @@ -0,0 +1,213 @@ +#include "global.hpp" + +void setup() +{ + pitchPID.setSetPoint(0.0); + pitchratePID.setSetPoint(0.0); + pitchPID.setBias(0.0); + pitchratePID.setBias(0.0); + pitchPID.setOutputLimits(-1.0,1.0); + pitchratePID.setOutputLimits(-1.0,1.0); + pitchPID.setInputLimits(-M_PI, M_PI); + pitchratePID.setInputLimits(-M_PI, M_PI); + + servo.period_us(15000.0); + servo.pulsewidth_us(1500.0); + + accelgyro.initialize(); + //加速度計のフルスケールレンジを設定 + accelgyro.setFullScaleAccelRange(ACCEL_FSR); + //角速度計のフルスケールレンジを設定 + accelgyro.setFullScaleGyroRange(GYRO_FSR); + //MPU6050のLPFを設定 + accelgyro.setDLPFMode(MPU6050_LPF); + //地磁気 + mag_sensor.enable(); +} + +void calibrate() +{ + 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_sensor.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.000001f; + 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 = 0; + break; + }; + pc.printf("Center\r\n"); + wait(2); + if(userButton.read() == 0) + { + pos_tail = 1; + break; + }; + pc.printf("Right\r\n"); + wait(2); + if(userButton.read() == 0) + { + pos_tail = 2; + break; + }; + }; + pc.printf("tail_address : %d\r\n", tail_address[pos_tail]); + switch(pos_tail) + { + case 0: + pc.printf("This MBED is Located at Left \r\n"); + break; + case 1: + pc.printf("This MBED is Located at Center \r\n"); + break; + case 2: + pc.printf("This MBED is Located at Right \r\n"); + break; + default: // error situation + pc.printf("error\r\n"); + break; + } + + //姿勢オフセットを計算 + rpy_align.y = 0.0f*M_PI/180.0f; + rpy_align.x = 0.0f*M_PI/180.0f; + float ave_pitch = 0.0f; + float ave_roll = 0.0f; + Timer _t; + _t.start(); + for (int i = 0 ; i < 2200; i++) + { + float tstart = _t.read(); + //姿勢角を更新 + getIMUval(); + ekf.updateBetweenMeasures(gyro, att_dt); + ekf.updateAcrossMeasures(LPmag/LPmag.Norm(), magref/magref.Norm(), ekf.Rmag); + ekf.updateAcrossMeasures(LPacc/LPacc.Norm(), accref/accref.Norm(), ekf.Racc); + ekf.computeAngles(rpy, rpy_g, rpy_align); + if(i>199) + { + ave_pitch += rpy.y; + ave_roll += rpy.x; + } + wait(0.001); + float tend = _t.read(); + att_dt = (tend-tstart); + } + + pc.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI); + 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; + } + } + + // ave_pitch,ave_rollを送る + int ave_pr[2] = {ave_pitch*100, ave_roll*100}; + for (int i = 8; i < 10; i++) + { + if (!isnan(ave_pr[i - 8])) + transfer_data.i[i] = ave_pr[i - 8]; + else + { + pc.printf("alignment data is NOT transferred\n"); + transfer_data.i[i] = 0; + } + } +// transfer_data.i[10] = tail_address[pos_tail]; + for (int i = 0; i < N_EEPROM; i++) + { + pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]); + } + writeEEPROM(eeprom_address, eeprom_pointeraddress, transfer_data.c, N_EEPROM*4); + wait(3); + + U read_test; + readEEPROM(eeprom_address, eeprom_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]); + } + + while(1) + { + wait(1000); + } +} \ No newline at end of file