HAPSRG / Mbed 2 deprecated HAPStail

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MatrixMath LPS25HB_I2C LSM9DS1 Matrix2 PIDcontroller LoopTicker SBUS_without_mainfile UsaPack solaESKF_wind Vector3 CalibrateMagneto FastPWM

run.cpp@71:62eb45ecffe9, 2021-08-20 (annotated)

Committer:
NaotoMorita
Date:
Fri Aug 20 07:48:41 2021 +0000
Revision:
71:62eb45ecffe9
Parent:
70:9e7be21475f8
Child:
73:be7a8b8188de
20210819

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];
NaotoMorita 61:c05353850017 11 agoffset[3] = float(read_calib.i[7]);
NaotoMorita 61:c05353850017 12 agoffset[4] = float(read_calib.i[8]);
NaotoMorita 61:c05353850017 13 agoffset[5] = float(read_calib.i[9]);
NaotoMorita 61:c05353850017 14 magbiasMin[0] = float(read_calib.i[1])/1000.0f;
NaotoMorita 61:c05353850017 15 magbiasMin[1] = float(read_calib.i[2])/1000.0f;
NaotoMorita 61:c05353850017 16 magbiasMin[2] = float(read_calib.i[3])/1000.0f;
NaotoMorita 61:c05353850017 17 magbiasMax[0] = float(read_calib.i[4])/1000.0f;
NaotoMorita 61:c05353850017 18 magbiasMax[1] = float(read_calib.i[5])/1000.0f;
NaotoMorita 61:c05353850017 19 magbiasMax[2] = float(read_calib.i[6])/1000.0f;
NaotoMorita 71:62eb45ecffe9 20 rpy_align.x = float(read_calib.i[10])/200000.0f;
NaotoMorita 71:62eb45ecffe9 21 rpy_align.y = float(read_calib.i[11])/200000.0f;
NaotoMorita 61:c05353850017 22 magCalibrator.setExtremes(magbiasMin,magbiasMax);
cocorlow 56:888379912f81 23 // tail_address[pos_tail] = (int)read_calib.i[10];
cocorlow 56:888379912f81 24
cocorlow 56:888379912f81 25 switch(pos_tail){
cocorlow 56:888379912f81 26 case 0:
cocorlow 56:888379912f81 27 pc.printf("This MBED is Located at Left \r\n");
cocorlow 56:888379912f81 28 break;
cocorlow 56:888379912f81 29 case 1:
cocorlow 56:888379912f81 30 pc.printf("This MBED is Located at Center \r\n");
cocorlow 56:888379912f81 31 break;
cocorlow 56:888379912f81 32 case 2:
cocorlow 56:888379912f81 33 pc.printf("This MBED is Located at Right \r\n");
cocorlow 56:888379912f81 34 break;
cocorlow 56:888379912f81 35 default: // error situation
cocorlow 56:888379912f81 36 pc.printf("error\r\n");
cocorlow 56:888379912f81 37 break;
cocorlow 56:888379912f81 38 }
cocorlow 56:888379912f81 39 pc.printf("tail_address : %d\r\n", tail_address[pos_tail]);
NaotoMorita 71:62eb45ecffe9 40 pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",rpy_align.x*180.0f/M_PI,rpy_align.y*180.0f/M_PI);
NaotoMorita 69:0caaad87cf1d 41
NaotoMorita 71:62eb45ecffe9 42 wait(0.5);
NaotoMorita 69:0caaad87cf1d 43 Timer _t;
NaotoMorita 69:0caaad87cf1d 44 _t.start();
NaotoMorita 71:62eb45ecffe9 45 magCalibrator.setExtremes(magbiasMin,magbiasMax);
NaotoMorita 71:62eb45ecffe9 46 for(int i = 0; i < 1000; i++){
NaotoMorita 71:62eb45ecffe9 47 getIMUval();
NaotoMorita 71:62eb45ecffe9 48 }
NaotoMorita 71:62eb45ecffe9 49 ekf.defineQhat(rpy_align);
NaotoMorita 71:62eb45ecffe9 50 float sum2accnorm = 0;
NaotoMorita 71:62eb45ecffe9 51 float sumaccnorm = 0;
cocorlow 56:888379912f81 52 for(int i = 0; i < 1000; i++){
NaotoMorita 69:0caaad87cf1d 53 float tstart = _t.read();
cocorlow 56:888379912f81 54 getIMUval();
NaotoMorita 69:0caaad87cf1d 55 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 71:62eb45ecffe9 56 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 71:62eb45ecffe9 57 ekf.fuseErr2Nominal();
NaotoMorita 71:62eb45ecffe9 58 ekf.resetBias();
NaotoMorita 71:62eb45ecffe9 59 //ekf.updateMagMeasures(mag);
NaotoMorita 69:0caaad87cf1d 60 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 69:0caaad87cf1d 61 sumaccnorm += acc.Norm();
NaotoMorita 69:0caaad87cf1d 62 sum2accnorm += acc.Norm()*acc.Norm();
NaotoMorita 69:0caaad87cf1d 63 float tend = _t.read();
NaotoMorita 69:0caaad87cf1d 64 att_dt = (tend-tstart);
cocorlow 56:888379912f81 65 }
NaotoMorita 69:0caaad87cf1d 66 accref.z = sumaccnorm / 1000.0f;
NaotoMorita 71:62eb45ecffe9 67
cocorlow 56:888379912f81 68 for (int i = 0; i < 3; i++)
cocorlow 56:888379912f81 69 {
cocorlow 56:888379912f81 70 if (i == pos_tail)
cocorlow 56:888379912f81 71 {
cocorlow 63:851e96f54a86 72 continue;
cocorlow 56:888379912f81 73 }
cocorlow 56:888379912f81 74 else
cocorlow 56:888379912f81 75 {
cocorlow 56:888379912f81 76 tail.Subscribe(tail_address[i], &(posValues[i]));
cocorlow 56:888379912f81 77 }
cocorlow 56:888379912f81 78 }
cocorlow 63:851e96f54a86 79 tail.Subscribe(time_address, &broadcast_time);
cocorlow 56:888379912f81 80
cocorlow 56:888379912f81 81 LoopTicker PIDtick;
cocorlow 56:888379912f81 82 PIDtick.attach(calcServoOut,PID_dt);
cocorlow 56:888379912f81 83
cocorlow 56:888379912f81 84
cocorlow 56:888379912f81 85 while(1)
cocorlow 56:888379912f81 86 {
cocorlow 56:888379912f81 87 float tstart = _t.read();
cocorlow 56:888379912f81 88 //姿勢角を更新
cocorlow 56:888379912f81 89 getIMUval();
NaotoMorita 71:62eb45ecffe9 90 ekf.updateNominal(gyro,acc,accref,att_dt);
NaotoMorita 71:62eb45ecffe9 91 ekf.updateErrState(gyro,acc, att_dt);
NaotoMorita 71:62eb45ecffe9 92 if(obsCount == 50){
NaotoMorita 71:62eb45ecffe9 93 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 71:62eb45ecffe9 94 ekf.updateMeasures(gyro,acc,accref);
NaotoMorita 71:62eb45ecffe9 95 }else{
NaotoMorita 71:62eb45ecffe9 96 ekf.updateStaticMeasures(gyro,acc,accref);
NaotoMorita 71:62eb45ecffe9 97 }
NaotoMorita 71:62eb45ecffe9 98 obsCount = 0;
NaotoMorita 69:0caaad87cf1d 99 }else{
NaotoMorita 71:62eb45ecffe9 100 if(ekf.determinDynStatus(acc,accref)){
NaotoMorita 71:62eb45ecffe9 101 ekf.updateAccMeasures(acc,accref);
NaotoMorita 71:62eb45ecffe9 102 }else{
NaotoMorita 71:62eb45ecffe9 103 ekf.updateStaticAccMeasures(acc,accref);
NaotoMorita 71:62eb45ecffe9 104 }
NaotoMorita 71:62eb45ecffe9 105 obsCount += 1;
NaotoMorita 69:0caaad87cf1d 106 }
NaotoMorita 71:62eb45ecffe9 107
NaotoMorita 71:62eb45ecffe9 108 ekf.updateGyroBiasConstraints(gyro);
NaotoMorita 71:62eb45ecffe9 109 ekf.fuseErr2Nominal();
NaotoMorita 69:0caaad87cf1d 110 ekf.resetBias();
NaotoMorita 69:0caaad87cf1d 111 ekf.computeAngles(rpy, rpy_align);
NaotoMorita 71:62eb45ecffe9 112 ekf.computeVb(vb);
cocorlow 56:888379912f81 113 PIDtick.loop();
cocorlow 56:888379912f81 114
cocorlow 56:888379912f81 115 float tend = _t.read();
cocorlow 56:888379912f81 116 att_dt = (tend-tstart);
cocorlow 56:888379912f81 117 }
cocorlow 56:888379912f81 118 }