solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
run.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-06-03
- Revision:
- 61:c05353850017
- Parent:
- 56:888379912f81
- Child:
- 66:e5afad70fdd8
File content as of revision 61:c05353850017:
#include "global.hpp" void run() { pc.printf("reading calibration value\r\n"); //キャリブレーション値を取得 U read_calib; readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4); wait(3); pos_tail = (int)read_calib.i[0]; agoffset[3] = float(read_calib.i[7]); agoffset[4] = float(read_calib.i[8]); agoffset[5] = float(read_calib.i[9]); magbiasMin[0] = float(read_calib.i[1])/1000.0f; magbiasMin[1] = float(read_calib.i[2])/1000.0f; magbiasMin[2] = float(read_calib.i[3])/1000.0f; magbiasMax[0] = float(read_calib.i[4])/1000.0f; magbiasMax[1] = float(read_calib.i[5])/1000.0f; magbiasMax[2] = float(read_calib.i[6])/1000.0f; rpy_align.y = float(read_calib.i[10])/200000.0f; rpy_align.x = float(read_calib.i[11])/200000.0f; magCalibrator.setExtremes(magbiasMin,magbiasMax); // tail_address[pos_tail] = (int)read_calib.i[10]; 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; } pc.printf("tail_address : %d\r\n", tail_address[pos_tail]); pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.y*180.0f/M_PI,rpy_align.x*180.0f/M_PI); getIMUval(); ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm()); float val2_thmg = 0; float val2_accnorm = 0; float sumLPaccnorm = 0; for(int i = 0; i < 1000; i++){ getIMUval(); val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm()); val2_thmg += (acos((mag % acc)/mag.Norm()/acc.Norm()))*(acos((mag % acc)/mag.Norm()/acc.Norm())); sumLPaccnorm += LPacc.Norm(); val2_accnorm += LPacc.Norm()*LPacc.Norm(); } accref.z = -sumLPaccnorm / 1000; val_thmg /= 1000; sigma_thmg = sqrt(val2_thmg/1000-val_thmg*val_thmg); sigma_accnorm = sqrt(val2_accnorm/1000-accref.z*accref.z); pc.printf("sigma: %f %f \r\n",sigma_thmg,sigma_accnorm); for (int i = 0; i < 3; i++) { if (i == pos_tail) { break; } else { tail.Subscribe(tail_address[i], &(posValues[i])); } } LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); Timer _t; _t.start(); while(1) { float tstart = _t.read(); //姿勢角を更新 getIMUval(); ekf.updateBetweenMeasures(gyro, att_dt); ekf.computeAngles(rpy, rpy_g, rpy_align); PIDtick.loop(); float tend = _t.read(); att_dt = (tend-tstart); } }