Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
setup.cpp
- Committer:
- cocorlow
- Date:
- 2021-05-31
- Revision:
- 56:888379912f81
- Child:
- 58:a7947322db87
File content as of revision 56:888379912f81:
#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); } }