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
Diff: run.cpp
- Revision:
- 66:e5afad70fdd8
- Parent:
- 61:c05353850017
- Child:
- 68:b9f6938fab9d
diff -r ea184054e659 -r e5afad70fdd8 run.cpp --- a/run.cpp Tue Jun 22 02:08:31 2021 +0000 +++ b/run.cpp Tue Jun 22 02:19:14 2021 +0000 @@ -2,70 +2,18 @@ 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; + float sum2accnorm = 0; + float sumaccnorm = 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(); + sumaccnorm += acc.Norm(); + sum2accnorm += acc.Norm()*acc.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])); - } - } + accref.z = -sumaccnorm / 1000.0f; + float sigma_accnorm = sqrt(sum2accnorm/1000.0f-accref.z*accref.z); + pc.Subscribe(0000, &(posValues)); LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); @@ -78,8 +26,8 @@ float tstart = _t.read(); //姿勢角を更新 getIMUval(); - ekf.updateBetweenMeasures(gyro, att_dt); - ekf.computeAngles(rpy, rpy_g, rpy_align); + //ekf.updateBetweenMeasures(gyro, att_dt); + //ekf.computeAngles(rpy, rpy_g, rpy_align); PIDtick.loop(); float tend = _t.read();