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?
User | Revision | Line number | New 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 | } |