HAPSRG / Mbed 2 deprecated hexaTest_Eigen

solaESKF_EIGEN

Dependencies:   mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM

main.cpp@53:3a6dfb55e087, 2021-05-24 (annotated)

Committer:
cocorlow
Date:
Mon May 24 05:45:56 2021 +0000
Revision:
53:3a6dfb55e087
Parent:
52:abec61ea3cd3
Child:
54:ca88777b295a
broadcast

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 0:6b18a09a6628 1 #include "mbed.h"
naker 20:2c3f113a8e8f 2 #include "PIDcontroller.h"
NaotoMorita 0:6b18a09a6628 3 #include "SBUS.hpp"
cocorlow 6:2cba569272fe 4 #include "LoopTicker.hpp"
naker 15:6abaac6e5b03 5 #include "MPU6050.h"
NaotoMorita 38:acc7cdcf56dd 6 #include "MAG3110.h"
NaotoMorita 38:acc7cdcf56dd 7 #include "I2Cdev.h"
NaotoMorita 36:e68ce293306e 8 #include "FastPWM.h"
NaotoMorita 36:e68ce293306e 9 #include "Matrix.h"
NaotoMorita 36:e68ce293306e 10 #include "MatrixMath.h"
NaotoMorita 38:acc7cdcf56dd 11 #include "math.h"
cocorlow 53:3a6dfb55e087 12 #include "UsaPack.hpp"
cocorlow 6:2cba569272fe 13
NaotoMorita 17:6b7a363d06d2 14 #define MPU6050_PWR_MGMT_1 0x6B
NaotoMorita 17:6b7a363d06d2 15 #define MPU_ADDRESS 0x68
NaotoMorita 17:6b7a363d06d2 16 #define pi 3.141562
NaotoMorita 17:6b7a363d06d2 17 #define ACCEL_FSR MPU6050_ACCEL_FS_8
NaotoMorita 38:acc7cdcf56dd 18 #define ACCEL_SSF 4096.0
NaotoMorita 17:6b7a363d06d2 19 #define GYRO_FSR MPU6050_GYRO_FS_250
NaotoMorita 17:6b7a363d06d2 20 #define GYRO_SSF 131.0
NaotoMorita 17:6b7a363d06d2 21 #define MPU6050_LPF MPU6050_DLPF_BW_256
NaotoMorita 38:acc7cdcf56dd 22 #define gyro_th 20.0
NaotoMorita 38:acc7cdcf56dd 23 #define PID_dt 0.015
NaotoMorita 38:acc7cdcf56dd 24 #define servoPwmMax 1800.0
NaotoMorita 38:acc7cdcf56dd 25 #define servoPwmMin 1200.0
NaotoMorita 38:acc7cdcf56dd 26 #define motorPwmMax 2000.0
NaotoMorita 38:acc7cdcf56dd 27 #define motorPwmMin 1100.0
NaotoMorita 49:73fe59148dd4 28
cocorlow 53:3a6dfb55e087 29 #define N_EEPROM 11
NaotoMorita 27:43bd0e444633 30
NaotoMorita 17:6b7a363d06d2 31 MPU6050 accelgyro;
NaotoMorita 38:acc7cdcf56dd 32 MAG3110 mag(PB_9,PB_8);
NaotoMorita 0:6b18a09a6628 33 SBUS sbus(PD_5, PD_6);
cocorlow 53:3a6dfb55e087 34 Serial pc(USBTX, USBRX, 115200);
NaotoMorita 38:acc7cdcf56dd 35 DigitalIn userButton(USER_BUTTON);
NaotoMorita 49:73fe59148dd4 36 DigitalIn locdef1(PG_2);
NaotoMorita 49:73fe59148dd4 37 DigitalIn locdef2(PG_3);
NaotoMorita 48:1065506191f2 38 FastPWM servo(PE_9);
NaotoMorita 36:e68ce293306e 39 PID pitchPID(5.0, 0,0,PID_dt); //rad
NaotoMorita 38:acc7cdcf56dd 40 PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 17:6b7a363d06d2 41 Timer t;
cocorlow 3:79e62f9b13c8 42
NaotoMorita 36:e68ce293306e 43 Matrix qhat(4,1);
NaotoMorita 44:f571273d3223 44 Matrix qhat_gyro(4,1);
NaotoMorita 36:e68ce293306e 45 Matrix Phat(4,4);
NaotoMorita 37:dad55cf05e3d 46 Matrix Qgyro(3,3);
NaotoMorita 36:e68ce293306e 47 Matrix Racc(3,3);
NaotoMorita 38:acc7cdcf56dd 48 Matrix Rmag(3,3);
NaotoMorita 38:acc7cdcf56dd 49 Matrix D(3,3);
NaotoMorita 36:e68ce293306e 50
NaotoMorita 38:acc7cdcf56dd 51 int loop_count = 0;
NaotoMorita 40:869f3791a3e2 52 int obs_count = 0;
NaotoMorita 36:e68ce293306e 53 float att_dt = 0.01;
naker 23:4a490fa8bf4a 54 float rc[16];
NaotoMorita 38:acc7cdcf56dd 55 float pitch = 0.0;
NaotoMorita 38:acc7cdcf56dd 56 float roll = 0.0;
NaotoMorita 38:acc7cdcf56dd 57 float yaw = 0.0;
NaotoMorita 44:f571273d3223 58
NaotoMorita 44:f571273d3223 59 float pitch_g = 0.0;
NaotoMorita 44:f571273d3223 60 float roll_g = 0.0;
NaotoMorita 44:f571273d3223 61 float yaw_g = 0.0;
NaotoMorita 44:f571273d3223 62
NaotoMorita 17:6b7a363d06d2 63 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 64 int16_t gx, gy, gz;
NaotoMorita 38:acc7cdcf56dd 65 MotionSensorDataUnits mdata;
NaotoMorita 38:acc7cdcf56dd 66 float magval[3] = {1.0,0.0,0.0};
NaotoMorita 38:acc7cdcf56dd 67 float acc_x,acc_y,acc_z;
NaotoMorita 38:acc7cdcf56dd 68 float dynacc_x,dynacc_y,dynacc_z;
NaotoMorita 38:acc7cdcf56dd 69 float gyro_x,gyro_y,gyro_z;
NaotoMorita 38:acc7cdcf56dd 70 float mag_x,mag_y,mag_z;
NaotoMorita 40:869f3791a3e2 71
NaotoMorita 40:869f3791a3e2 72 float LPacc_x=0.0;
NaotoMorita 40:869f3791a3e2 73 float LPacc_y=0.0;
NaotoMorita 40:869f3791a3e2 74 float LPacc_z=0.0;
NaotoMorita 40:869f3791a3e2 75 float LPmag_x = 0.0;
NaotoMorita 40:869f3791a3e2 76 float LPmag_y = 0.0;
NaotoMorita 40:869f3791a3e2 77 float LPmag_z = 0.0;;
NaotoMorita 40:869f3791a3e2 78
cocorlow 3:79e62f9b13c8 79 int out1, out2;
NaotoMorita 48:1065506191f2 80 float scaledServoOut[1]= {0};
NaotoMorita 48:1065506191f2 81 float servoOut[1] = {1500.0};
NaotoMorita 38:acc7cdcf56dd 82
NaotoMorita 38:acc7cdcf56dd 83 float accnorm;
NaotoMorita 38:acc7cdcf56dd 84 float magnorm;
NaotoMorita 40:869f3791a3e2 85 float LPaccnorm;
NaotoMorita 40:869f3791a3e2 86 float LPmagnorm;
NaotoMorita 38:acc7cdcf56dd 87 float accrefnorm;
NaotoMorita 38:acc7cdcf56dd 88 float magrefnorm;
NaotoMorita 38:acc7cdcf56dd 89
NaotoMorita 40:869f3791a3e2 90
NaotoMorita 38:acc7cdcf56dd 91 float val_thmg = 0.0;
NaotoMorita 40:869f3791a3e2 92 float th_mg = 0.0;
NaotoMorita 40:869f3791a3e2 93 float dynaccnorm = 0.0;
NaotoMorita 40:869f3791a3e2 94 float accnormerr = 0.0;
NaotoMorita 38:acc7cdcf56dd 95 float accref[3] = {0, 0, -0.98};
NaotoMorita 38:acc7cdcf56dd 96 float magref[3] = {0.65, 0, 0.75};
NaotoMorita 38:acc7cdcf56dd 97
NaotoMorita 49:73fe59148dd4 98 int calibrationFlag = 0;
NaotoMorita 49:73fe59148dd4 99 int pos_tail = 1;//1:left 2:center 3:right
NaotoMorita 49:73fe59148dd4 100 int agoffset[6] = {0, 0, 0, 386, -450, 48};
NaotoMorita 49:73fe59148dd4 101 float magbias[4] = {-215.833374, 207.740616, -167.444565, 36.688690};
NaotoMorita 52:abec61ea3cd3 102 float pitch_align = 0.0*3.141562/180.0;
NaotoMorita 52:abec61ea3cd3 103 float roll_align = 0.0*3.141562/180.0;
NaotoMorita 49:73fe59148dd4 104 // eepromのread writeのためのunionを定義
NaotoMorita 49:73fe59148dd4 105 int address = 0xAE; // EEPROMの3つの入力が全て+より
NaotoMorita 49:73fe59148dd4 106 int pointeraddress = 0;
NaotoMorita 49:73fe59148dd4 107
NaotoMorita 49:73fe59148dd4 108 I2C i2c(PB_9,PB_8); // sda, scl
NaotoMorita 49:73fe59148dd4 109 union U{
NaotoMorita 49:73fe59148dd4 110 int i[N_EEPROM]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
NaotoMorita 49:73fe59148dd4 111 char c[N_EEPROM*4]; // floatはcharの4倍
NaotoMorita 49:73fe59148dd4 112 };
cocorlow 3:79e62f9b13c8 113
cocorlow 53:3a6dfb55e087 114 // UsaPack
cocorlow 53:3a6dfb55e087 115 UsaPack tail(PG_14, PG_9, 115200);
cocorlow 53:3a6dfb55e087 116 int tail_address = 0;
cocorlow 53:3a6dfb55e087 117 struct valuePack
cocorlow 53:3a6dfb55e087 118 {
cocorlow 53:3a6dfb55e087 119 float dt;
cocorlow 53:3a6dfb55e087 120 int count;
cocorlow 53:3a6dfb55e087 121 float acc[3];
cocorlow 53:3a6dfb55e087 122 float gyro[3];
cocorlow 53:3a6dfb55e087 123 float mag[3];
cocorlow 53:3a6dfb55e087 124 float rot[3];
cocorlow 53:3a6dfb55e087 125 float rot_g[3];
cocorlow 53:3a6dfb55e087 126 };
cocorlow 53:3a6dfb55e087 127 valuePack vp;
cocorlow 53:3a6dfb55e087 128
cocorlow 53:3a6dfb55e087 129
NaotoMorita 38:acc7cdcf56dd 130 void writeSdcard()
NaotoMorita 38:acc7cdcf56dd 131 {
NaotoMorita 38:acc7cdcf56dd 132 //magcal.getExtremes(&magmin[0],&magmax[0]);
NaotoMorita 38:acc7cdcf56dd 133 //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]);
NaotoMorita 38:acc7cdcf56dd 134 //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]);
cocorlow 53:3a6dfb55e087 135 // sd.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
NaotoMorita 44:f571273d3223 136 //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
NaotoMorita 44:f571273d3223 137 //pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
cocorlow 53:3a6dfb55e087 138 //pc.printf("%d \r\n",userButton.read());
cocorlow 53:3a6dfb55e087 139 //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
cocorlow 53:3a6dfb55e087 140 //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
NaotoMorita 38:acc7cdcf56dd 141 //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
NaotoMorita 44:f571273d3223 142 //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z);
NaotoMorita 38:acc7cdcf56dd 143 //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z);
cocorlow 53:3a6dfb55e087 144
cocorlow 53:3a6dfb55e087 145 vp.dt = 1.0f/att_dt;
cocorlow 53:3a6dfb55e087 146 vp.count = obs_count;
cocorlow 53:3a6dfb55e087 147 vp.acc[0] = acc_x;
cocorlow 53:3a6dfb55e087 148 vp.acc[1] = acc_y;
cocorlow 53:3a6dfb55e087 149 vp.acc[2] = acc_z;
cocorlow 53:3a6dfb55e087 150 vp.gyro[0] = gyro_x;
cocorlow 53:3a6dfb55e087 151 vp.gyro[1] = gyro_y;
cocorlow 53:3a6dfb55e087 152 vp.gyro[2] = gyro_z;
cocorlow 53:3a6dfb55e087 153 vp.mag[0] = mag_x;
cocorlow 53:3a6dfb55e087 154 vp.mag[1] = mag_y;
cocorlow 53:3a6dfb55e087 155 vp.mag[2] = mag_z;
cocorlow 53:3a6dfb55e087 156 vp.rot[0] = roll*180.0f/pi;
cocorlow 53:3a6dfb55e087 157 vp.rot[1] = pitch*180.0f/pi;
cocorlow 53:3a6dfb55e087 158 vp.rot[2] = yaw*180.0f/pi;
cocorlow 53:3a6dfb55e087 159 vp.rot_g[0] = roll_g*180.0f/pi;
cocorlow 53:3a6dfb55e087 160 vp.rot_g[1] = pitch_g*180.0f/pi;
cocorlow 53:3a6dfb55e087 161 vp.rot_g[2] = yaw_g*180.0f/pi;
cocorlow 53:3a6dfb55e087 162 tail.Send(tail_address, &vp);
cocorlow 53:3a6dfb55e087 163 // pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi);
cocorlow 53:3a6dfb55e087 164
NaotoMorita 38:acc7cdcf56dd 165 }
NaotoMorita 27:43bd0e444633 166
NaotoMorita 49:73fe59148dd4 167 // eeprom書き込み・読み込みに必要な関数
NaotoMorita 49:73fe59148dd4 168 void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 49:73fe59148dd4 169 {
NaotoMorita 49:73fe59148dd4 170 char i2cBuffer[size + 2];
NaotoMorita 49:73fe59148dd4 171 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 49:73fe59148dd4 172 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 49:73fe59148dd4 173
NaotoMorita 49:73fe59148dd4 174 for (int i = 0; i < size; i++) {
NaotoMorita 49:73fe59148dd4 175 i2cBuffer[i + 2] = data[i];
NaotoMorita 49:73fe59148dd4 176 }
NaotoMorita 49:73fe59148dd4 177
NaotoMorita 49:73fe59148dd4 178 int result = i2c.write(address, i2cBuffer, size + 2, false);
NaotoMorita 49:73fe59148dd4 179 //sleep_ms(500);
NaotoMorita 49:73fe59148dd4 180 }
NaotoMorita 49:73fe59148dd4 181
NaotoMorita 49:73fe59148dd4 182 // this function has no read limit
NaotoMorita 49:73fe59148dd4 183 void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
NaotoMorita 49:73fe59148dd4 184 {
NaotoMorita 49:73fe59148dd4 185 char i2cBuffer[2];
NaotoMorita 49:73fe59148dd4 186 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
NaotoMorita 49:73fe59148dd4 187 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
NaotoMorita 49:73fe59148dd4 188
NaotoMorita 49:73fe59148dd4 189 // Reset eeprom pointer address
NaotoMorita 49:73fe59148dd4 190 int result = i2c.write(address, i2cBuffer, 2, false);
NaotoMorita 49:73fe59148dd4 191
NaotoMorita 49:73fe59148dd4 192 //sleep_ms(500);
NaotoMorita 49:73fe59148dd4 193
NaotoMorita 49:73fe59148dd4 194 // Read eeprom
NaotoMorita 49:73fe59148dd4 195 i2c.read(address, data, size);
NaotoMorita 49:73fe59148dd4 196 //sleep_ms(500);
NaotoMorita 49:73fe59148dd4 197 }
NaotoMorita 49:73fe59148dd4 198
NaotoMorita 35:4535af88f7bf 199 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
cocorlow 7:70161eb0f854 200 {
cocorlow 7:70161eb0f854 201 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 202 }
NaotoMorita 27:43bd0e444633 203
NaotoMorita 38:acc7cdcf56dd 204 void calcMagRef(float mx, float my, float mz){
NaotoMorita 44:f571273d3223 205
NaotoMorita 44:f571273d3223 206 Matrix magvec(3,1);
NaotoMorita 44:f571273d3223 207 magvec << mx << my << mz;
NaotoMorita 44:f571273d3223 208 Matrix magnedvec = MatrixMath::Transpose(D)*magvec;
NaotoMorita 44:f571273d3223 209 magref[0] = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
NaotoMorita 38:acc7cdcf56dd 210 magref[1] = 0.0;
NaotoMorita 44:f571273d3223 211 magref[2] = magnedvec(3,1);
NaotoMorita 38:acc7cdcf56dd 212 }
NaotoMorita 38:acc7cdcf56dd 213
NaotoMorita 38:acc7cdcf56dd 214
NaotoMorita 38:acc7cdcf56dd 215 void getIMUval(){
NaotoMorita 38:acc7cdcf56dd 216 // gx gy gz ax ay az
NaotoMorita 38:acc7cdcf56dd 217 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
NaotoMorita 38:acc7cdcf56dd 218 ax = ax - agoffset[0];
NaotoMorita 38:acc7cdcf56dd 219 ay = ay - agoffset[1];
NaotoMorita 38:acc7cdcf56dd 220 az = -az - agoffset[2];
NaotoMorita 38:acc7cdcf56dd 221 gx = gx - agoffset[3];
NaotoMorita 38:acc7cdcf56dd 222 gy = gy - agoffset[4];
NaotoMorita 38:acc7cdcf56dd 223 gz = -gz - agoffset[5];
NaotoMorita 38:acc7cdcf56dd 224 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 38:acc7cdcf56dd 225 acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 38:acc7cdcf56dd 226 acc_y = float(ay) / ACCEL_SSF;
NaotoMorita 38:acc7cdcf56dd 227 acc_z = float(az) / ACCEL_SSF;
NaotoMorita 38:acc7cdcf56dd 228 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 38:acc7cdcf56dd 229 gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
NaotoMorita 38:acc7cdcf56dd 230 gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
NaotoMorita 38:acc7cdcf56dd 231 gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
NaotoMorita 38:acc7cdcf56dd 232 mag.getAxis(mdata); // flush the magnetmeter
NaotoMorita 38:acc7cdcf56dd 233 magval[0] = (mdata.x - magbias[0]);
NaotoMorita 38:acc7cdcf56dd 234 magval[1] = (mdata.y - magbias[1]);
NaotoMorita 38:acc7cdcf56dd 235 magval[2] = (mdata.z - magbias[2]);
NaotoMorita 38:acc7cdcf56dd 236 mag_x = -magval[0]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 237 mag_y = -magval[1]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 238 mag_z = -magval[2]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 239
NaotoMorita 40:869f3791a3e2 240 float lpc_acc = 0.15;
NaotoMorita 40:869f3791a3e2 241 LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x;
NaotoMorita 40:869f3791a3e2 242 LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y;
NaotoMorita 40:869f3791a3e2 243 LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z;
NaotoMorita 40:869f3791a3e2 244
NaotoMorita 40:869f3791a3e2 245 float lpc_mag = 0.15;
NaotoMorita 40:869f3791a3e2 246 LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x;
NaotoMorita 40:869f3791a3e2 247 LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y;
NaotoMorita 40:869f3791a3e2 248 LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z;
NaotoMorita 40:869f3791a3e2 249
NaotoMorita 38:acc7cdcf56dd 250 accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
NaotoMorita 38:acc7cdcf56dd 251 magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
NaotoMorita 40:869f3791a3e2 252 LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z);
NaotoMorita 40:869f3791a3e2 253 LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z);
NaotoMorita 38:acc7cdcf56dd 254 accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
NaotoMorita 38:acc7cdcf56dd 255 magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
NaotoMorita 40:869f3791a3e2 256 calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm);
NaotoMorita 40:869f3791a3e2 257
NaotoMorita 40:869f3791a3e2 258
NaotoMorita 38:acc7cdcf56dd 259 }
NaotoMorita 38:acc7cdcf56dd 260
NaotoMorita 38:acc7cdcf56dd 261 void updateBetweenMeasures(){
NaotoMorita 40:869f3791a3e2 262 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 40:869f3791a3e2 263 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 40:869f3791a3e2 264 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 40:869f3791a3e2 265 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 40:869f3791a3e2 266
NaotoMorita 40:869f3791a3e2 267 Matrix B(4,3);
NaotoMorita 40:869f3791a3e2 268 B << q1 << q2 << q3
NaotoMorita 40:869f3791a3e2 269 <<-q0 << q3 <<-q2
NaotoMorita 40:869f3791a3e2 270 <<-q3 <<-q0 << q1
NaotoMorita 40:869f3791a3e2 271 << q2 <<-q1 <<-q0;
NaotoMorita 40:869f3791a3e2 272 B *= 0.5f;
NaotoMorita 40:869f3791a3e2 273
NaotoMorita 38:acc7cdcf56dd 274 Matrix phi(4,4);
NaotoMorita 38:acc7cdcf56dd 275 phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 276 << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 277 << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 278 << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
NaotoMorita 44:f571273d3223 279
NaotoMorita 38:acc7cdcf56dd 280 qhat = phi*qhat;
NaotoMorita 38:acc7cdcf56dd 281 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 282 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 283
NaotoMorita 44:f571273d3223 284 qhat_gyro = phi*qhat_gyro;
NaotoMorita 44:f571273d3223 285 qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro));
NaotoMorita 44:f571273d3223 286 qhat_gyro *= (1.0f/ qnorm);
NaotoMorita 44:f571273d3223 287
NaotoMorita 38:acc7cdcf56dd 288 Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
NaotoMorita 38:acc7cdcf56dd 289
NaotoMorita 40:869f3791a3e2 290 q0 = qhat.getNumber( 1, 1 );
NaotoMorita 40:869f3791a3e2 291 q1 = qhat.getNumber( 2, 1 );
NaotoMorita 40:869f3791a3e2 292 q2 = qhat.getNumber( 3, 1 );
NaotoMorita 40:869f3791a3e2 293 q3 = qhat.getNumber( 4, 1 );
NaotoMorita 40:869f3791a3e2 294
NaotoMorita 40:869f3791a3e2 295 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
NaotoMorita 40:869f3791a3e2 296 D.add(1,2,2*(q1*q2 + q0*q3));
NaotoMorita 40:869f3791a3e2 297 D.add(1,3,2*(q1*q3 - q0*q2));
NaotoMorita 40:869f3791a3e2 298 D.add(2,1,2*(q1*q2 - q0*q3));
NaotoMorita 40:869f3791a3e2 299 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
NaotoMorita 40:869f3791a3e2 300 D.add(2,3,2*(q2*q3 + q0*q1));
NaotoMorita 40:869f3791a3e2 301 D.add(3,1,2*(q1*q3 + q0*q2));
NaotoMorita 40:869f3791a3e2 302 D.add(3,2,2*(q2*q3 - q0*q1));
NaotoMorita 40:869f3791a3e2 303 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
NaotoMorita 38:acc7cdcf56dd 304
NaotoMorita 38:acc7cdcf56dd 305 }
NaotoMorita 38:acc7cdcf56dd 306
NaotoMorita 38:acc7cdcf56dd 307 void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
NaotoMorita 38:acc7cdcf56dd 308
NaotoMorita 38:acc7cdcf56dd 309 Matrix u(3,1);
NaotoMorita 38:acc7cdcf56dd 310 Matrix v(3,1);
NaotoMorita 38:acc7cdcf56dd 311
NaotoMorita 38:acc7cdcf56dd 312 u << u1 << u2 <<u3;
NaotoMorita 38:acc7cdcf56dd 313 v << v1 << v2 <<v3;
NaotoMorita 38:acc7cdcf56dd 314
NaotoMorita 38:acc7cdcf56dd 315 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 316 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 317 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 318 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 319
NaotoMorita 38:acc7cdcf56dd 320 Matrix A1(3,3);
NaotoMorita 38:acc7cdcf56dd 321 A1 << q0 << q3 << -q2
NaotoMorita 38:acc7cdcf56dd 322 <<-q3 << q0 << q1
NaotoMorita 38:acc7cdcf56dd 323 <<q2 <<-q1 <<q0;
NaotoMorita 38:acc7cdcf56dd 324 A1 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 325
NaotoMorita 38:acc7cdcf56dd 326 Matrix A2(3,3);
NaotoMorita 38:acc7cdcf56dd 327 A2 << q1 << q2 << q3
NaotoMorita 38:acc7cdcf56dd 328 << q2 <<-q1 << q0
NaotoMorita 38:acc7cdcf56dd 329 << q3 <<-q0 <<-q1;
NaotoMorita 38:acc7cdcf56dd 330 A2 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 331
NaotoMorita 38:acc7cdcf56dd 332 Matrix A3(3,3);
NaotoMorita 38:acc7cdcf56dd 333 A3 <<-q2 << q1 <<-q0
NaotoMorita 38:acc7cdcf56dd 334 << q1 << q2 << q3
NaotoMorita 38:acc7cdcf56dd 335 << q0 << q3 <<-q2;
NaotoMorita 38:acc7cdcf56dd 336 A3 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 337
NaotoMorita 38:acc7cdcf56dd 338 Matrix A4(3,3);
NaotoMorita 38:acc7cdcf56dd 339 A4 <<-q3 << q0 << q1
NaotoMorita 38:acc7cdcf56dd 340 <<-q0 <<-q3 << q2
NaotoMorita 38:acc7cdcf56dd 341 << q1 << q2 << q3;
NaotoMorita 38:acc7cdcf56dd 342 A4 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 343
NaotoMorita 38:acc7cdcf56dd 344 Matrix H(3,4);
NaotoMorita 38:acc7cdcf56dd 345
NaotoMorita 38:acc7cdcf56dd 346 Matrix ab1(A1*u);
NaotoMorita 38:acc7cdcf56dd 347 Matrix ab2(A2*u);
NaotoMorita 38:acc7cdcf56dd 348 Matrix ab3(A3*u);
NaotoMorita 38:acc7cdcf56dd 349 Matrix ab4(A4*u);
NaotoMorita 38:acc7cdcf56dd 350
NaotoMorita 38:acc7cdcf56dd 351 H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 352 << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 353 << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 354
NaotoMorita 38:acc7cdcf56dd 355
NaotoMorita 38:acc7cdcf56dd 356 Matrix K(4,3);
NaotoMorita 38:acc7cdcf56dd 357 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 38:acc7cdcf56dd 358
NaotoMorita 38:acc7cdcf56dd 359 Matrix dq(4,1);
NaotoMorita 38:acc7cdcf56dd 360 dq = K*(v-D*u);
NaotoMorita 38:acc7cdcf56dd 361 qhat = qhat+dq;
NaotoMorita 38:acc7cdcf56dd 362
NaotoMorita 38:acc7cdcf56dd 363 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 364 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 365
NaotoMorita 38:acc7cdcf56dd 366 Matrix eye4(4,4);
NaotoMorita 38:acc7cdcf56dd 367 eye4 << 1 << 0 << 0 << 0
NaotoMorita 38:acc7cdcf56dd 368 << 0 << 1 << 0 << 0
NaotoMorita 38:acc7cdcf56dd 369 << 0 << 0 << 1 << 0
NaotoMorita 38:acc7cdcf56dd 370 << 0 << 0 << 0 << 1;
NaotoMorita 38:acc7cdcf56dd 371 Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 38:acc7cdcf56dd 372 }
NaotoMorita 38:acc7cdcf56dd 373
NaotoMorita 38:acc7cdcf56dd 374 void computeAngles()
naker 20:2c3f113a8e8f 375 {
NaotoMorita 38:acc7cdcf56dd 376 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 377 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 378 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 379 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 380 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
NaotoMorita 38:acc7cdcf56dd 381 pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
NaotoMorita 38:acc7cdcf56dd 382 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 383
NaotoMorita 44:f571273d3223 384 q0 = qhat_gyro.getNumber( 1, 1 );
NaotoMorita 44:f571273d3223 385 q1 = qhat_gyro.getNumber( 2, 1 );
NaotoMorita 44:f571273d3223 386 q2 = qhat_gyro.getNumber( 3, 1 );
NaotoMorita 44:f571273d3223 387 q3 = qhat_gyro.getNumber( 4, 1 );
NaotoMorita 44:f571273d3223 388 roll_g = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
NaotoMorita 44:f571273d3223 389 pitch_g = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
NaotoMorita 44:f571273d3223 390 yaw_g = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 391
NaotoMorita 38:acc7cdcf56dd 392 }
NaotoMorita 38:acc7cdcf56dd 393
NaotoMorita 38:acc7cdcf56dd 394 void triad(float fb1,float fb2, float fb3, float fn1, float fn2, float fn3,float mb1,float mb2, float mb3, float mn1, float mn2, float mn3){
NaotoMorita 38:acc7cdcf56dd 395 Matrix W1(3,1);
NaotoMorita 38:acc7cdcf56dd 396 W1 << fb1 << fb2<< fb3;
NaotoMorita 38:acc7cdcf56dd 397 Matrix W2(3,1);
NaotoMorita 38:acc7cdcf56dd 398 W2 << mb1 << mb2<< mb3;
NaotoMorita 38:acc7cdcf56dd 399
NaotoMorita 38:acc7cdcf56dd 400 Matrix V1(3,1);
NaotoMorita 38:acc7cdcf56dd 401 V1 << fn1 << fn2<< fn3;
NaotoMorita 38:acc7cdcf56dd 402 Matrix V2(3,1);
NaotoMorita 38:acc7cdcf56dd 403 V2 << mn1 << mn2<< mn3;
NaotoMorita 38:acc7cdcf56dd 404
NaotoMorita 38:acc7cdcf56dd 405
NaotoMorita 38:acc7cdcf56dd 406 Matrix Ou2(3,1);
NaotoMorita 38:acc7cdcf56dd 407 Ou2 << W1.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 408 Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
NaotoMorita 38:acc7cdcf56dd 409 Matrix Ou3(3,1);
NaotoMorita 38:acc7cdcf56dd 410 Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 411 Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
NaotoMorita 38:acc7cdcf56dd 412 Matrix R2(3,1);
NaotoMorita 38:acc7cdcf56dd 413 R2 << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 414 R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
NaotoMorita 38:acc7cdcf56dd 415 Matrix R3(3,1);
NaotoMorita 38:acc7cdcf56dd 416 R3 << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 417 R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
NaotoMorita 38:acc7cdcf56dd 418
NaotoMorita 38:acc7cdcf56dd 419 Matrix Mou(3,3);
NaotoMorita 38:acc7cdcf56dd 420 Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 421 << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 422 << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 423 Matrix Mr(3,3);
NaotoMorita 38:acc7cdcf56dd 424 Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 425 << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 426 << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 427
NaotoMorita 38:acc7cdcf56dd 428 Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
NaotoMorita 38:acc7cdcf56dd 429
NaotoMorita 38:acc7cdcf56dd 430 float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
NaotoMorita 38:acc7cdcf56dd 431
NaotoMorita 38:acc7cdcf56dd 432 qhat.add(1,1,0.5*sqtrp1);
NaotoMorita 38:acc7cdcf56dd 433 qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 434 qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 435 qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 436
NaotoMorita 38:acc7cdcf56dd 437 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 438 qhat *= (1.0f/ qnorm);
NaotoMorita 44:f571273d3223 439
NaotoMorita 44:f571273d3223 440 qhat_gyro = qhat;
NaotoMorita 44:f571273d3223 441
NaotoMorita 44:f571273d3223 442 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 44:f571273d3223 443 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 44:f571273d3223 444 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 44:f571273d3223 445 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 44:f571273d3223 446
NaotoMorita 44:f571273d3223 447 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 448 D.add(1,2,2*(q1*q2 + q0*q3));
NaotoMorita 44:f571273d3223 449 D.add(1,3,2*(q1*q3 - q0*q2));
NaotoMorita 44:f571273d3223 450 D.add(2,1,2*(q1*q2 - q0*q3));
NaotoMorita 44:f571273d3223 451 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 452 D.add(2,3,2*(q2*q3 + q0*q1));
NaotoMorita 44:f571273d3223 453 D.add(3,1,2*(q1*q3 + q0*q2));
NaotoMorita 44:f571273d3223 454 D.add(3,2,2*(q2*q3 - q0*q1));
NaotoMorita 44:f571273d3223 455 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
NaotoMorita 44:f571273d3223 456
NaotoMorita 38:acc7cdcf56dd 457 }
NaotoMorita 38:acc7cdcf56dd 458
NaotoMorita 38:acc7cdcf56dd 459 void calcDynAcc(){
NaotoMorita 40:869f3791a3e2 460 dynacc_x = LPacc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]);
NaotoMorita 40:869f3791a3e2 461 dynacc_y = LPacc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]);
NaotoMorita 40:869f3791a3e2 462 dynacc_z = LPacc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]);
NaotoMorita 38:acc7cdcf56dd 463 }
NaotoMorita 38:acc7cdcf56dd 464
naker 22:da9893b71f24 465 // 割り込まれた時点での出力(computeの結果）を返す関数
NaotoMorita 27:43bd0e444633 466 void calcServoOut(){
NaotoMorita 28:fc6c46c1db65 467 if(sbus.failSafe == false) {
NaotoMorita 28:fc6c46c1db65 468 // sbusデータの読み込み
NaotoMorita 28:fc6c46c1db65 469 for (int i =0 ; i < 16;i ++){
NaotoMorita 35:4535af88f7bf 470 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
NaotoMorita 27:43bd0e444633 471 }
NaotoMorita 28:fc6c46c1db65 472 }
NaotoMorita 52:abec61ea3cd3 473 pitchPID.setGain(6.36, 10.6,0.0);
NaotoMorita 52:abec61ea3cd3 474 pitchratePID.setGain(0.9540,0.0,0.0);
NaotoMorita 28:fc6c46c1db65 475 pitchPID.setProcessValue(pitch);
NaotoMorita 28:fc6c46c1db65 476 pitchratePID.setProcessValue(gyro_y);
NaotoMorita 45:b439a135c24b 477 float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
NaotoMorita 28:fc6c46c1db65 478
NaotoMorita 48:1065506191f2 479 scaledServoOut[0]=de;
NaotoMorita 31:8d02f3b9a067 480
NaotoMorita 38:acc7cdcf56dd 481 float LP_servo = 0.2;
NaotoMorita 36:e68ce293306e 482 for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
NaotoMorita 36:e68ce293306e 483 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
NaotoMorita 28:fc6c46c1db65 484 if(servoOut[i]<servoPwmMin) {
NaotoMorita 28:fc6c46c1db65 485 servoOut[i] = servoPwmMin;
NaotoMorita 28:fc6c46c1db65 486 };
NaotoMorita 28:fc6c46c1db65 487 if(servoOut[i]>servoPwmMax) {
NaotoMorita 28:fc6c46c1db65 488 servoOut[i] = servoPwmMax;
NaotoMorita 28:fc6c46c1db65 489 };
NaotoMorita 28:fc6c46c1db65 490 }
NaotoMorita 28:fc6c46c1db65 491
NaotoMorita 48:1065506191f2 492 servo.pulsewidth_us(servoOut[0]);
NaotoMorita 32:c4f06cb0e1d6 493
NaotoMorita 44:f571273d3223 494 //観測アップデート
NaotoMorita 44:f571273d3223 495 calcDynAcc();
NaotoMorita 44:f571273d3223 496 th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg);
NaotoMorita 44:f571273d3223 497 dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
NaotoMorita 44:f571273d3223 498 accnormerr = abs(LPaccnorm-accrefnorm);
NaotoMorita 44:f571273d3223 499 //静止時100個の平均　0.01877522 0.00514146 0.00477393
NaotoMorita 44:f571273d3223 500
NaotoMorita 44:f571273d3223 501 //int ang_th = th_mg < 0.01877522;
NaotoMorita 44:f571273d3223 502 //int dyn_th = dynaccnorm < 0.00514146;
NaotoMorita 44:f571273d3223 503 //int norm_th = accnormerr< 0.00477393;
NaotoMorita 44:f571273d3223 504 int ang_th = th_mg < 0.01877522/5.0;
NaotoMorita 44:f571273d3223 505 int dyn_th = dynaccnorm < 0.00514146/5.0;
NaotoMorita 44:f571273d3223 506 int norm_th = accnormerr< 0.00477393/5.0;
NaotoMorita 44:f571273d3223 507 if(dyn_th+ang_th+norm_th>0){
NaotoMorita 44:f571273d3223 508 //if(dyn_th+ang_th+norm_th>-1){
NaotoMorita 44:f571273d3223 509 obs_count += 1;
NaotoMorita 44:f571273d3223 510 updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
NaotoMorita 44:f571273d3223 511 updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
NaotoMorita 44:f571273d3223 512 }
NaotoMorita 44:f571273d3223 513
NaotoMorita 44:f571273d3223 514 if(loop_count >= 10){
NaotoMorita 31:8d02f3b9a067 515 writeSdcard();
NaotoMorita 36:e68ce293306e 516 loop_count = 1;
NaotoMorita 40:869f3791a3e2 517 obs_count = 0;
NaotoMorita 27:43bd0e444633 518 }else{
NaotoMorita 27:43bd0e444633 519 loop_count +=1;
naker 26:62d9857eaecc 520 }
NaotoMorita 44:f571273d3223 521
NaotoMorita 44:f571273d3223 522
naker 22:da9893b71f24 523 }
cocorlow 7:70161eb0f854 524
NaotoMorita 27:43bd0e444633 525 int main()
NaotoMorita 36:e68ce293306e 526 {
NaotoMorita 28:fc6c46c1db65 527 pitchPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 528 pitchratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 529 pitchPID.setBias(0.0);
NaotoMorita 48:1065506191f2 530 pitchratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 531 pitchPID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 532 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 533 pitchPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 534 pitchratePID.setInputLimits(-pi,pi);
NaotoMorita 29:34ee662f454e 535
NaotoMorita 48:1065506191f2 536 servo.period_us(15000.0);
NaotoMorita 48:1065506191f2 537 servo.pulsewidth_us(1500.0);
NaotoMorita 36:e68ce293306e 538
cocorlow 53:3a6dfb55e087 539 // pc.baud(57600);
NaotoMorita 36:e68ce293306e 540 accelgyro.initialize();
NaotoMorita 36:e68ce293306e 541 //加速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 542 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 38:acc7cdcf56dd 543 //角速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 544 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 36:e68ce293306e 545 //MPU6050のLPFを設定
NaotoMorita 36:e68ce293306e 546 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 38:acc7cdcf56dd 547 //地磁気
NaotoMorita 38:acc7cdcf56dd 548 mag.enable();
NaotoMorita 49:73fe59148dd4 549
NaotoMorita 49:73fe59148dd4 550 qhat << 1 << 0 << 0 << 0;
NaotoMorita 38:acc7cdcf56dd 551
NaotoMorita 49:73fe59148dd4 552 D.add(1,1,1.0);
NaotoMorita 49:73fe59148dd4 553 D.add(2,2,1.0);
NaotoMorita 49:73fe59148dd4 554 D.add(3,3,1.0);
NaotoMorita 49:73fe59148dd4 555
NaotoMorita 52:abec61ea3cd3 556 Phat.add(1,1,0.05);
NaotoMorita 52:abec61ea3cd3 557 Phat.add(2,2,0.05);
NaotoMorita 52:abec61ea3cd3 558 Phat.add(3,3,0.05);
NaotoMorita 52:abec61ea3cd3 559 Phat.add(4,4,0.05);
NaotoMorita 38:acc7cdcf56dd 560
NaotoMorita 49:73fe59148dd4 561 Qgyro.add(1,1,0.0224);
NaotoMorita 49:73fe59148dd4 562 Qgyro.add(2,2,0.0224);
NaotoMorita 49:73fe59148dd4 563 Qgyro.add(3,3,0.0224);
NaotoMorita 49:73fe59148dd4 564
NaotoMorita 52:abec61ea3cd3 565 Racc.add(1,1,0.0330*200);
NaotoMorita 52:abec61ea3cd3 566 Racc.add(2,2,0.0330*200);
NaotoMorita 52:abec61ea3cd3 567 Racc.add(3,3,0.0330*200);
NaotoMorita 49:73fe59148dd4 568
NaotoMorita 49:73fe59148dd4 569 Rmag.add(1,1,1.0);
NaotoMorita 49:73fe59148dd4 570 Rmag.add(2,2,1.0);
NaotoMorita 49:73fe59148dd4 571 Rmag.add(3,3,1.0);
NaotoMorita 49:73fe59148dd4 572
NaotoMorita 49:73fe59148dd4 573 calibrationFlag = userButton.read();
NaotoMorita 49:73fe59148dd4 574 if(calibrationFlag == 0){
NaotoMorita 38:acc7cdcf56dd 575
NaotoMorita 49:73fe59148dd4 576 pc.printf("reading calibration value\r\n");
NaotoMorita 49:73fe59148dd4 577 //キャリブレーション値を取得
NaotoMorita 49:73fe59148dd4 578 U read_calib;
NaotoMorita 52:abec61ea3cd3 579 readEEPROM(address, pointeraddress, read_calib.c, N_EEPROM*4);
NaotoMorita 49:73fe59148dd4 580 wait(3);
NaotoMorita 49:73fe59148dd4 581 pos_tail = read_calib.i[0];
NaotoMorita 49:73fe59148dd4 582 agoffset[3] = float(read_calib.i[5]);
NaotoMorita 49:73fe59148dd4 583 agoffset[4] = float(read_calib.i[6]);
NaotoMorita 49:73fe59148dd4 584 agoffset[5] = float(read_calib.i[7]);
NaotoMorita 49:73fe59148dd4 585 magbias[0] = float(read_calib.i[1])/1000.0;
NaotoMorita 49:73fe59148dd4 586 magbias[1] = float(read_calib.i[2])/1000.0;
NaotoMorita 49:73fe59148dd4 587 magbias[2] = float(read_calib.i[3])/1000.0;
NaotoMorita 49:73fe59148dd4 588 magbias[3] = float(read_calib.i[4])/1000.0;
NaotoMorita 52:abec61ea3cd3 589 pitch_align = float(read_calib.i[8])/200000.0;
NaotoMorita 52:abec61ea3cd3 590 roll_align = float(read_calib.i[9])/200000.0;
cocorlow 53:3a6dfb55e087 591 tail_address = (int)read_calib.i[10];
NaotoMorita 52:abec61ea3cd3 592
NaotoMorita 49:73fe59148dd4 593 switch(pos_tail){
NaotoMorita 49:73fe59148dd4 594 case 1:
NaotoMorita 49:73fe59148dd4 595 pc.printf("This MBED is Located at Left \r\n");
NaotoMorita 49:73fe59148dd4 596 break;
NaotoMorita 49:73fe59148dd4 597 case 2:
NaotoMorita 49:73fe59148dd4 598 pc.printf("This MBED is Located at Center \r\n");
NaotoMorita 49:73fe59148dd4 599 break;
NaotoMorita 49:73fe59148dd4 600 case 3:
NaotoMorita 49:73fe59148dd4 601 pc.printf("This MBED is Located at Right \r\n");
NaotoMorita 49:73fe59148dd4 602 break;
NaotoMorita 49:73fe59148dd4 603 default: // error situation
NaotoMorita 49:73fe59148dd4 604 pc.printf("error\r\n");
NaotoMorita 49:73fe59148dd4 605 break;
NaotoMorita 49:73fe59148dd4 606 }
cocorlow 53:3a6dfb55e087 607 pc.printf("tail_address : %d\r\n", tail_address);
NaotoMorita 52:abec61ea3cd3 608 pc.printf("Alignment values are %f(pitch deg) %f(roll deg)\r\n",pitch_align*180.0/pi,roll_align*180.0/pi);
NaotoMorita 38:acc7cdcf56dd 609 getIMUval();
NaotoMorita 38:acc7cdcf56dd 610 triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm);
NaotoMorita 43:f0dd73430192 611 float sumLPaccnorm = 0;
NaotoMorita 38:acc7cdcf56dd 612 for(int i = 0; i<1000 ;i++){
NaotoMorita 38:acc7cdcf56dd 613 getIMUval();
NaotoMorita 38:acc7cdcf56dd 614 val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
NaotoMorita 43:f0dd73430192 615 sumLPaccnorm += LPaccnorm;
NaotoMorita 38:acc7cdcf56dd 616 }
NaotoMorita 43:f0dd73430192 617 accref[2]=-sumLPaccnorm/1000;
NaotoMorita 38:acc7cdcf56dd 618 val_thmg /= 1000;
NaotoMorita 38:acc7cdcf56dd 619 LoopTicker PIDtick;
NaotoMorita 38:acc7cdcf56dd 620 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 38:acc7cdcf56dd 621
NaotoMorita 38:acc7cdcf56dd 622 t.start();
NaotoMorita 28:fc6c46c1db65 623
NaotoMorita 38:acc7cdcf56dd 624 while(1) {
NaotoMorita 38:acc7cdcf56dd 625 float tstart = t.read();
NaotoMorita 38:acc7cdcf56dd 626 //姿勢角を更新
NaotoMorita 38:acc7cdcf56dd 627 getIMUval();
NaotoMorita 38:acc7cdcf56dd 628 updateBetweenMeasures();
NaotoMorita 38:acc7cdcf56dd 629 computeAngles();
NaotoMorita 38:acc7cdcf56dd 630 PIDtick.loop();
NaotoMorita 38:acc7cdcf56dd 631
NaotoMorita 38:acc7cdcf56dd 632 float tend = t.read();
NaotoMorita 38:acc7cdcf56dd 633 att_dt = (tend-tstart);
NaotoMorita 38:acc7cdcf56dd 634 }
NaotoMorita 38:acc7cdcf56dd 635 }else{
NaotoMorita 49:73fe59148dd4 636 pc.printf("\r\nEnter to Calibration Mode\r\n");
NaotoMorita 49:73fe59148dd4 637 wait(5);
NaotoMorita 49:73fe59148dd4 638 pc.printf("Acc and Gyro Calibration Start\r\n");
NaotoMorita 49:73fe59148dd4 639
NaotoMorita 49:73fe59148dd4 640 int gxs = 0;
NaotoMorita 49:73fe59148dd4 641 int gys = 0;
NaotoMorita 49:73fe59148dd4 642 int gzs = 0;
NaotoMorita 49:73fe59148dd4 643 int iter_n = 10000;
NaotoMorita 49:73fe59148dd4 644 for(int i = 0;i<iter_n ;i++){
NaotoMorita 49:73fe59148dd4 645 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
NaotoMorita 49:73fe59148dd4 646 gxs += gx;
NaotoMorita 49:73fe59148dd4 647 gys += gy;
NaotoMorita 49:73fe59148dd4 648 gzs -= gz;
NaotoMorita 49:73fe59148dd4 649 //wait(0.01);
NaotoMorita 49:73fe59148dd4 650 }
NaotoMorita 49:73fe59148dd4 651 gxs = gxs /iter_n;
NaotoMorita 49:73fe59148dd4 652 gys = gys /iter_n;
NaotoMorita 49:73fe59148dd4 653 gzs = gzs /iter_n;
NaotoMorita 49:73fe59148dd4 654 agoffset[3] = gxs;
NaotoMorita 49:73fe59148dd4 655 agoffset[4] = gys;
NaotoMorita 49:73fe59148dd4 656 agoffset[5] = gzs;
NaotoMorita 49:73fe59148dd4 657 pc.printf("Gyrooffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
NaotoMorita 49:73fe59148dd4 658
NaotoMorita 49:73fe59148dd4 659 pc.printf("Mag Calibration Start\r\n");
NaotoMorita 49:73fe59148dd4 660 float f = 0;
NaotoMorita 49:73fe59148dd4 661 while(1){
NaotoMorita 49:73fe59148dd4 662 mag.getAxis(mdata); // flush the magnetmeter
NaotoMorita 49:73fe59148dd4 663 magval[0] = (mdata.x - magbias[0]);
NaotoMorita 49:73fe59148dd4 664 magval[1] = (mdata.y - magbias[1]);
NaotoMorita 49:73fe59148dd4 665 magval[2] = (mdata.z - magbias[2]);
NaotoMorita 49:73fe59148dd4 666 float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
NaotoMorita 49:73fe59148dd4 667 float lr = 0.00001f;
NaotoMorita 49:73fe59148dd4 668 f = mag_r - magbias[3]*magbias[3];
NaotoMorita 49:73fe59148dd4 669 magbias[0] = magbias[0] + 4 * lr * f * magval[0];
NaotoMorita 49:73fe59148dd4 670 magbias[1] = magbias[1] + 4 * lr * f * magval[1];
NaotoMorita 49:73fe59148dd4 671 magbias[2] = magbias[2] + 4 * lr * f * magval[2];
NaotoMorita 49:73fe59148dd4 672 magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
NaotoMorita 49:73fe59148dd4 673
NaotoMorita 49:73fe59148dd4 674 if(userButton.read() == 1){
NaotoMorita 49:73fe59148dd4 675 break;
NaotoMorita 49:73fe59148dd4 676 }
NaotoMorita 49:73fe59148dd4 677 wait(0.001);
NaotoMorita 49:73fe59148dd4 678 }
NaotoMorita 49:73fe59148dd4 679 pc.printf("Magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
NaotoMorita 49:73fe59148dd4 680
NaotoMorita 49:73fe59148dd4 681 pc.printf("Determine Position of MBED\r\n");
NaotoMorita 49:73fe59148dd4 682 wait(1);
NaotoMorita 49:73fe59148dd4 683 pc.printf("Press the user button\r\n");
NaotoMorita 49:73fe59148dd4 684
NaotoMorita 49:73fe59148dd4 685 while(userButton.read() == 0){wait(0.01);};
NaotoMorita 49:73fe59148dd4 686 while(1){
NaotoMorita 49:73fe59148dd4 687 pc.printf("Left\r\n");
NaotoMorita 49:73fe59148dd4 688 wait(2);
NaotoMorita 49:73fe59148dd4 689 if(userButton.read() == 0){
NaotoMorita 49:73fe59148dd4 690 pos_tail=1;
cocorlow 53:3a6dfb55e087 691 tail_address = 1111;
NaotoMorita 49:73fe59148dd4 692 break;
NaotoMorita 49:73fe59148dd4 693 };
NaotoMorita 49:73fe59148dd4 694 pc.printf("Center\r\n");
NaotoMorita 49:73fe59148dd4 695 wait(2);
NaotoMorita 49:73fe59148dd4 696 if(userButton.read() == 0){
NaotoMorita 49:73fe59148dd4 697 pos_tail=2;
cocorlow 53:3a6dfb55e087 698 tail_address = 2222;
NaotoMorita 49:73fe59148dd4 699 break;
NaotoMorita 49:73fe59148dd4 700 };
NaotoMorita 49:73fe59148dd4 701 pc.printf("Right\r\n");
NaotoMorita 49:73fe59148dd4 702 wait(2);
NaotoMorita 49:73fe59148dd4 703 if(userButton.read() == 0){
NaotoMorita 49:73fe59148dd4 704 pos_tail=3;
cocorlow 53:3a6dfb55e087 705 tail_address = 3333;
NaotoMorita 49:73fe59148dd4 706 break;
NaotoMorita 49:73fe59148dd4 707 };
NaotoMorita 49:73fe59148dd4 708 };
cocorlow 53:3a6dfb55e087 709 pc.printf("tail_address : %d\r\n", tail_address);
NaotoMorita 49:73fe59148dd4 710 switch(pos_tail){
NaotoMorita 49:73fe59148dd4 711 case 1:
NaotoMorita 49:73fe59148dd4 712 pc.printf("This MBED is Located at Left \r\n");
NaotoMorita 49:73fe59148dd4 713 break;
NaotoMorita 49:73fe59148dd4 714 case 2:
NaotoMorita 49:73fe59148dd4 715 pc.printf("This MBED is Located at Center \r\n");
NaotoMorita 49:73fe59148dd4 716 break;
NaotoMorita 49:73fe59148dd4 717 case 3:
NaotoMorita 49:73fe59148dd4 718 pc.printf("This MBED is Located at Right \r\n");
NaotoMorita 49:73fe59148dd4 719 break;
NaotoMorita 49:73fe59148dd4 720 default: // error situation
NaotoMorita 49:73fe59148dd4 721 pc.printf("error\r\n");
NaotoMorita 49:73fe59148dd4 722 break;
NaotoMorita 49:73fe59148dd4 723 }
NaotoMorita 49:73fe59148dd4 724
NaotoMorita 52:abec61ea3cd3 725 //姿勢オフセットを計算
NaotoMorita 52:abec61ea3cd3 726 pitch_align = 0.0*3.141562/180.0;
NaotoMorita 52:abec61ea3cd3 727 roll_align = 0.0*3.141562/180.0;
NaotoMorita 52:abec61ea3cd3 728 float ave_pitch = 0.0;
NaotoMorita 52:abec61ea3cd3 729 float ave_roll = 0.0;
NaotoMorita 52:abec61ea3cd3 730 for (int i = 0 ; i<2200; i++){
NaotoMorita 52:abec61ea3cd3 731 float tstart = t.read();
NaotoMorita 52:abec61ea3cd3 732 //姿勢角を更新
NaotoMorita 52:abec61ea3cd3 733 getIMUval();
NaotoMorita 52:abec61ea3cd3 734 updateBetweenMeasures();
NaotoMorita 52:abec61ea3cd3 735 updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
NaotoMorita 52:abec61ea3cd3 736 updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
NaotoMorita 52:abec61ea3cd3 737 computeAngles();
NaotoMorita 52:abec61ea3cd3 738 if(i>199){
NaotoMorita 52:abec61ea3cd3 739 ave_pitch += pitch;
NaotoMorita 52:abec61ea3cd3 740 ave_roll += roll;
NaotoMorita 52:abec61ea3cd3 741 }
NaotoMorita 52:abec61ea3cd3 742 wait(0.001);
NaotoMorita 52:abec61ea3cd3 743 float tend = t.read();
NaotoMorita 52:abec61ea3cd3 744 att_dt = (tend-tstart);
NaotoMorita 52:abec61ea3cd3 745 }
NaotoMorita 49:73fe59148dd4 746
NaotoMorita 52:abec61ea3cd3 747 pc.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0*180.0f/pi,ave_roll/2000.0*180.0f/pi);
NaotoMorita 49:73fe59148dd4 748
NaotoMorita 49:73fe59148dd4 749 pc.printf("Writing to EEPROM...\r\n");
NaotoMorita 49:73fe59148dd4 750 U transfer_data;
NaotoMorita 49:73fe59148dd4 751 transfer_data.i[0] = pos_tail;
NaotoMorita 49:73fe59148dd4 752 for (int i = 1; i < 5; i++) {
NaotoMorita 49:73fe59148dd4 753 if (!isnan(magbias[i - 1]))
NaotoMorita 49:73fe59148dd4 754 transfer_data.i[i] = int(magbias[i - 1]*1000); // intに丸めた値を送る。
NaotoMorita 49:73fe59148dd4 755 else {
NaotoMorita 49:73fe59148dd4 756 pc.printf("Mag bias is NOT transferred\n");
NaotoMorita 49:73fe59148dd4 757 transfer_data.i[i] = 100;
NaotoMorita 49:73fe59148dd4 758 }
NaotoMorita 49:73fe59148dd4 759 }
NaotoMorita 49:73fe59148dd4 760 // gxs,gys,gzsを送る
NaotoMorita 49:73fe59148dd4 761 int gxyzs[3] = {gxs, gys, gzs};
NaotoMorita 49:73fe59148dd4 762 for (int i = 5; i < 8; i++) {
NaotoMorita 49:73fe59148dd4 763 if (!isnan(gxyzs[i - 5]))
NaotoMorita 49:73fe59148dd4 764 transfer_data.i[i] = gxyzs[i - 5];
NaotoMorita 49:73fe59148dd4 765 else {
NaotoMorita 49:73fe59148dd4 766 pc.printf("gxyzs is NOT transferred\n");
NaotoMorita 49:73fe59148dd4 767 transfer_data.i[i] = 0;
NaotoMorita 49:73fe59148dd4 768 }
NaotoMorita 49:73fe59148dd4 769 }
NaotoMorita 49:73fe59148dd4 770
NaotoMorita 52:abec61ea3cd3 771 // ave_pitch,ave_rollを送る
NaotoMorita 52:abec61ea3cd3 772 int ave_pr[2] = {ave_pitch*100, ave_roll*100};
NaotoMorita 52:abec61ea3cd3 773 for (int i = 8; i < 10; i++) {
NaotoMorita 52:abec61ea3cd3 774 if (!isnan(ave_pr[i - 8]))
NaotoMorita 52:abec61ea3cd3 775 transfer_data.i[i] = ave_pr[i - 8];
NaotoMorita 52:abec61ea3cd3 776 else {
NaotoMorita 52:abec61ea3cd3 777 pc.printf("alignment data is NOT transferred\n");
NaotoMorita 52:abec61ea3cd3 778 transfer_data.i[i] = 0;
NaotoMorita 52:abec61ea3cd3 779 }
NaotoMorita 52:abec61ea3cd3 780 }
cocorlow 53:3a6dfb55e087 781 transfer_data.i[10] = tail_address;
NaotoMorita 49:73fe59148dd4 782 for (int i = 0; i < N_EEPROM; i++) {
cocorlow 53:3a6dfb55e087 783 pc.printf("transfer_data[%d]: %d\r\n", i, transfer_data.i[i]);
NaotoMorita 49:73fe59148dd4 784 }
NaotoMorita 49:73fe59148dd4 785 writeEEPROM(address, pointeraddress, transfer_data.c, N_EEPROM*4);
NaotoMorita 49:73fe59148dd4 786 wait(3);
NaotoMorita 49:73fe59148dd4 787
NaotoMorita 49:73fe59148dd4 788 U read_test;
NaotoMorita 49:73fe59148dd4 789 readEEPROM(address, pointeraddress, read_test.c, N_EEPROM*4);
NaotoMorita 49:73fe59148dd4 790 wait(3);
NaotoMorita 49:73fe59148dd4 791 for (int i = 0 ; i < N_EEPROM; i ++){
NaotoMorita 49:73fe59148dd4 792 pc.printf("transfer_data[%d]: %d\r\n",i, read_test.i[i]);
NaotoMorita 49:73fe59148dd4 793 }
NaotoMorita 49:73fe59148dd4 794
NaotoMorita 49:73fe59148dd4 795 while(1) {wait(1000);}
naker 20:2c3f113a8e8f 796 }
NaotoMorita 0:6b18a09a6628 797 }