HAPSRG / Mbed 2 deprecated optWingforHAPS_Eigen

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

run.cpp@56:888379912f81, 2021-05-31 (annotated)

Committer:
cocorlow
Date:
Mon May 31 18:59:36 2021 +0000
Revision:
56:888379912f81
Child:
61:c05353850017
file divided

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cocorlow 56:888379912f81 1 #include "global.hpp"
cocorlow 56:888379912f81 2
cocorlow 56:888379912f81 3 void run()
cocorlow 56:888379912f81 4 {
cocorlow 56:888379912f81 5 pc.printf("reading calibration value\r\n");
cocorlow 56:888379912f81 6 //キャリブレーション値を取得
cocorlow 56:888379912f81 7 U read_calib;
cocorlow 56:888379912f81 8 readEEPROM(eeprom_address, eeprom_pointeraddress, read_calib.c, N_EEPROM*4);
cocorlow 56:888379912f81 9 wait(3);
cocorlow 56:888379912f81 10 pos_tail = (int)read_calib.i[0];
cocorlow 56:888379912f81 11 agoffset[3] = float(read_calib.i[5]);
cocorlow 56:888379912f81 12 agoffset[4] = float(read_calib.i[6]);
cocorlow 56:888379912f81 13 agoffset[5] = float(read_calib.i[7]);
cocorlow 56:888379912f81 14 magbias[0] = float(read_calib.i[1])/1000.0f;
cocorlow 56:888379912f81 15 magbias[1] = float(read_calib.i[2])/1000.0f;
cocorlow 56:888379912f81 16 magbias[2] = float(read_calib.i[3])/1000.0f;
cocorlow 56:888379912f81 17 magbias[3] = float(read_calib.i[4])/1000.0f;
cocorlow 56:888379912f81 18 rpy_align.y = float(read_calib.i[8])/200000.0f;
cocorlow 56:888379912f81 19 rpy_align.x = float(read_calib.i[9])/200000.0f;
cocorlow 56:888379912f81 20 // tail_address[pos_tail] = (int)read_calib.i[10];
cocorlow 56:888379912f81 21
cocorlow 56:888379912f81 22 switch(pos_tail){
cocorlow 56:888379912f81 23 case 0:
cocorlow 56:888379912f81 24 pc.printf("This MBED is Located at Left \r\n");
cocorlow 56:888379912f81 25 break;
cocorlow 56:888379912f81 26 case 1:
cocorlow 56:888379912f81 27 pc.printf("This MBED is Located at Center \r\n");
cocorlow 56:888379912f81 28 break;
cocorlow 56:888379912f81 29 case 2:
cocorlow 56:888379912f81 30 pc.printf("This MBED is Located at Right \r\n");
cocorlow 56:888379912f81 31 break;
cocorlow 56:888379912f81 32 default: // error situation
cocorlow 56:888379912f81 33 pc.printf("error\r\n");
cocorlow 56:888379912f81 34 break;
cocorlow 56:888379912f81 35 }
cocorlow 56:888379912f81 36 pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
cocorlow 56:888379912f81 37 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);
cocorlow 56:888379912f81 38 getIMUval();
cocorlow 56:888379912f81 39 ekf.triad(acc/acc.Norm(), accref/accref.Norm(), mag/mag.Norm(), magref/magref.Norm());
cocorlow 56:888379912f81 40 float sumLPaccnorm = 0;
cocorlow 56:888379912f81 41 for(int i = 0; i < 1000; i++){
cocorlow 56:888379912f81 42 getIMUval();
cocorlow 56:888379912f81 43 val_thmg += acos((mag % acc)/mag.Norm()/acc.Norm());
cocorlow 56:888379912f81 44 sumLPaccnorm += LPacc.Norm();
cocorlow 56:888379912f81 45 }
cocorlow 56:888379912f81 46 accref.z = -sumLPaccnorm / 1000;
cocorlow 56:888379912f81 47 val_thmg /= 1000;
cocorlow 56:888379912f81 48
cocorlow 56:888379912f81 49 for (int i = 0; i < 3; i++)
cocorlow 56:888379912f81 50 {
cocorlow 56:888379912f81 51 if (i == pos_tail)
cocorlow 56:888379912f81 52 {
cocorlow 56:888379912f81 53 break;
cocorlow 56:888379912f81 54 }
cocorlow 56:888379912f81 55 else
cocorlow 56:888379912f81 56 {
cocorlow 56:888379912f81 57 tail.Subscribe(tail_address[i], &(posValues[i]));
cocorlow 56:888379912f81 58 }
cocorlow 56:888379912f81 59 }
cocorlow 56:888379912f81 60
cocorlow 56:888379912f81 61 LoopTicker PIDtick;
cocorlow 56:888379912f81 62 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 63
cocorlow 56:888379912f81 64 Timer _t;
cocorlow 56:888379912f81 65 _t.start();
cocorlow 56:888379912f81 66
cocorlow 56:888379912f81 67 while(1)
cocorlow 56:888379912f81 68 {
cocorlow 56:888379912f81 69 float tstart = _t.read();
cocorlow 56:888379912f81 70 //姿勢角を更新
cocorlow 56:888379912f81 71 getIMUval();
cocorlow 56:888379912f81 72 ekf.updateBetweenMeasures(gyro, att_dt);
cocorlow 56:888379912f81 73 ekf.computeAngles(rpy, rpy_g, rpy_align);
cocorlow 56:888379912f81 74 PIDtick.loop();
cocorlow 56:888379912f81 75
cocorlow 56:888379912f81 76 float tend = _t.read();
cocorlow 56:888379912f81 77 att_dt = (tend-tstart);
cocorlow 56:888379912f81 78 }
cocorlow 56:888379912f81 79 }