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:
- cocorlow
- Date:
- 2021-05-31
- Revision:
- 56:888379912f81
- Child:
- 61:c05353850017
File content as of revision 56:888379912f81:
#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[5]); agoffset[4] = float(read_calib.i[6]); agoffset[5] = float(read_calib.i[7]); magbias[0] = float(read_calib.i[1])/1000.0f; magbias[1] = float(read_calib.i[2])/1000.0f; magbias[2] = float(read_calib.i[3])/1000.0f; magbias[3] = float(read_calib.i[4])/1000.0f; rpy_align.y = float(read_calib.i[8])/200000.0f; rpy_align.x = float(read_calib.i[9])/200000.0f; // 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 sumLPaccnorm = 0; for(int i = 0; i < 1000; i++){ getIMUval(); val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm()); sumLPaccnorm += LPacc.Norm(); } accref.z = -sumLPaccnorm / 1000; val_thmg /= 1000; 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); } }