solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 56:888379912f81
- Child:
- 61:c05353850017
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/run.cpp Mon May 31 18:59:36 2021 +0000 @@ -0,0 +1,79 @@ +#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); + } +} \ No newline at end of file