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@50:e07ffa9f565a, 2021-05-19 (annotated)

Committer:
hiroki_u
Date:
Wed May 19 06:31:09 2021 +0000
Revision:
50:e07ffa9f565a
Parent:
48:1065506191f2 
uchiyama

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiroki_u 50:e07ffa9f565a 1 // uchiyama
NaotoMorita 0:6b18a09a6628 2 #include "mbed.h"
naker 20:2c3f113a8e8f 3 #include "PIDcontroller.h"
NaotoMorita 0:6b18a09a6628 4 #include "SBUS.hpp"
cocorlow 6:2cba569272fe 5 #include "LoopTicker.hpp"
naker 15:6abaac6e5b03 6 #include "MPU6050.h"
NaotoMorita 38:acc7cdcf56dd 7 #include "MAG3110.h"
NaotoMorita 38:acc7cdcf56dd 8 #include "I2Cdev.h"
NaotoMorita 36:e68ce293306e 9 #include "FastPWM.h"
NaotoMorita 36:e68ce293306e 10 #include "Matrix.h"
NaotoMorita 36:e68ce293306e 11 #include "MatrixMath.h"
NaotoMorita 38:acc7cdcf56dd 12 #include "math.h"
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 38:acc7cdcf56dd 28 #define pitch_align 8.0*3.141562/180.0
NaotoMorita 38:acc7cdcf56dd 29 #define roll_align -2.8*3.141562/180.0
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);
NaotoMorita 0:6b18a09a6628 34 Serial pc(USBTX, USBRX);
NaotoMorita 38:acc7cdcf56dd 35 DigitalIn userButton(USER_BUTTON);
NaotoMorita 48:1065506191f2 36 FastPWM servo(PE_9);
NaotoMorita 36:e68ce293306e 37 PID pitchPID(5.0, 0,0,PID_dt); //rad
NaotoMorita 38:acc7cdcf56dd 38 PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s
NaotoMorita 17:6b7a363d06d2 39 Timer t;
cocorlow 3:79e62f9b13c8 40
NaotoMorita 36:e68ce293306e 41 Matrix qhat(4,1);
NaotoMorita 44:f571273d3223 42 Matrix qhat_gyro(4,1);
NaotoMorita 36:e68ce293306e 43 Matrix Phat(4,4);
NaotoMorita 37:dad55cf05e3d 44 Matrix Qgyro(3,3);
NaotoMorita 36:e68ce293306e 45 Matrix Racc(3,3);
NaotoMorita 38:acc7cdcf56dd 46 Matrix Rmag(3,3);
NaotoMorita 38:acc7cdcf56dd 47 Matrix D(3,3);
NaotoMorita 36:e68ce293306e 48
NaotoMorita 38:acc7cdcf56dd 49 int loop_count = 0;
NaotoMorita 40:869f3791a3e2 50 int obs_count = 0;
NaotoMorita 36:e68ce293306e 51 float att_dt = 0.01;
naker 23:4a490fa8bf4a 52 float rc[16];
NaotoMorita 38:acc7cdcf56dd 53 float pitch = 0.0;
NaotoMorita 38:acc7cdcf56dd 54 float roll = 0.0;
NaotoMorita 38:acc7cdcf56dd 55 float yaw = 0.0;
NaotoMorita 44:f571273d3223 56
NaotoMorita 44:f571273d3223 57 float pitch_g = 0.0;
NaotoMorita 44:f571273d3223 58 float roll_g = 0.0;
NaotoMorita 44:f571273d3223 59 float yaw_g = 0.0;
NaotoMorita 44:f571273d3223 60
NaotoMorita 17:6b7a363d06d2 61 int16_t ax, ay, az;
NaotoMorita 17:6b7a363d06d2 62 int16_t gx, gy, gz;
NaotoMorita 38:acc7cdcf56dd 63 MotionSensorDataUnits mdata;
NaotoMorita 38:acc7cdcf56dd 64 float magval[3] = {1.0,0.0,0.0};
NaotoMorita 38:acc7cdcf56dd 65 float acc_x,acc_y,acc_z;
NaotoMorita 38:acc7cdcf56dd 66 float dynacc_x,dynacc_y,dynacc_z;
NaotoMorita 38:acc7cdcf56dd 67 float gyro_x,gyro_y,gyro_z;
NaotoMorita 38:acc7cdcf56dd 68 float mag_x,mag_y,mag_z;
NaotoMorita 40:869f3791a3e2 69
NaotoMorita 40:869f3791a3e2 70 float LPacc_x=0.0;
NaotoMorita 40:869f3791a3e2 71 float LPacc_y=0.0;
NaotoMorita 40:869f3791a3e2 72 float LPacc_z=0.0;
NaotoMorita 40:869f3791a3e2 73 float LPmag_x = 0.0;
NaotoMorita 40:869f3791a3e2 74 float LPmag_y = 0.0;
NaotoMorita 40:869f3791a3e2 75 float LPmag_z = 0.0;;
NaotoMorita 40:869f3791a3e2 76
cocorlow 3:79e62f9b13c8 77 int out1, out2;
NaotoMorita 48:1065506191f2 78 float scaledServoOut[1]= {0};
NaotoMorita 48:1065506191f2 79 float servoOut[1] = {1500.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 44:f571273d3223 96 int agoffset[6] = {0, 0, 0, -117, -563, 2 };
NaotoMorita 44:f571273d3223 97 float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610};
cocorlow 3:79e62f9b13c8 98
NaotoMorita 38:acc7cdcf56dd 99 void writeSdcard()
NaotoMorita 38:acc7cdcf56dd 100 {
NaotoMorita 38:acc7cdcf56dd 101 //magcal.getExtremes(&magmin[0],&magmax[0]);
NaotoMorita 38:acc7cdcf56dd 102 //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 103 //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 48:1065506191f2 104 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);
NaotoMorita 44:f571273d3223 105 //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 106 //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 107 //pc.printf("%d \r\n",userButton.read());
NaotoMorita 38:acc7cdcf56dd 108 //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 109 //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi);
NaotoMorita 38:acc7cdcf56dd 110 //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 111 //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 112 //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 113 }
NaotoMorita 27:43bd0e444633 114
NaotoMorita 35:4535af88f7bf 115 float mapfloat(float x, float in_min, float in_max, float out_min, float out_max)
cocorlow 7:70161eb0f854 116 {
cocorlow 7:70161eb0f854 117 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
cocorlow 7:70161eb0f854 118 }
NaotoMorita 27:43bd0e444633 119
NaotoMorita 38:acc7cdcf56dd 120 void calcMagRef(float mx, float my, float mz){
NaotoMorita 44:f571273d3223 121
NaotoMorita 44:f571273d3223 122 Matrix magvec(3,1);
NaotoMorita 44:f571273d3223 123 magvec << mx << my << mz;
NaotoMorita 44:f571273d3223 124 Matrix magnedvec = MatrixMath::Transpose(D)*magvec;
NaotoMorita 44:f571273d3223 125 magref[0] = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
NaotoMorita 38:acc7cdcf56dd 126 magref[1] = 0.0;
NaotoMorita 44:f571273d3223 127 magref[2] = magnedvec(3,1);
NaotoMorita 38:acc7cdcf56dd 128 }
NaotoMorita 38:acc7cdcf56dd 129
NaotoMorita 38:acc7cdcf56dd 130
NaotoMorita 38:acc7cdcf56dd 131 void getIMUval(){
NaotoMorita 38:acc7cdcf56dd 132 // gx gy gz ax ay az
NaotoMorita 38:acc7cdcf56dd 133 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
NaotoMorita 38:acc7cdcf56dd 134 ax = ax - agoffset[0];
NaotoMorita 38:acc7cdcf56dd 135 ay = ay - agoffset[1];
NaotoMorita 38:acc7cdcf56dd 136 az = -az - agoffset[2];
NaotoMorita 38:acc7cdcf56dd 137 gx = gx - agoffset[3];
NaotoMorita 38:acc7cdcf56dd 138 gy = gy - agoffset[4];
NaotoMorita 38:acc7cdcf56dd 139 gz = -gz - agoffset[5];
NaotoMorita 38:acc7cdcf56dd 140 // 加速度値を分解能で割って加速度(G)に変換する
NaotoMorita 38:acc7cdcf56dd 141 acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g
NaotoMorita 38:acc7cdcf56dd 142 acc_y = float(ay) / ACCEL_SSF;
NaotoMorita 38:acc7cdcf56dd 143 acc_z = float(az) / ACCEL_SSF;
NaotoMorita 38:acc7cdcf56dd 144 // 角速度値を分解能で割って角速度(rad per sec)に変換する
NaotoMorita 38:acc7cdcf56dd 145 gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s)
NaotoMorita 38:acc7cdcf56dd 146 gyro_y = float(gy) / GYRO_SSF * 0.0174533f;
NaotoMorita 38:acc7cdcf56dd 147 gyro_z = float(gz) / GYRO_SSF * 0.0174533f;
NaotoMorita 38:acc7cdcf56dd 148 mag.getAxis(mdata); // flush the magnetmeter
NaotoMorita 38:acc7cdcf56dd 149 magval[0] = (mdata.x - magbias[0]);
NaotoMorita 38:acc7cdcf56dd 150 magval[1] = (mdata.y - magbias[1]);
NaotoMorita 38:acc7cdcf56dd 151 magval[2] = (mdata.z - magbias[2]);
NaotoMorita 38:acc7cdcf56dd 152 mag_x = -magval[0]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 153 mag_y = -magval[1]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 154 mag_z = -magval[2]/magbias[3];
NaotoMorita 38:acc7cdcf56dd 155
NaotoMorita 40:869f3791a3e2 156 float lpc_acc = 0.15;
NaotoMorita 40:869f3791a3e2 157 LPacc_x = lpc_acc*acc_x+(1.0-lpc_acc)*LPacc_x;
NaotoMorita 40:869f3791a3e2 158 LPacc_y = lpc_acc*acc_y+(1.0-lpc_acc)*LPacc_y;
NaotoMorita 40:869f3791a3e2 159 LPacc_z = lpc_acc*acc_z+(1.0-lpc_acc)*LPacc_z;
NaotoMorita 40:869f3791a3e2 160
NaotoMorita 40:869f3791a3e2 161 float lpc_mag = 0.15;
NaotoMorita 40:869f3791a3e2 162 LPmag_x = lpc_mag*mag_x+(1.0-lpc_mag)*LPmag_x;
NaotoMorita 40:869f3791a3e2 163 LPmag_y = lpc_mag*mag_y+(1.0-lpc_mag)*LPmag_y;
NaotoMorita 40:869f3791a3e2 164 LPmag_z = lpc_mag*mag_z+(1.0-lpc_mag)*LPmag_z;
NaotoMorita 40:869f3791a3e2 165
NaotoMorita 38:acc7cdcf56dd 166 accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z);
NaotoMorita 38:acc7cdcf56dd 167 magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z);
NaotoMorita 40:869f3791a3e2 168 LPaccnorm = sqrt(LPacc_x*LPacc_x+LPacc_y*LPacc_y+LPacc_z*LPacc_z);
NaotoMorita 40:869f3791a3e2 169 LPmagnorm = sqrt(LPmag_x*LPmag_x+LPmag_y*LPmag_y+LPmag_z*LPmag_z);
NaotoMorita 38:acc7cdcf56dd 170 accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]);
NaotoMorita 38:acc7cdcf56dd 171 magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]);
NaotoMorita 40:869f3791a3e2 172 calcMagRef(LPmag_x/LPmagnorm, LPmag_y/LPmagnorm, LPmag_z/LPmagnorm);
NaotoMorita 40:869f3791a3e2 173
NaotoMorita 40:869f3791a3e2 174
NaotoMorita 38:acc7cdcf56dd 175 }
NaotoMorita 38:acc7cdcf56dd 176
NaotoMorita 38:acc7cdcf56dd 177 void updateBetweenMeasures(){
NaotoMorita 40:869f3791a3e2 178 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 40:869f3791a3e2 179 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 40:869f3791a3e2 180 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 40:869f3791a3e2 181 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 40:869f3791a3e2 182
NaotoMorita 40:869f3791a3e2 183 Matrix B(4,3);
NaotoMorita 40:869f3791a3e2 184 B << q1 << q2 << q3
NaotoMorita 40:869f3791a3e2 185 <<-q0 << q3 <<-q2
NaotoMorita 40:869f3791a3e2 186 <<-q3 <<-q0 << q1
NaotoMorita 40:869f3791a3e2 187 << q2 <<-q1 <<-q0;
NaotoMorita 40:869f3791a3e2 188 B *= 0.5f;
NaotoMorita 40:869f3791a3e2 189
NaotoMorita 38:acc7cdcf56dd 190 Matrix phi(4,4);
NaotoMorita 38:acc7cdcf56dd 191 phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 192 << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 193 << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt
NaotoMorita 38:acc7cdcf56dd 194 << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0;
NaotoMorita 44:f571273d3223 195
NaotoMorita 38:acc7cdcf56dd 196 qhat = phi*qhat;
NaotoMorita 38:acc7cdcf56dd 197 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 198 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 199
NaotoMorita 44:f571273d3223 200 qhat_gyro = phi*qhat_gyro;
NaotoMorita 44:f571273d3223 201 qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro));
NaotoMorita 44:f571273d3223 202 qhat_gyro *= (1.0f/ qnorm);
NaotoMorita 44:f571273d3223 203
NaotoMorita 38:acc7cdcf56dd 204 Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B);
NaotoMorita 38:acc7cdcf56dd 205
NaotoMorita 40:869f3791a3e2 206 q0 = qhat.getNumber( 1, 1 );
NaotoMorita 40:869f3791a3e2 207 q1 = qhat.getNumber( 2, 1 );
NaotoMorita 40:869f3791a3e2 208 q2 = qhat.getNumber( 3, 1 );
NaotoMorita 40:869f3791a3e2 209 q3 = qhat.getNumber( 4, 1 );
NaotoMorita 40:869f3791a3e2 210
NaotoMorita 40:869f3791a3e2 211 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
NaotoMorita 40:869f3791a3e2 212 D.add(1,2,2*(q1*q2 + q0*q3));
NaotoMorita 40:869f3791a3e2 213 D.add(1,3,2*(q1*q3 - q0*q2));
NaotoMorita 40:869f3791a3e2 214 D.add(2,1,2*(q1*q2 - q0*q3));
NaotoMorita 40:869f3791a3e2 215 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
NaotoMorita 40:869f3791a3e2 216 D.add(2,3,2*(q2*q3 + q0*q1));
NaotoMorita 40:869f3791a3e2 217 D.add(3,1,2*(q1*q3 + q0*q2));
NaotoMorita 40:869f3791a3e2 218 D.add(3,2,2*(q2*q3 - q0*q1));
NaotoMorita 40:869f3791a3e2 219 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
NaotoMorita 38:acc7cdcf56dd 220
NaotoMorita 38:acc7cdcf56dd 221 }
NaotoMorita 38:acc7cdcf56dd 222
NaotoMorita 38:acc7cdcf56dd 223 void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){
NaotoMorita 38:acc7cdcf56dd 224
NaotoMorita 38:acc7cdcf56dd 225 Matrix u(3,1);
NaotoMorita 38:acc7cdcf56dd 226 Matrix v(3,1);
NaotoMorita 38:acc7cdcf56dd 227
NaotoMorita 38:acc7cdcf56dd 228 u << u1 << u2 <<u3;
NaotoMorita 38:acc7cdcf56dd 229 v << v1 << v2 <<v3;
NaotoMorita 38:acc7cdcf56dd 230
NaotoMorita 38:acc7cdcf56dd 231 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 232 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 233 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 234 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 235
NaotoMorita 38:acc7cdcf56dd 236 Matrix A1(3,3);
NaotoMorita 38:acc7cdcf56dd 237 A1 << q0 << q3 << -q2
NaotoMorita 38:acc7cdcf56dd 238 <<-q3 << q0 << q1
NaotoMorita 38:acc7cdcf56dd 239 <<q2 <<-q1 <<q0;
NaotoMorita 38:acc7cdcf56dd 240 A1 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 241
NaotoMorita 38:acc7cdcf56dd 242 Matrix A2(3,3);
NaotoMorita 38:acc7cdcf56dd 243 A2 << q1 << q2 << q3
NaotoMorita 38:acc7cdcf56dd 244 << q2 <<-q1 << q0
NaotoMorita 38:acc7cdcf56dd 245 << q3 <<-q0 <<-q1;
NaotoMorita 38:acc7cdcf56dd 246 A2 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 247
NaotoMorita 38:acc7cdcf56dd 248 Matrix A3(3,3);
NaotoMorita 38:acc7cdcf56dd 249 A3 <<-q2 << q1 <<-q0
NaotoMorita 38:acc7cdcf56dd 250 << q1 << q2 << q3
NaotoMorita 38:acc7cdcf56dd 251 << q0 << q3 <<-q2;
NaotoMorita 38:acc7cdcf56dd 252 A3 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 253
NaotoMorita 38:acc7cdcf56dd 254 Matrix A4(3,3);
NaotoMorita 38:acc7cdcf56dd 255 A4 <<-q3 << q0 << q1
NaotoMorita 38:acc7cdcf56dd 256 <<-q0 <<-q3 << q2
NaotoMorita 38:acc7cdcf56dd 257 << q1 << q2 << q3;
NaotoMorita 38:acc7cdcf56dd 258 A4 *= 2.0f;
NaotoMorita 38:acc7cdcf56dd 259
NaotoMorita 38:acc7cdcf56dd 260 Matrix H(3,4);
NaotoMorita 38:acc7cdcf56dd 261
NaotoMorita 38:acc7cdcf56dd 262 Matrix ab1(A1*u);
NaotoMorita 38:acc7cdcf56dd 263 Matrix ab2(A2*u);
NaotoMorita 38:acc7cdcf56dd 264 Matrix ab3(A3*u);
NaotoMorita 38:acc7cdcf56dd 265 Matrix ab4(A4*u);
NaotoMorita 38:acc7cdcf56dd 266
NaotoMorita 38:acc7cdcf56dd 267 H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 268 << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 269 << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 270
NaotoMorita 38:acc7cdcf56dd 271
NaotoMorita 38:acc7cdcf56dd 272 Matrix K(4,3);
NaotoMorita 38:acc7cdcf56dd 273 K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 38:acc7cdcf56dd 274
NaotoMorita 38:acc7cdcf56dd 275 Matrix dq(4,1);
NaotoMorita 38:acc7cdcf56dd 276 dq = K*(v-D*u);
NaotoMorita 38:acc7cdcf56dd 277 qhat = qhat+dq;
NaotoMorita 38:acc7cdcf56dd 278
NaotoMorita 38:acc7cdcf56dd 279 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 280 qhat *= (1.0f/ qnorm);
NaotoMorita 38:acc7cdcf56dd 281
NaotoMorita 38:acc7cdcf56dd 282 Matrix eye4(4,4);
NaotoMorita 38:acc7cdcf56dd 283 eye4 << 1 << 0 << 0 << 0
NaotoMorita 38:acc7cdcf56dd 284 << 0 << 1 << 0 << 0
NaotoMorita 38:acc7cdcf56dd 285 << 0 << 0 << 1 << 0
NaotoMorita 38:acc7cdcf56dd 286 << 0 << 0 << 0 << 1;
NaotoMorita 38:acc7cdcf56dd 287 Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 38:acc7cdcf56dd 288 }
NaotoMorita 38:acc7cdcf56dd 289
NaotoMorita 38:acc7cdcf56dd 290 void computeAngles()
naker 20:2c3f113a8e8f 291 {
NaotoMorita 38:acc7cdcf56dd 292 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 38:acc7cdcf56dd 293 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 38:acc7cdcf56dd 294 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 295 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 38:acc7cdcf56dd 296 roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
NaotoMorita 38:acc7cdcf56dd 297 pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
NaotoMorita 38:acc7cdcf56dd 298 yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 299
NaotoMorita 44:f571273d3223 300 q0 = qhat_gyro.getNumber( 1, 1 );
NaotoMorita 44:f571273d3223 301 q1 = qhat_gyro.getNumber( 2, 1 );
NaotoMorita 44:f571273d3223 302 q2 = qhat_gyro.getNumber( 3, 1 );
NaotoMorita 44:f571273d3223 303 q3 = qhat_gyro.getNumber( 4, 1 );
NaotoMorita 44:f571273d3223 304 roll_g = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align;
NaotoMorita 44:f571273d3223 305 pitch_g = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align;
NaotoMorita 44:f571273d3223 306 yaw_g = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 307
NaotoMorita 38:acc7cdcf56dd 308 }
NaotoMorita 38:acc7cdcf56dd 309
NaotoMorita 38:acc7cdcf56dd 310 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 311 Matrix W1(3,1);
NaotoMorita 38:acc7cdcf56dd 312 W1 << fb1 << fb2<< fb3;
NaotoMorita 38:acc7cdcf56dd 313 Matrix W2(3,1);
NaotoMorita 38:acc7cdcf56dd 314 W2 << mb1 << mb2<< mb3;
NaotoMorita 38:acc7cdcf56dd 315
NaotoMorita 38:acc7cdcf56dd 316 Matrix V1(3,1);
NaotoMorita 38:acc7cdcf56dd 317 V1 << fn1 << fn2<< fn3;
NaotoMorita 38:acc7cdcf56dd 318 Matrix V2(3,1);
NaotoMorita 38:acc7cdcf56dd 319 V2 << mn1 << mn2<< mn3;
NaotoMorita 38:acc7cdcf56dd 320
NaotoMorita 38:acc7cdcf56dd 321
NaotoMorita 38:acc7cdcf56dd 322 Matrix Ou2(3,1);
NaotoMorita 38:acc7cdcf56dd 323 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 324 Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2));
NaotoMorita 38:acc7cdcf56dd 325 Matrix Ou3(3,1);
NaotoMorita 38:acc7cdcf56dd 326 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 327 Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3));
NaotoMorita 38:acc7cdcf56dd 328 Matrix R2(3,1);
NaotoMorita 38:acc7cdcf56dd 329 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 330 R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2));
NaotoMorita 38:acc7cdcf56dd 331 Matrix R3(3,1);
NaotoMorita 38:acc7cdcf56dd 332 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 333 R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3));
NaotoMorita 38:acc7cdcf56dd 334
NaotoMorita 38:acc7cdcf56dd 335 Matrix Mou(3,3);
NaotoMorita 38:acc7cdcf56dd 336 Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 337 << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 338 << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 339 Matrix Mr(3,3);
NaotoMorita 38:acc7cdcf56dd 340 Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 )
NaotoMorita 38:acc7cdcf56dd 341 << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 )
NaotoMorita 38:acc7cdcf56dd 342 << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 );
NaotoMorita 38:acc7cdcf56dd 343
NaotoMorita 38:acc7cdcf56dd 344 Matrix Cbn = Mr*MatrixMath::Transpose(Mou);
NaotoMorita 38:acc7cdcf56dd 345
NaotoMorita 38:acc7cdcf56dd 346 float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 ));
NaotoMorita 38:acc7cdcf56dd 347
NaotoMorita 38:acc7cdcf56dd 348 qhat.add(1,1,0.5*sqtrp1);
NaotoMorita 38:acc7cdcf56dd 349 qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 350 qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 351 qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1);
NaotoMorita 38:acc7cdcf56dd 352
NaotoMorita 38:acc7cdcf56dd 353 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 38:acc7cdcf56dd 354 qhat *= (1.0f/ qnorm);
NaotoMorita 44:f571273d3223 355
NaotoMorita 44:f571273d3223 356 qhat_gyro = qhat;
NaotoMorita 44:f571273d3223 357
NaotoMorita 44:f571273d3223 358 float q0 = qhat.getNumber( 1, 1 );
NaotoMorita 44:f571273d3223 359 float q1 = qhat.getNumber( 2, 1 );
NaotoMorita 44:f571273d3223 360 float q2 = qhat.getNumber( 3, 1 );
NaotoMorita 44:f571273d3223 361 float q3 = qhat.getNumber( 4, 1 );
NaotoMorita 44:f571273d3223 362
NaotoMorita 44:f571273d3223 363 D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 364 D.add(1,2,2*(q1*q2 + q0*q3));
NaotoMorita 44:f571273d3223 365 D.add(1,3,2*(q1*q3 - q0*q2));
NaotoMorita 44:f571273d3223 366 D.add(2,1,2*(q1*q2 - q0*q3));
NaotoMorita 44:f571273d3223 367 D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3);
NaotoMorita 44:f571273d3223 368 D.add(2,3,2*(q2*q3 + q0*q1));
NaotoMorita 44:f571273d3223 369 D.add(3,1,2*(q1*q3 + q0*q2));
NaotoMorita 44:f571273d3223 370 D.add(3,2,2*(q2*q3 - q0*q1));
NaotoMorita 44:f571273d3223 371 D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3);
NaotoMorita 44:f571273d3223 372
NaotoMorita 38:acc7cdcf56dd 373 }
NaotoMorita 38:acc7cdcf56dd 374
NaotoMorita 38:acc7cdcf56dd 375 void calcDynAcc(){
NaotoMorita 40:869f3791a3e2 376 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 377 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 378 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 379 }
NaotoMorita 38:acc7cdcf56dd 380
NaotoMorita 38:acc7cdcf56dd 381 void execCalibration(){
NaotoMorita 38:acc7cdcf56dd 382 pc.printf("\r\nEnter to Calibration Mode\r\n");
NaotoMorita 38:acc7cdcf56dd 383 wait(5);
NaotoMorita 38:acc7cdcf56dd 384 pc.printf("\r\n Acc and Gyro Calibration Start\r\n");
NaotoMorita 38:acc7cdcf56dd 385 int axs = 0;
NaotoMorita 38:acc7cdcf56dd 386 int ays = 0;
NaotoMorita 38:acc7cdcf56dd 387 int azs = 0;
NaotoMorita 38:acc7cdcf56dd 388 int gxs = 0;
NaotoMorita 38:acc7cdcf56dd 389 int gys = 0;
NaotoMorita 38:acc7cdcf56dd 390 int gzs = 0;
NaotoMorita 38:acc7cdcf56dd 391 int iter_n = 5000;
NaotoMorita 38:acc7cdcf56dd 392 for(int i = 0;i<iter_n ;i++){
NaotoMorita 38:acc7cdcf56dd 393 accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz);
NaotoMorita 38:acc7cdcf56dd 394 axs += ax;
NaotoMorita 38:acc7cdcf56dd 395 ays += ay;
NaotoMorita 38:acc7cdcf56dd 396 azs -= az;
NaotoMorita 38:acc7cdcf56dd 397 gxs += gx;
NaotoMorita 38:acc7cdcf56dd 398 gys += gy;
NaotoMorita 38:acc7cdcf56dd 399 gzs -= gz;
NaotoMorita 38:acc7cdcf56dd 400 wait(0.001);
NaotoMorita 38:acc7cdcf56dd 401 }
NaotoMorita 38:acc7cdcf56dd 402 axs = axs /iter_n;
NaotoMorita 38:acc7cdcf56dd 403 ays = ays /iter_n;
NaotoMorita 38:acc7cdcf56dd 404 azs = azs /iter_n;
NaotoMorita 38:acc7cdcf56dd 405 gxs = gxs /iter_n;
NaotoMorita 38:acc7cdcf56dd 406 gys = gys /iter_n;
NaotoMorita 38:acc7cdcf56dd 407 gzs = gzs /iter_n;
NaotoMorita 38:acc7cdcf56dd 408 pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs);
NaotoMorita 38:acc7cdcf56dd 409
NaotoMorita 38:acc7cdcf56dd 410 pc.printf("\r\n Mag Calibration Start\r\n");
NaotoMorita 38:acc7cdcf56dd 411 float f = 0;
NaotoMorita 38:acc7cdcf56dd 412 while(1){
NaotoMorita 38:acc7cdcf56dd 413 mag.getAxis(mdata); // flush the magnetmeter
NaotoMorita 38:acc7cdcf56dd 414 magval[0] = (mdata.x - magbias[0]);
NaotoMorita 38:acc7cdcf56dd 415 magval[1] = (mdata.y - magbias[1]);
NaotoMorita 38:acc7cdcf56dd 416 magval[2] = (mdata.z - magbias[2]);
NaotoMorita 38:acc7cdcf56dd 417 float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2];
NaotoMorita 38:acc7cdcf56dd 418 float lr = 0.00001f;
NaotoMorita 38:acc7cdcf56dd 419 f = mag_r - magbias[3]*magbias[3];
NaotoMorita 38:acc7cdcf56dd 420 magbias[0] = magbias[0] + 4 * lr * f * magval[0];
NaotoMorita 38:acc7cdcf56dd 421 magbias[1] = magbias[1] + 4 * lr * f * magval[1];
NaotoMorita 38:acc7cdcf56dd 422 magbias[2] = magbias[2] + 4 * lr * f * magval[2];
NaotoMorita 38:acc7cdcf56dd 423 magbias[3] = magbias[3] + 4 * lr * f * magbias[3];
NaotoMorita 38:acc7cdcf56dd 424
NaotoMorita 38:acc7cdcf56dd 425 if(userButton.read() == 1){
NaotoMorita 38:acc7cdcf56dd 426 break;
NaotoMorita 38:acc7cdcf56dd 427 }
NaotoMorita 38:acc7cdcf56dd 428 wait(0.001);
NaotoMorita 38:acc7cdcf56dd 429 }
NaotoMorita 38:acc7cdcf56dd 430 pc.printf("magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]);
NaotoMorita 38:acc7cdcf56dd 431 pc.printf("\r\n Refvec Calibration waiting\r\n");
NaotoMorita 38:acc7cdcf56dd 432 wait(5);
NaotoMorita 38:acc7cdcf56dd 433 pc.printf("\r\n Calibration Start\r\n");
NaotoMorita 38:acc7cdcf56dd 434 float arefs[3] = {0.0,0.0,0.0};
NaotoMorita 38:acc7cdcf56dd 435 float mrefs[3] = {0.0,0.0,0.0};
NaotoMorita 38:acc7cdcf56dd 436 for(int i = 0;i<iter_n ;i++){
NaotoMorita 38:acc7cdcf56dd 437 getIMUval();
NaotoMorita 38:acc7cdcf56dd 438 arefs[0] += acc_x;
NaotoMorita 38:acc7cdcf56dd 439 arefs[1] += acc_y;
NaotoMorita 38:acc7cdcf56dd 440 arefs[2] += acc_z;
NaotoMorita 38:acc7cdcf56dd 441 mrefs[0] += mag_x;
NaotoMorita 38:acc7cdcf56dd 442 mrefs[1] += mag_y;
NaotoMorita 38:acc7cdcf56dd 443 mrefs[2] += mag_z;
NaotoMorita 38:acc7cdcf56dd 444 wait(0.001);
NaotoMorita 38:acc7cdcf56dd 445 }
NaotoMorita 38:acc7cdcf56dd 446 arefs[0] /= iter_n;
NaotoMorita 38:acc7cdcf56dd 447 arefs[1] /= iter_n;
NaotoMorita 38:acc7cdcf56dd 448 arefs[2] /= iter_n;
NaotoMorita 38:acc7cdcf56dd 449 mrefs[0] /= iter_n;
NaotoMorita 38:acc7cdcf56dd 450 mrefs[1] /= iter_n;
NaotoMorita 38:acc7cdcf56dd 451 mrefs[2] /= iter_n;
NaotoMorita 38:acc7cdcf56dd 452 pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]);
NaotoMorita 38:acc7cdcf56dd 453 pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]);
NaotoMorita 38:acc7cdcf56dd 454 while(1) {wait(1000);}
naker 20:2c3f113a8e8f 455 }
NaotoMorita 27:43bd0e444633 456
naker 22:da9893b71f24 457 // 割り込まれた時点での出力(computeの結果）を返す関数
NaotoMorita 27:43bd0e444633 458 void calcServoOut(){
NaotoMorita 28:fc6c46c1db65 459 if(sbus.failSafe == false) {
NaotoMorita 28:fc6c46c1db65 460 // sbusデータの読み込み
NaotoMorita 28:fc6c46c1db65 461 for (int i =0 ; i < 16;i ++){
NaotoMorita 35:4535af88f7bf 462 rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input
NaotoMorita 27:43bd0e444633 463 }
NaotoMorita 28:fc6c46c1db65 464 }
NaotoMorita 28:fc6c46c1db65 465
NaotoMorita 28:fc6c46c1db65 466 pitchPID.setProcessValue(pitch);
NaotoMorita 28:fc6c46c1db65 467 pitchratePID.setProcessValue(gyro_y);
NaotoMorita 45:b439a135c24b 468 float de = pitchPID.compute()+pitchratePID.compute()-rc[1];
NaotoMorita 28:fc6c46c1db65 469
NaotoMorita 48:1065506191f2 470 scaledServoOut[0]=de;
NaotoMorita 31:8d02f3b9a067 471
NaotoMorita 38:acc7cdcf56dd 472 float LP_servo = 0.2;
NaotoMorita 36:e68ce293306e 473 for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){
NaotoMorita 36:e68ce293306e 474 servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i];
NaotoMorita 28:fc6c46c1db65 475 if(servoOut[i]<servoPwmMin) {
NaotoMorita 28:fc6c46c1db65 476 servoOut[i] = servoPwmMin;
NaotoMorita 28:fc6c46c1db65 477 };
NaotoMorita 28:fc6c46c1db65 478 if(servoOut[i]>servoPwmMax) {
NaotoMorita 28:fc6c46c1db65 479 servoOut[i] = servoPwmMax;
NaotoMorita 28:fc6c46c1db65 480 };
NaotoMorita 28:fc6c46c1db65 481 }
NaotoMorita 28:fc6c46c1db65 482
NaotoMorita 48:1065506191f2 483 servo.pulsewidth_us(servoOut[0]);
NaotoMorita 32:c4f06cb0e1d6 484
NaotoMorita 44:f571273d3223 485 //観測アップデート
NaotoMorita 44:f571273d3223 486 calcDynAcc();
NaotoMorita 44:f571273d3223 487 th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg);
NaotoMorita 44:f571273d3223 488 dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z);
NaotoMorita 44:f571273d3223 489 accnormerr = abs(LPaccnorm-accrefnorm);
NaotoMorita 44:f571273d3223 490 //静止時100個の平均　0.01877522 0.00514146 0.00477393
NaotoMorita 44:f571273d3223 491
NaotoMorita 44:f571273d3223 492 //int ang_th = th_mg < 0.01877522;
NaotoMorita 44:f571273d3223 493 //int dyn_th = dynaccnorm < 0.00514146;
NaotoMorita 44:f571273d3223 494 //int norm_th = accnormerr< 0.00477393;
NaotoMorita 44:f571273d3223 495 int ang_th = th_mg < 0.01877522/5.0;
NaotoMorita 44:f571273d3223 496 int dyn_th = dynaccnorm < 0.00514146/5.0;
NaotoMorita 44:f571273d3223 497 int norm_th = accnormerr< 0.00477393/5.0;
NaotoMorita 44:f571273d3223 498 if(dyn_th+ang_th+norm_th>0){
NaotoMorita 44:f571273d3223 499 //if(dyn_th+ang_th+norm_th>-1){
NaotoMorita 44:f571273d3223 500 obs_count += 1;
NaotoMorita 44:f571273d3223 501 updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag);
NaotoMorita 44:f571273d3223 502 updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc);
NaotoMorita 44:f571273d3223 503 }
NaotoMorita 44:f571273d3223 504
NaotoMorita 44:f571273d3223 505 if(loop_count >= 10){
NaotoMorita 31:8d02f3b9a067 506 writeSdcard();
NaotoMorita 36:e68ce293306e 507 loop_count = 1;
NaotoMorita 40:869f3791a3e2 508 obs_count = 0;
NaotoMorita 27:43bd0e444633 509 }else{
NaotoMorita 27:43bd0e444633 510 loop_count +=1;
naker 26:62d9857eaecc 511 }
NaotoMorita 44:f571273d3223 512
NaotoMorita 44:f571273d3223 513
naker 22:da9893b71f24 514 }
cocorlow 7:70161eb0f854 515
NaotoMorita 27:43bd0e444633 516 int main()
NaotoMorita 36:e68ce293306e 517 {
NaotoMorita 28:fc6c46c1db65 518 pitchPID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 519 pitchratePID.setSetPoint(0.0);
NaotoMorita 28:fc6c46c1db65 520 pitchPID.setBias(0.0);
NaotoMorita 48:1065506191f2 521 pitchratePID.setBias(0.0);
NaotoMorita 28:fc6c46c1db65 522 pitchPID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 523 pitchratePID.setOutputLimits(-1.0,1.0);
NaotoMorita 28:fc6c46c1db65 524 pitchPID.setInputLimits(-pi,pi);
NaotoMorita 28:fc6c46c1db65 525 pitchratePID.setInputLimits(-pi,pi);
NaotoMorita 29:34ee662f454e 526
NaotoMorita 48:1065506191f2 527 servo.period_us(15000.0);
NaotoMorita 48:1065506191f2 528 servo.pulsewidth_us(1500.0);
NaotoMorita 36:e68ce293306e 529
NaotoMorita 38:acc7cdcf56dd 530 pc.baud(57600);
NaotoMorita 36:e68ce293306e 531 accelgyro.initialize();
NaotoMorita 36:e68ce293306e 532 //加速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 533 accelgyro.setFullScaleAccelRange(ACCEL_FSR);
NaotoMorita 38:acc7cdcf56dd 534 //角速度計のフルスケールレンジを設定
NaotoMorita 36:e68ce293306e 535 accelgyro.setFullScaleGyroRange(GYRO_FSR);
NaotoMorita 36:e68ce293306e 536 //MPU6050のLPFを設定
NaotoMorita 36:e68ce293306e 537 accelgyro.setDLPFMode(MPU6050_LPF);
NaotoMorita 38:acc7cdcf56dd 538 //地磁気
NaotoMorita 38:acc7cdcf56dd 539 mag.enable();
NaotoMorita 38:acc7cdcf56dd 540
NaotoMorita 38:acc7cdcf56dd 541 if(userButton.read() == 0){
NaotoMorita 38:acc7cdcf56dd 542 qhat << 1 << 0 << 0 << 0;
NaotoMorita 44:f571273d3223 543
NaotoMorita 44:f571273d3223 544 D.add(1,1,1.0);
NaotoMorita 44:f571273d3223 545 D.add(2,2,1.0);
NaotoMorita 44:f571273d3223 546 D.add(3,3,1.0);
NaotoMorita 38:acc7cdcf56dd 547
NaotoMorita 38:acc7cdcf56dd 548 Phat << 0.01 << 0 <<0 <<0
NaotoMorita 38:acc7cdcf56dd 549 << 0 << 0.01 <<0 <<0
NaotoMorita 38:acc7cdcf56dd 550 << 0 << 0 <<0.001 <<0
NaotoMorita 38:acc7cdcf56dd 551 << 0 << 0 << 0 << 0.001;
NaotoMorita 38:acc7cdcf56dd 552
NaotoMorita 44:f571273d3223 553 Qgyro << 0.0224<< 0 <<0
NaotoMorita 44:f571273d3223 554 << 0 <<0.0224<<0
NaotoMorita 44:f571273d3223 555 << 0 << 0 <<0.0224;
naker 22:da9893b71f24 556
NaotoMorita 44:f571273d3223 557 Racc.add(1,1,0.0330*100);
NaotoMorita 44:f571273d3223 558 Racc.add(2,2,0.0330*100);
NaotoMorita 44:f571273d3223 559 Racc.add(3,3,0.0330*100);
NaotoMorita 44:f571273d3223 560
NaotoMorita 44:f571273d3223 561 Rmag.add(1,1,1.0);
NaotoMorita 44:f571273d3223 562 Rmag.add(2,2,1.0);
NaotoMorita 44:f571273d3223 563 Rmag.add(3,3,1.0);
NaotoMorita 44:f571273d3223 564
NaotoMorita 38:acc7cdcf56dd 565 getIMUval();
NaotoMorita 38:acc7cdcf56dd 566 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 567 float sumLPaccnorm = 0;
NaotoMorita 38:acc7cdcf56dd 568 for(int i = 0; i<1000 ;i++){
NaotoMorita 38:acc7cdcf56dd 569 getIMUval();
NaotoMorita 38:acc7cdcf56dd 570 val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm);
NaotoMorita 43:f0dd73430192 571 sumLPaccnorm += LPaccnorm;
NaotoMorita 38:acc7cdcf56dd 572 }
NaotoMorita 43:f0dd73430192 573 accref[2]=-sumLPaccnorm/1000;
NaotoMorita 38:acc7cdcf56dd 574 val_thmg /= 1000;
NaotoMorita 38:acc7cdcf56dd 575 LoopTicker PIDtick;
NaotoMorita 38:acc7cdcf56dd 576 PIDtick.attach(calcServoOut,PID_dt);
NaotoMorita 38:acc7cdcf56dd 577
NaotoMorita 38:acc7cdcf56dd 578 t.start();
NaotoMorita 28:fc6c46c1db65 579
NaotoMorita 38:acc7cdcf56dd 580 while(1) {
NaotoMorita 38:acc7cdcf56dd 581 float tstart = t.read();
NaotoMorita 38:acc7cdcf56dd 582 //姿勢角を更新
NaotoMorita 38:acc7cdcf56dd 583 getIMUval();
NaotoMorita 38:acc7cdcf56dd 584 updateBetweenMeasures();
NaotoMorita 38:acc7cdcf56dd 585 computeAngles();
NaotoMorita 38:acc7cdcf56dd 586 PIDtick.loop();
NaotoMorita 38:acc7cdcf56dd 587
NaotoMorita 38:acc7cdcf56dd 588 float tend = t.read();
NaotoMorita 38:acc7cdcf56dd 589 att_dt = (tend-tstart);
NaotoMorita 38:acc7cdcf56dd 590 }
NaotoMorita 38:acc7cdcf56dd 591 }else{
NaotoMorita 38:acc7cdcf56dd 592 execCalibration();
naker 20:2c3f113a8e8f 593 }
NaotoMorita 0:6b18a09a6628 594 }