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@42:24c93e42c3f4, 2021-03-25 (annotated)

Committer:
naker
Date:
Thu Mar 25 02:55:00 2021 +0000
Revision:
42:24c93e42c3f4
Parent:
40:869f3791a3e2 
implemented eeprom function

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 6:2cba569272fe 12
NaotoMorita 17:6b7a363d06d2 13 #define MPU6050_PWR_MGMT_1 0x6B
NaotoMorita 17:6b7a363d06d2 14 #define MPU_ADDRESS 0x68
NaotoMorita 17:6b7a363d06d2 15 #define pi 3.141562
NaotoMorita 17:6b7a363d06d2 16 #define ACCEL_FSR MPU6050_ACCEL_FS_8
NaotoMorita 38:acc7cdcf56dd 17 #define ACCEL_SSF 4096.0
NaotoMorita 17:6b7a363d06d2 18 #define GYRO_FSR MPU6050_GYRO_FS_250
NaotoMorita 17:6b7a363d06d2 19 #define GYRO_SSF 131.0
NaotoMorita 17:6b7a363d06d2 20 #define MPU6050_LPF MPU6050_DLPF_BW_256
NaotoMorita 38:acc7cdcf56dd 21 #define gyro_th 20.0
NaotoMorita 38:acc7cdcf56dd 22 #define PID_dt 0.015
NaotoMorita 38:acc7cdcf56dd 23 #define servoPwmMax 1800.0
NaotoMorita 38:acc7cdcf56dd 24 #define servoPwmMin 1200.0
NaotoMorita 38:acc7cdcf56dd 25 #define motorPwmMax 2000.0
NaotoMorita 38:acc7cdcf56dd 26 #define motorPwmMin 1100.0
NaotoMorita 38:acc7cdcf56dd 27 #define pitch_align 8.0*3.141562/180.0
NaotoMorita 38:acc7cdcf56dd 28 #define roll_align -2.8*3.141562/180.0
NaotoMorita 27:43bd0e444633 29
NaotoMorita 17:6b7a363d06d2 30 MPU6050 accelgyro;
NaotoMorita 38:acc7cdcf56dd 31 MAG3110 mag(PB_9,PB_8);
NaotoMorita 0:6b18a09a6628 32 SBUS sbus(PD_5, PD_6);
NaotoMorita 0:6b18a09a6628 33 Serial pc(USBTX, USBRX);
naker 26:62d9857eaecc 34 Serial sd(PG_14,PG_9);
NaotoMorita 38:acc7cdcf56dd 35 DigitalIn userButton(USER_BUTTON);
NaotoMorita 36:e68ce293306e 36 FastPWM servoRight(PE_9);
NaotoMorita 36:e68ce293306e 37 FastPWM servoLeft(PE_11);
NaotoMorita 38:acc7cdcf56dd 38 FastPWM servoThrust(PE_13);
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 36:e68ce293306e 41 PID rollPID(5.0,0.0,0.0,PID_dt);
NaotoMorita 38:acc7cdcf56dd 42 PID rollratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 17:6b7a363d06d2 43 Timer t;
cocorlow 3:79e62f9b13c8 44
NaotoMorita 36:e68ce293306e 45 Matrix qhat(4,1);
NaotoMorita 36:e68ce293306e 46 Matrix Phat(4,4);
NaotoMorita 37:dad55cf05e3d 47 Matrix Qgyro(3,3);
NaotoMorita 36:e68ce293306e 48 Matrix Racc(3,3);
NaotoMorita 38:acc7cdcf56dd 49 Matrix Rmag(3,3);
NaotoMorita 38:acc7cdcf56dd 50 Matrix D(3,3);
NaotoMorita 36:e68ce293306e 51
NaotoMorita 38:acc7cdcf56dd 52 int loop_count = 0;
NaotoMorita 40:869f3791a3e2 53 int obs_count = 0;
NaotoMorita 36:e68ce293306e 54 float att_dt = 0.01;
naker 23:4a490fa8bf4a 55 float rc[16];
NaotoMorita 38:acc7cdcf56dd 56 float pitch = 0.0;
NaotoMorita 38:acc7cdcf56dd 57 float roll = 0.0;
NaotoMorita 38:acc7cdcf56dd 58 float yaw = 0.0;
NaotoMorita 17:6b7a363d06d2 59 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 60 int16_t gx, gy, gz;
NaotoMorita 38:acc7cdcf56dd 61 MotionSensorDataUnits mdata;
NaotoMorita 38:acc7cdcf56dd 62 float magval[3] = {1.0,0.0,0.0};
NaotoMorita 38:acc7cdcf56dd 63 float acc_x,acc_y,acc_z;
NaotoMorita 38:acc7cdcf56dd 64 float dynacc_x,dynacc_y,dynacc_z;
NaotoMorita 38:acc7cdcf56dd 65 float gyro_x,gyro_y,gyro_z;
NaotoMorita 38:acc7cdcf56dd 66 float mag_x,mag_y,mag_z;
NaotoMorita 40:869f3791a3e2 67
NaotoMorita 40:869f3791a3e2 68 float LPacc_x=0.0;
NaotoMorita 40:869f3791a3e2 69 float LPacc_y=0.0;
NaotoMorita 40:869f3791a3e2 70 float LPacc_z=0.0;
NaotoMorita 40:869f3791a3e2 71 float LPmag_x = 0.0;
NaotoMorita 40:869f3791a3e2 72 float LPmag_y = 0.0;
NaotoMorita 40:869f3791a3e2 73 float LPmag_z = 0.0;;
NaotoMorita 40:869f3791a3e2 74
cocorlow 3:79e62f9b13c8 75 int out1, out2;
NaotoMorita 36:e68ce293306e 76 float scaledServoOut[3]= {0,0};
NaotoMorita 28:fc6c46c1db65 77 float scaledMotorOut[1]= {-1};
NaotoMorita 36:e68ce293306e 78 float servoOut[3] = {1500.0,1500.0};
NaotoMorita 36:e68ce293306e 79 float motorOut[1] = {1100.0};
NaotoMorita 38:acc7cdcf56dd 80
NaotoMorita 38:acc7cdcf56dd 81 float accnorm;
NaotoMorita 38:acc7cdcf56dd 82 float magnorm;
NaotoMorita 40:869f3791a3e2 83 float LPaccnorm;
NaotoMorita 40:869f3791a3e2 84 float LPmagnorm;
NaotoMorita 38:acc7cdcf56dd 85 float accrefnorm;
NaotoMorita 38:acc7cdcf56dd 86 float magrefnorm;
NaotoMorita 38:acc7cdcf56dd 87
NaotoMorita 40:869f3791a3e2 88
NaotoMorita 38:acc7cdcf56dd 89 float val_thmg = 0.0;
NaotoMorita 40:869f3791a3e2 90 float th_mg = 0.0;
NaotoMorita 40:869f3791a3e2 91 float dynaccnorm = 0.0;
NaotoMorita 40:869f3791a3e2 92 float accnormerr = 0.0;
NaotoMorita 38:acc7cdcf56dd 93 float accref[3] = {0, 0, -0.98};
NaotoMorita 38:acc7cdcf56dd 94 float magref[3] = {0.65, 0, 0.75};
NaotoMorita 38:acc7cdcf56dd 95
NaotoMorita 38:acc7cdcf56dd 96 int agoffset[6] = {0, 0, 0, -123, -575, -1};
NaotoMorita 38:acc7cdcf56dd 97 float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077};
cocorlow 3:79e62f9b13c8 98
naker 42:24c93e42c3f4 99 // eepromのread writeのためのunionを定義
naker 42:24c93e42c3f4 100 int address = 0xAE; // EEPROMの3つの入力が全て+より
naker 42:24c93e42c3f4 101 int pointeraddress = 0;
naker 42:24c93e42c3f4 102
naker 42:24c93e42c3f4 103 I2C i2c(PB_9,PB_8); // sda, scl
naker 42:24c93e42c3f4 104 union U{
naker 42:24c93e42c3f4 105 int i[8]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
naker 42:24c93e42c3f4 106 char c[32]; // floatはcharの4倍
naker 42:24c93e42c3f4 107 };
naker 42:24c93e42c3f4 108 int gxs = 0;
naker 42:24c93e42c3f4 109 int gys = 0;
naker 42:24c93e42c3f4 110 int gzs = 0;
naker 42:24c93e42c3f4 111
naker 42:24c93e42c3f4 112
naker 42:24c93e42c3f4 113
naker 42:24c93e42c3f4 114 // eeprom書き込み・読み込みに必要な関数
naker 42:24c93e42c3f4 115 void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
naker 42:24c93e42c3f4 116 {
naker 42:24c93e42c3f4 117 char i2cBuffer[size + 2];
naker 42:24c93e42c3f4 118 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
naker 42:24c93e42c3f4 119 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
naker 42:24c93e42c3f4 120
naker 42:24c93e42c3f4 121 for (int i = 0; i < size; i++) {
naker 42:24c93e42c3f4 122 i2cBuffer[i + 2] = data[i];
naker 42:24c93e42c3f4 123 }
naker 42:24c93e42c3f4 124
naker 42:24c93e42c3f4 125 int result = i2c.write(address, i2cBuffer, size + 2, false);
naker 42:24c93e42c3f4 126 //sleep_ms(500);
naker 42:24c93e42c3f4 127 }
naker 42:24c93e42c3f4 128
naker 42:24c93e42c3f4 129 // this function has no read limit
naker 42:24c93e42c3f4 130 void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
naker 42:24c93e42c3f4 131 {
naker 42:24c93e42c3f4 132 char i2cBuffer[2];
naker 42:24c93e42c3f4 133 i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
naker 42:24c93e42c3f4 134 i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
naker 42:24c93e42c3f4 135
naker 42:24c93e42c3f4 136 // Reset eeprom pointer address
naker 42:24c93e42c3f4 137 int result = i2c.write(address, i2cBuffer, 2, false);
naker 42:24c93e42c3f4 138
naker 42:24c93e42c3f4 139 //sleep_ms(500);
naker 42:24c93e42c3f4 140
naker 42:24c93e42c3f4 141 // Read eeprom
naker 42:24c93e42c3f4 142 i2c.read(address, data, size);
naker 42:24c93e42c3f4 143 //sleep_ms(500);
naker 42:24c93e42c3f4 144 }
naker 42:24c93e42c3f4 145
NaotoMorita 38:acc7cdcf56dd 146 void writeSdcard()
NaotoMorita 38:acc7cdcf56dd 147 {
NaotoMorita 38:acc7cdcf56dd 148 //magcal.getExtremes(&magmin[0],&magmax[0]);
NaotoMorita 38:acc7cdcf56dd 149 //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 150 //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]);
NaotoMorita 40:869f3791a3e2 151 //pc.printf("%f %d %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);
NaotoMorita 40:869f3791a3e2 152 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);
NaotoMorita 38:acc7cdcf56dd 153 //pc.printf("%d \r\n",userButton.read());
NaotoMorita 38:acc7cdcf56dd 154 //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);
NaotoMorita 38:acc7cdcf56dd 155 //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
NaotoMorita 38:acc7cdcf56dd 156 //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 38:acc7cdcf56dd 157 //pc.printf("%f %f %f %f %f : %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
NaotoMorita 38:acc7cdcf56dd 158 //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);
NaotoMorita 38:acc7cdcf56dd 159 }
NaotoMorita 27:43bd0e444633 160
NaotoMorita 35:4535af88f7bf 161 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
cocorlow 7:70161eb0f854 162 {
cocorlow 7:70161eb0f854 163 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 164 }
NaotoMorita 27:43bd0e444633 165
NaotoMorita 38:acc7cdcf56dd 166 void calcMagRef(float mx, float my, float mz){
NaotoMorita 38:acc7cdcf56dd 167 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 168 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 169 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 170 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 171 float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2));
NaotoMorita 38:acc7cdcf56dd 172 float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1));
NaotoMorita 38:acc7cdcf56dd 173 float bx = sqrt(hx * hx + hy * hy);
NaotoMorita 38:acc7cdcf56dd 174 float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2));
NaotoMorita 38:acc7cdcf56dd 175 magref[0] = bx;
NaotoMorita 38:acc7cdcf56dd 176 magref[1] = 0.0;
NaotoMorita 38:acc7cdcf56dd 177 magref[2] = bz;
NaotoMorita 38:acc7cdcf56dd 178 }
NaotoMorita 38:acc7cdcf56dd 179
NaotoMorita 38:acc7cdcf56dd 180
NaotoMorita 38:acc7cdcf56dd 181 void getIMUval(){
NaotoMorita 38:acc7cdcf56dd 182 // gx gy gz ax ay az
NaotoMorita 38:acc7cdcf56dd 183 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
NaotoMorita 38:acc7cdcf56dd 184 ax = ax - agoffset[0];
NaotoMorita 38:acc7cdcf56dd 185 ay = ay - agoffset[1];
NaotoMorita 38:acc7cdcf56dd 186 az = -az - agoffset[2];
NaotoMorita 38:acc7cdcf56dd 187 gx = gx - agoffset[3];
NaotoMorita 38:acc7cdcf56dd 188 gy = gy - agoffset[4];
NaotoMorita 38:acc7cdcf56dd 189 gz = -gz - agoffset[5];
NaotoMorita 38:acc7cdcf56dd 190 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 38:acc7cdcf56dd 191 acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 38:acc7cdcf56dd 192 acc_y = float(ay) / ACCEL_SSF;
NaotoMorita 38:acc7cdcf56dd 193 acc_z = float(az) / ACCEL_SSF;
NaotoMorita 38:acc7cdcf56dd 194 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 38:acc7cdcf56dd 195 gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
NaotoMorita 38:acc7cdcf56dd 196 gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
NaotoMorita 38:acc7cdcf56dd 197 gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
NaotoMorita 38:acc7cdcf56dd 198 mag.getAxis(mdata); // flush the magnetmeter
NaotoMorita 38:acc7cdcf56dd 199 magval[0] = (mdata.x - magbias[0]);
NaotoMorita 38:acc7cdcf56dd 200 magval[1] = (mdata.y - magbias[1]);
NaotoMorita 38:acc7cdcf56dd 201 magval[2] = (mdata.z - magbias[2]);
NaotoMorita 38:acc7cdcf56dd 202 mag_x = -magval[0]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 203 mag_y = -magval[1]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 204 mag_z = -magval[2]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 205
NaotoMorita 40:869f3791a3e2 206 float lpc_acc = 0.15;
NaotoMorita 40:869f3791a3e2 207 LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x;
NaotoMorita 40:869f3791a3e2 208 LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y;
NaotoMorita 40:869f3791a3e2 209 LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z;
NaotoMorita 40:869f3791a3e2 210
NaotoMorita 40:869f3791a3e2 211 float lpc_mag = 0.15;
NaotoMorita 40:869f3791a3e2 212 LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x;
NaotoMorita 40:869f3791a3e2 213 LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y;
NaotoMorita 40:869f3791a3e2 214 LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z;
NaotoMorita 40:869f3791a3e2 215
NaotoMorita 38:acc7cdcf56dd 216 accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
NaotoMorita 38:acc7cdcf56dd 217 magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
NaotoMorita 40:869f3791a3e2 218 LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z);
NaotoMorita 40:869f3791a3e2 219 LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z);
NaotoMorita 38:acc7cdcf56dd 220 accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
NaotoMorita 38:acc7cdcf56dd 221 magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
NaotoMorita 40:869f3791a3e2 222 calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm);
NaotoMorita 40:869f3791a3e2 223
NaotoMorita 40:869f3791a3e2 224
NaotoMorita 38:acc7cdcf56dd 225 }
NaotoMorita 38:acc7cdcf56dd 226
NaotoMorita 38:acc7cdcf56dd 227 void updateBetweenMeasures(){
NaotoMorita 40:869f3791a3e2 228 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 40:869f3791a3e2 229 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 40:869f3791a3e2 230 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 40:869f3791a3e2 231 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 40:869f3791a3e2 232
NaotoMorita 40:869f3791a3e2 233 Matrix B(4,3);
NaotoMorita 40:869f3791a3e2 234 B << q1 << q2 << q3
NaotoMorita 40:869f3791a3e2 235 <<-q0 << q3 <<-q2
NaotoMorita 40:869f3791a3e2 236 <<-q3 <<-q0 << q1
NaotoMorita 40:869f3791a3e2 237 << q2 <<-q1 <<-q0;
NaotoMorita 40:869f3791a3e2 238 B *= 0.5f;
NaotoMorita 40:869f3791a3e2 239
NaotoMorita 38:acc7cdcf56dd 240 Matrix phi(4,4);
NaotoMorita 38:acc7cdcf56dd 241 phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 242 << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 243 << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 244 << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
NaotoMorita 38:acc7cdcf56dd 245 qhat = phi*qhat;
NaotoMorita 38:acc7cdcf56dd 246
NaotoMorita 38:acc7cdcf56dd 247 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 248 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 249
NaotoMorita 38:acc7cdcf56dd 250 Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
NaotoMorita 38:acc7cdcf56dd 251
NaotoMorita 40:869f3791a3e2 252 q0 = qhat.getNumber( 1, 1 );
NaotoMorita 40:869f3791a3e2 253 q1 = qhat.getNumber( 2, 1 );
NaotoMorita 40:869f3791a3e2 254 q2 = qhat.getNumber( 3, 1 );
NaotoMorita 40:869f3791a3e2 255 q3 = qhat.getNumber( 4, 1 );
NaotoMorita 40:869f3791a3e2 256
NaotoMorita 40:869f3791a3e2 257 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
NaotoMorita 40:869f3791a3e2 258 D.add(1,2,2*(q1*q2 + q0*q3));
NaotoMorita 40:869f3791a3e2 259 D.add(1,3,2*(q1*q3 - q0*q2));
NaotoMorita 40:869f3791a3e2 260 D.add(2,1,2*(q1*q2 - q0*q3));
NaotoMorita 40:869f3791a3e2 261 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
NaotoMorita 40:869f3791a3e2 262 D.add(2,3,2*(q2*q3 + q0*q1));
NaotoMorita 40:869f3791a3e2 263 D.add(3,1,2*(q1*q3 + q0*q2));
NaotoMorita 40:869f3791a3e2 264 D.add(3,2,2*(q2*q3 - q0*q1));
NaotoMorita 40:869f3791a3e2 265 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
NaotoMorita 38:acc7cdcf56dd 266
NaotoMorita 38:acc7cdcf56dd 267 }
NaotoMorita 38:acc7cdcf56dd 268
NaotoMorita 38:acc7cdcf56dd 269 void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
NaotoMorita 38:acc7cdcf56dd 270
NaotoMorita 38:acc7cdcf56dd 271 Matrix u(3,1);
NaotoMorita 38:acc7cdcf56dd 272 Matrix v(3,1);
NaotoMorita 38:acc7cdcf56dd 273
NaotoMorita 38:acc7cdcf56dd 274 u << u1 << u2 <<u3;
NaotoMorita 38:acc7cdcf56dd 275 v << v1 << v2 <<v3;
NaotoMorita 38:acc7cdcf56dd 276
NaotoMorita 38:acc7cdcf56dd 277 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 278 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 279 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 280 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 281
NaotoMorita 38:acc7cdcf56dd 282 Matrix A1(3,3);
NaotoMorita 38:acc7cdcf56dd 283 A1 << q0 << q3 << -q2
NaotoMorita 38:acc7cdcf56dd 284 <<-q3 << q0 << q1
NaotoMorita 38:acc7cdcf56dd 285 <<q2 <<-q1 <<q0;
NaotoMorita 38:acc7cdcf56dd 286 A1 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 287
NaotoMorita 38:acc7cdcf56dd 288 Matrix A2(3,3);
NaotoMorita 38:acc7cdcf56dd 289 A2 << q1 << q2 << q3
NaotoMorita 38:acc7cdcf56dd 290 << q2 <<-q1 << q0
NaotoMorita 38:acc7cdcf56dd 291 << q3 <<-q0 <<-q1;
NaotoMorita 38:acc7cdcf56dd 292 A2 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 293
NaotoMorita 38:acc7cdcf56dd 294 Matrix A3(3,3);
NaotoMorita 38:acc7cdcf56dd 295 A3 <<-q2 << q1 <<-q0
NaotoMorita 38:acc7cdcf56dd 296 << q1 << q2 << q3
NaotoMorita 38:acc7cdcf56dd 297 << q0 << q3 <<-q2;
NaotoMorita 38:acc7cdcf56dd 298 A3 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 299
NaotoMorita 38:acc7cdcf56dd 300 Matrix A4(3,3);
NaotoMorita 38:acc7cdcf56dd 301 A4 <<-q3 << q0 << q1
NaotoMorita 38:acc7cdcf56dd 302 <<-q0 <<-q3 << q2
NaotoMorita 38:acc7cdcf56dd 303 << q1 << q2 << q3;
NaotoMorita 38:acc7cdcf56dd 304 A4 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 305
NaotoMorita 38:acc7cdcf56dd 306 Matrix H(3,4);
NaotoMorita 38:acc7cdcf56dd 307
NaotoMorita 38:acc7cdcf56dd 308 Matrix ab1(A1*u);
NaotoMorita 38:acc7cdcf56dd 309 Matrix ab2(A2*u);
NaotoMorita 38:acc7cdcf56dd 310 Matrix ab3(A3*u);
NaotoMorita 38:acc7cdcf56dd 311 Matrix ab4(A4*u);
NaotoMorita 38:acc7cdcf56dd 312
NaotoMorita 38:acc7cdcf56dd 313 H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 314 << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 315 << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 316
NaotoMorita 38:acc7cdcf56dd 317
NaotoMorita 38:acc7cdcf56dd 318 Matrix K(4,3);
NaotoMorita 38:acc7cdcf56dd 319 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 38:acc7cdcf56dd 320
NaotoMorita 38:acc7cdcf56dd 321 Matrix dq(4,1);
NaotoMorita 38:acc7cdcf56dd 322 dq = K*(v-D*u);
NaotoMorita 38:acc7cdcf56dd 323 qhat = qhat+dq;
NaotoMorita 38:acc7cdcf56dd 324
NaotoMorita 38:acc7cdcf56dd 325 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 326 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 327
NaotoMorita 38:acc7cdcf56dd 328 Matrix eye4(4,4);
NaotoMorita 38:acc7cdcf56dd 329 eye4 << 1 << 0 << 0 << 0
NaotoMorita 38:acc7cdcf56dd 330 << 0 << 1 << 0 << 0
NaotoMorita 38:acc7cdcf56dd 331 << 0 << 0 << 1 << 0
NaotoMorita 38:acc7cdcf56dd 332 << 0 << 0 << 0 << 1;
NaotoMorita 38:acc7cdcf56dd 333 Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 38:acc7cdcf56dd 334 }
NaotoMorita 38:acc7cdcf56dd 335
NaotoMorita 38:acc7cdcf56dd 336 void computeAngles()
naker 20:2c3f113a8e8f 337 {
NaotoMorita 38:acc7cdcf56dd 338 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 339 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 340 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 341 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 342 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
NaotoMorita 38:acc7cdcf56dd 343 pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
NaotoMorita 38:acc7cdcf56dd 344 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 38:acc7cdcf56dd 345 }
NaotoMorita 38:acc7cdcf56dd 346
NaotoMorita 38:acc7cdcf56dd 347 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 348 Matrix W1(3,1);
NaotoMorita 38:acc7cdcf56dd 349 W1 << fb1 << fb2<< fb3;
NaotoMorita 38:acc7cdcf56dd 350 Matrix W2(3,1);
NaotoMorita 38:acc7cdcf56dd 351 W2 << mb1 << mb2<< mb3;
NaotoMorita 38:acc7cdcf56dd 352
NaotoMorita 38:acc7cdcf56dd 353 Matrix V1(3,1);
NaotoMorita 38:acc7cdcf56dd 354 V1 << fn1 << fn2<< fn3;
NaotoMorita 38:acc7cdcf56dd 355 Matrix V2(3,1);
NaotoMorita 38:acc7cdcf56dd 356 V2 << mn1 << mn2<< mn3;
NaotoMorita 38:acc7cdcf56dd 357
NaotoMorita 38:acc7cdcf56dd 358
NaotoMorita 38:acc7cdcf56dd 359 Matrix Ou2(3,1);
NaotoMorita 38:acc7cdcf56dd 360 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 361 Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
NaotoMorita 38:acc7cdcf56dd 362 Matrix Ou3(3,1);
NaotoMorita 38:acc7cdcf56dd 363 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 364 Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
NaotoMorita 38:acc7cdcf56dd 365 Matrix R2(3,1);
NaotoMorita 38:acc7cdcf56dd 366 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 367 R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
NaotoMorita 38:acc7cdcf56dd 368 Matrix R3(3,1);
NaotoMorita 38:acc7cdcf56dd 369 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 370 R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
NaotoMorita 38:acc7cdcf56dd 371
NaotoMorita 38:acc7cdcf56dd 372 Matrix Mou(3,3);
NaotoMorita 38:acc7cdcf56dd 373 Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 374 << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 375 << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 376 Matrix Mr(3,3);
NaotoMorita 38:acc7cdcf56dd 377 Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 378 << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 379 << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 380
NaotoMorita 38:acc7cdcf56dd 381 Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
NaotoMorita 38:acc7cdcf56dd 382
NaotoMorita 38:acc7cdcf56dd 383 float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
NaotoMorita 38:acc7cdcf56dd 384
NaotoMorita 38:acc7cdcf56dd 385 qhat.add(1,1,0.5*sqtrp1);
NaotoMorita 38:acc7cdcf56dd 386 qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 387 qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 388 qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 389
NaotoMorita 38:acc7cdcf56dd 390 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 391 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 392 }
NaotoMorita 38:acc7cdcf56dd 393
NaotoMorita 38:acc7cdcf56dd 394 void calcDynAcc(){
NaotoMorita 40:869f3791a3e2 395 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 396 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 397 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 398 }
NaotoMorita 38:acc7cdcf56dd 399
naker 42:24c93e42c3f4 400 void execCalibration() {
naker 42:24c93e42c3f4 401 pc.printf("\r\nEnter to Calibration Mode\r\n");
naker 42:24c93e42c3f4 402 wait(5);
naker 42:24c93e42c3f4 403 pc.printf("\r\n Acc and Gyro Calibration Start\r\n");
naker 42:24c93e42c3f4 404 int axs = 0;
naker 42:24c93e42c3f4 405 int ays = 0;
naker 42:24c93e42c3f4 406 int azs = 0;
naker 42:24c93e42c3f4 407 gxs = 0;
naker 42:24c93e42c3f4 408 gys = 0;
naker 42:24c93e42c3f4 409 gzs = 0;
naker 42:24c93e42c3f4 410
naker 42:24c93e42c3f4 411 int iter_n = 5000;
naker 42:24c93e42c3f4 412 for (int i = 0; i < iter_n; i++) {
naker 42:24c93e42c3f4 413 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
naker 42:24c93e42c3f4 414 axs += ax;
naker 42:24c93e42c3f4 415 ays += ay;
naker 42:24c93e42c3f4 416 azs -= az;
naker 42:24c93e42c3f4 417 gxs += gx;
naker 42:24c93e42c3f4 418 gys += gy;
naker 42:24c93e42c3f4 419 gzs -= gz;
naker 42:24c93e42c3f4 420 wait(0.001);
naker 42:24c93e42c3f4 421 }
naker 42:24c93e42c3f4 422 axs = axs / iter_n;
naker 42:24c93e42c3f4 423 ays = ays / iter_n;
naker 42:24c93e42c3f4 424 azs = azs / iter_n;
naker 42:24c93e42c3f4 425 gxs = gxs / iter_n;
naker 42:24c93e42c3f4 426 gys = gys / iter_n;
naker 42:24c93e42c3f4 427 gzs = gzs / iter_n;
naker 42:24c93e42c3f4 428 pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n", gxs, gys, gzs);
naker 42:24c93e42c3f4 429
naker 42:24c93e42c3f4 430 pc.printf("\r\n Mag Calibration Start\r\n");
naker 42:24c93e42c3f4 431 float f = 0;
naker 42:24c93e42c3f4 432 while (1) {
naker 42:24c93e42c3f4 433 mag.getAxis(mdata); // flush the magnetmeter
naker 42:24c93e42c3f4 434 magval[0] = (mdata.x - magbias[0]);
naker 42:24c93e42c3f4 435 magval[1] = (mdata.y - magbias[1]);
naker 42:24c93e42c3f4 436 magval[2] = (mdata.z - magbias[2]);
naker 42:24c93e42c3f4 437 float mag_r =
naker 42:24c93e42c3f4 438 magval[0] * magval[0] + magval[1] * magval[1] + magval[2] * magval[2];
naker 42:24c93e42c3f4 439 float lr = 0.00001f;
naker 42:24c93e42c3f4 440 f = mag_r - magbias[3] * magbias[3];
naker 42:24c93e42c3f4 441 magbias[0] = magbias[0] + 4 * lr * f * magval[0];
naker 42:24c93e42c3f4 442 magbias[1] = magbias[1] + 4 * lr * f * magval[1];
naker 42:24c93e42c3f4 443 magbias[2] = magbias[2] + 4 * lr * f * magval[2];
naker 42:24c93e42c3f4 444 magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
naker 42:24c93e42c3f4 445
naker 42:24c93e42c3f4 446 if (userButton.read() == 1) {
naker 42:24c93e42c3f4 447 break;
NaotoMorita 38:acc7cdcf56dd 448 }
naker 42:24c93e42c3f4 449 wait(0.001);
naker 42:24c93e42c3f4 450 }
naker 42:24c93e42c3f4 451 pc.printf("magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2],
naker 42:24c93e42c3f4 452 magbias[3]);
naker 42:24c93e42c3f4 453 pc.printf("\r\n Refvec Calibration waiting\r\n");
naker 42:24c93e42c3f4 454 wait(5);
naker 42:24c93e42c3f4 455 pc.printf("\r\n Calibration Start\r\n");
naker 42:24c93e42c3f4 456 float arefs[3] = {0.0, 0.0, 0.0};
naker 42:24c93e42c3f4 457 float mrefs[3] = {0.0, 0.0, 0.0};
naker 42:24c93e42c3f4 458 for (int i = 0; i < iter_n; i++) {
naker 42:24c93e42c3f4 459 getIMUval();
naker 42:24c93e42c3f4 460 arefs[0] += acc_x;
naker 42:24c93e42c3f4 461 arefs[1] += acc_y;
naker 42:24c93e42c3f4 462 arefs[2] += acc_z;
naker 42:24c93e42c3f4 463 mrefs[0] += mag_x;
naker 42:24c93e42c3f4 464 mrefs[1] += mag_y;
naker 42:24c93e42c3f4 465 mrefs[2] += mag_z;
naker 42:24c93e42c3f4 466 wait(0.001);
naker 42:24c93e42c3f4 467 }
naker 42:24c93e42c3f4 468 arefs[0] /= iter_n;
naker 42:24c93e42c3f4 469 arefs[1] /= iter_n;
naker 42:24c93e42c3f4 470 arefs[2] /= iter_n;
naker 42:24c93e42c3f4 471 mrefs[0] /= iter_n;
naker 42:24c93e42c3f4 472 mrefs[1] /= iter_n;
naker 42:24c93e42c3f4 473 mrefs[2] /= iter_n;
naker 42:24c93e42c3f4 474 pc.printf("\r\naccreg : %f, %f, %f\r\n", arefs[0], arefs[1], arefs[2]);
naker 42:24c93e42c3f4 475 pc.printf("\r\nmagreg : %f, %f, %f\r\n", mrefs[0], mrefs[1], mrefs[2]);
naker 42:24c93e42c3f4 476 // while(1) {wait(1000);}
naker 42:24c93e42c3f4 477 pc.printf("\r Writing to EEPROM...\r\n");
naker 42:24c93e42c3f4 478 U transfer_data;
naker 42:24c93e42c3f4 479 transfer_data.i[0] = 2;
naker 42:24c93e42c3f4 480 for (int i = 1; i < 5; i++) {
naker 42:24c93e42c3f4 481 if (!isnan(magbias[i - 1]))
naker 42:24c93e42c3f4 482 transfer_data.i[i] = (int)magbias[i - 1]; // intに丸めた値を送る。
naker 42:24c93e42c3f4 483 else {
naker 42:24c93e42c3f4 484 pc.printf("mag bias is NOT transferred\n");
naker 42:24c93e42c3f4 485 transfer_data.i[i] = 100;
NaotoMorita 38:acc7cdcf56dd 486 }
naker 42:24c93e42c3f4 487 }
naker 42:24c93e42c3f4 488 // gxs,gys,gzsを送る
naker 42:24c93e42c3f4 489 int gxyzs[3] = {gxs, gys, gzs};
naker 42:24c93e42c3f4 490 for (int i = 5; i < 8; i++) {
naker 42:24c93e42c3f4 491 if (!isnan(gxyzs[i - 5]))
naker 42:24c93e42c3f4 492 transfer_data.i[i] = gxyzs[i - 5];
naker 42:24c93e42c3f4 493 else {
naker 42:24c93e42c3f4 494 pc.printf("gxyzs is NOT transferred\n");
naker 42:24c93e42c3f4 495 transfer_data.i[i] = 0;
NaotoMorita 38:acc7cdcf56dd 496 }
naker 42:24c93e42c3f4 497 }
naker 42:24c93e42c3f4 498
naker 42:24c93e42c3f4 499 for (int i = 0; i < 8; i++) {
naker 42:24c93e42c3f4 500 pc.printf("transfer_data[%d]: %\d\n", i, transfer_data.i[i]);
naker 42:24c93e42c3f4 501 }
naker 42:24c93e42c3f4 502 writeEEPROM(address, pointeraddress, transfer_data.c, 32);
naker 42:24c93e42c3f4 503 wait(3);
naker 42:24c93e42c3f4 504 U read_test;
naker 42:24c93e42c3f4 505 readEEPROM(address, pointeraddress, read_test.c, 32);
naker 42:24c93e42c3f4 506 wait(3);
naker 42:24c93e42c3f4 507 for (int i = 0 ; i < 8; i ++){
naker 42:24c93e42c3f4 508 pc.printf("transfer_data[%d]: %d\n",i, read_test.i[i]);
naker 42:24c93e42c3f4 509 }
naker 20:2c3f113a8e8f 510 }
NaotoMorita 27:43bd0e444633 511
naker 42:24c93e42c3f4 512
naker 22:da9893b71f24 513 // 割り込まれた時点での出力(computeの結果）を返す関数
NaotoMorita 27:43bd0e444633 514 void calcServoOut(){
NaotoMorita 28:fc6c46c1db65 515 if(sbus.failSafe == false) {
NaotoMorita 28:fc6c46c1db65 516 // sbusデータの読み込み
NaotoMorita 28:fc6c46c1db65 517 for (int i =0 ; i < 16;i ++){
NaotoMorita 35:4535af88f7bf 518 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
NaotoMorita 27:43bd0e444633 519 }
NaotoMorita 28:fc6c46c1db65 520 }
NaotoMorita 28:fc6c46c1db65 521
NaotoMorita 28:fc6c46c1db65 522 pitchPID.setProcessValue(pitch);
NaotoMorita 28:fc6c46c1db65 523 pitchratePID.setProcessValue(gyro_y);
NaotoMorita 28:fc6c46c1db65 524 rollPID.setProcessValue(roll);
NaotoMorita 28:fc6c46c1db65 525 rollratePID.setProcessValue(gyro_x);
NaotoMorita 38:acc7cdcf56dd 526 float de = pitchPID.compute()+pitchratePID.compute()-rc[1]+rc[0];
NaotoMorita 36:e68ce293306e 527 float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1];
NaotoMorita 28:fc6c46c1db65 528 float dT = rc[2];
NaotoMorita 28:fc6c46c1db65 529
NaotoMorita 28:fc6c46c1db65 530 scaledServoOut[0]=de+da;
NaotoMorita 36:e68ce293306e 531 scaledServoOut[1]=-de+da;
NaotoMorita 28:fc6c46c1db65 532 scaledMotorOut[0]= dT;
NaotoMorita 31:8d02f3b9a067 533
NaotoMorita 38:acc7cdcf56dd 534 float LP_servo = 0.2;
NaotoMorita 38:acc7cdcf56dd 535 float LP_motor = 0.2;
NaotoMorita 36:e68ce293306e 536 for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
NaotoMorita 36:e68ce293306e 537 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
NaotoMorita 28:fc6c46c1db65 538 if(servoOut[i]<servoPwmMin) {
NaotoMorita 28:fc6c46c1db65 539 servoOut[i] = servoPwmMin;
NaotoMorita 28:fc6c46c1db65 540 };
NaotoMorita 28:fc6c46c1db65 541 if(servoOut[i]>servoPwmMax) {
NaotoMorita 28:fc6c46c1db65 542 servoOut[i] = servoPwmMax;
NaotoMorita 28:fc6c46c1db65 543 };
NaotoMorita 28:fc6c46c1db65 544 }
NaotoMorita 28:fc6c46c1db65 545
NaotoMorita 36:e68ce293306e 546 for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){
NaotoMorita 36:e68ce293306e 547 motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i];
NaotoMorita 28:fc6c46c1db65 548 if(motorOut[i]<motorPwmMin) {
NaotoMorita 28:fc6c46c1db65 549 motorOut[i] = motorPwmMin;
NaotoMorita 28:fc6c46c1db65 550 };
NaotoMorita 28:fc6c46c1db65 551 if(motorOut[i]>motorPwmMax) {
NaotoMorita 28:fc6c46c1db65 552 motorOut[i] = motorPwmMax;
NaotoMorita 28:fc6c46c1db65 553 };
NaotoMorita 28:fc6c46c1db65 554 }
NaotoMorita 36:e68ce293306e 555 servoRight.pulsewidth_us(servoOut[0]);
NaotoMorita 36:e68ce293306e 556 servoLeft.pulsewidth_us(servoOut[1]);
NaotoMorita 36:e68ce293306e 557 servoThrust.pulsewidth_us(motorOut[0]);
NaotoMorita 32:c4f06cb0e1d6 558
NaotoMorita 36:e68ce293306e 559 if(loop_count > 10){
NaotoMorita 31:8d02f3b9a067 560 writeSdcard();
NaotoMorita 36:e68ce293306e 561 loop_count = 1;
NaotoMorita 40:869f3791a3e2 562 obs_count = 0;
NaotoMorita 27:43bd0e444633 563 }else{
NaotoMorita 27:43bd0e444633 564 loop_count +=1;
naker 26:62d9857eaecc 565 }
naker 22:da9893b71f24 566 }
cocorlow 7:70161eb0f854 567
NaotoMorita 27:43bd0e444633 568 int main()
naker 42:24c93e42c3f4 569 {
naker 42:24c93e42c3f4 570 pc.baud(57600);
naker 42:24c93e42c3f4 571 U read_data; // eepromからの読み込み
naker 42:24c93e42c3f4 572 readEEPROM(address, pointeraddress, read_data.c, 32);
naker 42:24c93e42c3f4 573 pc.printf("%d",read_data.i[0]); // for debug
naker 42:24c93e42c3f4 574 if(read_data.i[0] < 0){
naker 42:24c93e42c3f4 575 pc.printf("No Data Stored in EEPROM!!!");
naker 42:24c93e42c3f4 576 execCalibration();
naker 42:24c93e42c3f4 577 }
naker 42:24c93e42c3f4 578 for (int i = 1 ; i < 5; i ++) magbias[i] = read_data.i[i];
naker 42:24c93e42c3f4 579 gxs = read_data.i[5];
naker 42:24c93e42c3f4 580 gys = read_data.i[6];
naker 42:24c93e42c3f4 581 gzs = read_data.i[7];
NaotoMorita 28:fc6c46c1db65 582 pitchPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 583 pitchratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 584 rollPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 585 rollratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 586 pitchPID.setBias(0.0);
naker 42:24c93e42c3f4 587 pitchratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 588 rollPID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 589 rollratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 590 pitchPID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 591 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 592 rollPID.setOutputLimits(-1.0,1.0);
NaotoMorita 29:34ee662f454e 593 rollratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 594 pitchPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 595 pitchratePID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 596 rollPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 597 rollratePID.setInputLimits(-pi,pi);
NaotoMorita 29:34ee662f454e 598
NaotoMorita 36:e68ce293306e 599 servoRight.period_us(15000.0);
NaotoMorita 36:e68ce293306e 600 servoLeft.period_us(15000.0);
NaotoMorita 36:e68ce293306e 601 servoThrust.period_us(15000.0);
NaotoMorita 36:e68ce293306e 602 servoRight.pulsewidth_us(1500.0);
NaotoMorita 36:e68ce293306e 603 servoLeft.pulsewidth_us(1500.0);
NaotoMorita 36:e68ce293306e 604 servoThrust.pulsewidth_us(1100.0);
NaotoMorita 36:e68ce293306e 605
NaotoMorita 38:acc7cdcf56dd 606 sd.baud(57600);
NaotoMorita 38:acc7cdcf56dd 607 sd.printf("\r\n Program Start \r\n");
NaotoMorita 36:e68ce293306e 608 accelgyro.initialize();
NaotoMorita 36:e68ce293306e 609 //加速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 610 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 38:acc7cdcf56dd 611 //角速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 612 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 36:e68ce293306e 613 //MPU6050のLPFを設定
NaotoMorita 36:e68ce293306e 614 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 38:acc7cdcf56dd 615 //地磁気
NaotoMorita 38:acc7cdcf56dd 616 mag.enable();
NaotoMorita 38:acc7cdcf56dd 617
NaotoMorita 38:acc7cdcf56dd 618 if(userButton.read() == 0){
NaotoMorita 38:acc7cdcf56dd 619 qhat << 1 << 0 << 0 << 0;
NaotoMorita 38:acc7cdcf56dd 620
NaotoMorita 38:acc7cdcf56dd 621 Phat << 0.01 << 0 <<0 <<0
NaotoMorita 38:acc7cdcf56dd 622 << 0 << 0.01 <<0 <<0
NaotoMorita 38:acc7cdcf56dd 623 << 0 << 0 <<0.001 <<0
NaotoMorita 38:acc7cdcf56dd 624 << 0 << 0 << 0 << 0.001;
NaotoMorita 38:acc7cdcf56dd 625
NaotoMorita 38:acc7cdcf56dd 626 Qgyro << 5.4732e-04 << 0 <<0
NaotoMorita 38:acc7cdcf56dd 627 << 0 <<5.4732e-04 <<0
NaotoMorita 38:acc7cdcf56dd 628 << 0 << 0 <<5.4732e-04;
naker 22:da9893b71f24 629
NaotoMorita 38:acc7cdcf56dd 630 Racc.add(1,1,1.0);
NaotoMorita 38:acc7cdcf56dd 631 Racc.add(2,2,1.0);
NaotoMorita 38:acc7cdcf56dd 632 Racc.add(3,3,1.0);
NaotoMorita 38:acc7cdcf56dd 633
NaotoMorita 38:acc7cdcf56dd 634 Rmag << 1 << 0 <<0
NaotoMorita 38:acc7cdcf56dd 635 << 0 << 1 <<0
NaotoMorita 38:acc7cdcf56dd 636 << 0 << 0 <<1;
NaotoMorita 38:acc7cdcf56dd 637 getIMUval();
NaotoMorita 38:acc7cdcf56dd 638 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 38:acc7cdcf56dd 639 for(int i = 0; i<1000 ;i++){
NaotoMorita 38:acc7cdcf56dd 640 getIMUval();
NaotoMorita 38:acc7cdcf56dd 641 val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
NaotoMorita 38:acc7cdcf56dd 642 }
NaotoMorita 38:acc7cdcf56dd 643 val_thmg /= 1000;
NaotoMorita 38:acc7cdcf56dd 644 LoopTicker PIDtick;
NaotoMorita 38:acc7cdcf56dd 645 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 38:acc7cdcf56dd 646
NaotoMorita 38:acc7cdcf56dd 647 t.start();
NaotoMorita 28:fc6c46c1db65 648
NaotoMorita 38:acc7cdcf56dd 649 while(1) {
NaotoMorita 38:acc7cdcf56dd 650 float tstart = t.read();
NaotoMorita 38:acc7cdcf56dd 651 //姿勢角を更新
NaotoMorita 38:acc7cdcf56dd 652 getIMUval();
NaotoMorita 38:acc7cdcf56dd 653 updateBetweenMeasures();
NaotoMorita 38:acc7cdcf56dd 654 calcDynAcc();
NaotoMorita 40:869f3791a3e2 655 th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg);
NaotoMorita 40:869f3791a3e2 656 dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
NaotoMorita 40:869f3791a3e2 657 accnormerr = abs(LPaccnorm-accrefnorm);
NaotoMorita 38:acc7cdcf56dd 658 //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref));
NaotoMorita 40:869f3791a3e2 659 int ang_th = th_mg<0.01;
NaotoMorita 40:869f3791a3e2 660 int dyn_th = dynaccnorm < 0.01;
NaotoMorita 40:869f3791a3e2 661 int norm_th = accnormerr< 0.01;
NaotoMorita 38:acc7cdcf56dd 662 //pc.printf("%d %d %d %f %f %f\r\n",ang_th,dyn_th,norm_th,abs(th_mg-val_thmg),dynaccnorm ,abs(accnorm-accrefnorm));
NaotoMorita 40:869f3791a3e2 663 if(dyn_th+ang_th+norm_th>0){
NaotoMorita 40:869f3791a3e2 664 obs_count += 1;
NaotoMorita 40:869f3791a3e2 665 updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
NaotoMorita 40:869f3791a3e2 666 updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
NaotoMorita 38:acc7cdcf56dd 667 }
NaotoMorita 38:acc7cdcf56dd 668 computeAngles();
NaotoMorita 38:acc7cdcf56dd 669 PIDtick.loop();
NaotoMorita 38:acc7cdcf56dd 670
NaotoMorita 38:acc7cdcf56dd 671 float tend = t.read();
NaotoMorita 38:acc7cdcf56dd 672 att_dt = (tend-tstart);
NaotoMorita 38:acc7cdcf56dd 673 }
NaotoMorita 38:acc7cdcf56dd 674 }else{
NaotoMorita 38:acc7cdcf56dd 675 execCalibration();
naker 20:2c3f113a8e8f 676 }
NaotoMorita 0:6b18a09a6628 677 }