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@41:4c02fcf105a9, 2021-03-22 (annotated)
- Committer:
- naker
- Date:
- Mon Mar 22 02:55:42 2021 +0000
- Revision:
- 41:4c02fcf105a9
- Parent:
- 38:acc7cdcf56dd
add comment and commentout
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 | 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 | 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 | 17:6b7a363d06d2 | 58 | int16_t ax, ay, az; |
NaotoMorita | 17:6b7a363d06d2 | 59 | int16_t gx, gy, gz; |
NaotoMorita | 38:acc7cdcf56dd | 60 | MotionSensorDataUnits mdata; |
NaotoMorita | 38:acc7cdcf56dd | 61 | float magval[3] = {1.0,0.0,0.0}; |
NaotoMorita | 38:acc7cdcf56dd | 62 | float acc_x,acc_y,acc_z; |
NaotoMorita | 38:acc7cdcf56dd | 63 | float dynacc_x,dynacc_y,dynacc_z; |
NaotoMorita | 38:acc7cdcf56dd | 64 | float gyro_x,gyro_y,gyro_z; |
NaotoMorita | 38:acc7cdcf56dd | 65 | float mag_x,mag_y,mag_z; |
cocorlow | 3:79e62f9b13c8 | 66 | int out1, out2; |
NaotoMorita | 36:e68ce293306e | 67 | float scaledServoOut[3]= {0,0}; |
NaotoMorita | 28:fc6c46c1db65 | 68 | float scaledMotorOut[1]= {-1}; |
NaotoMorita | 36:e68ce293306e | 69 | float servoOut[3] = {1500.0,1500.0}; |
NaotoMorita | 36:e68ce293306e | 70 | float motorOut[1] = {1100.0}; |
NaotoMorita | 38:acc7cdcf56dd | 71 | |
NaotoMorita | 38:acc7cdcf56dd | 72 | float accnorm; |
NaotoMorita | 38:acc7cdcf56dd | 73 | float magnorm; |
NaotoMorita | 38:acc7cdcf56dd | 74 | float accrefnorm; |
NaotoMorita | 38:acc7cdcf56dd | 75 | float magrefnorm; |
NaotoMorita | 38:acc7cdcf56dd | 76 | |
NaotoMorita | 38:acc7cdcf56dd | 77 | float val_thmg = 0.0; |
NaotoMorita | 38:acc7cdcf56dd | 78 | float accref[3] = {0, 0, -0.98}; |
NaotoMorita | 38:acc7cdcf56dd | 79 | float magref[3] = {0.65, 0, 0.75}; |
NaotoMorita | 38:acc7cdcf56dd | 80 | |
NaotoMorita | 38:acc7cdcf56dd | 81 | int agoffset[6] = {0, 0, 0, -123, -575, -1}; |
NaotoMorita | 38:acc7cdcf56dd | 82 | float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077}; |
cocorlow | 3:79e62f9b13c8 | 83 | |
NaotoMorita | 38:acc7cdcf56dd | 84 | void writeSdcard() |
NaotoMorita | 38:acc7cdcf56dd | 85 | { |
NaotoMorita | 38:acc7cdcf56dd | 86 | //magcal.getExtremes(&magmin[0],&magmax[0]); |
NaotoMorita | 38:acc7cdcf56dd | 87 | //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 | 88 | //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 | 38:acc7cdcf56dd | 89 | sd.printf("%f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,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 | 38:acc7cdcf56dd | 90 | //pc.printf("%d \r\n",userButton.read()); |
NaotoMorita | 38:acc7cdcf56dd | 91 | //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 | 92 | //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); |
NaotoMorita | 38:acc7cdcf56dd | 93 | //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 | 94 | //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 | 95 | //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 | 96 | } |
NaotoMorita | 27:43bd0e444633 | 97 | |
NaotoMorita | 35:4535af88f7bf | 98 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 7:70161eb0f854 | 99 | { |
cocorlow | 7:70161eb0f854 | 100 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cocorlow | 7:70161eb0f854 | 101 | } |
NaotoMorita | 27:43bd0e444633 | 102 | |
NaotoMorita | 38:acc7cdcf56dd | 103 | void calcMagRef(float mx, float my, float mz){ |
NaotoMorita | 38:acc7cdcf56dd | 104 | float q0 = qhat.getNumber( 1, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 105 | float q1 = qhat.getNumber( 2, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 106 | float q2 = qhat.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 107 | float q3 = qhat.getNumber( 4, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 108 | float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2)); |
NaotoMorita | 38:acc7cdcf56dd | 109 | float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1)); |
NaotoMorita | 38:acc7cdcf56dd | 110 | float bx = sqrt(hx * hx + hy * hy); |
NaotoMorita | 38:acc7cdcf56dd | 111 | float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2)); |
NaotoMorita | 38:acc7cdcf56dd | 112 | magref[0] = bx; |
NaotoMorita | 38:acc7cdcf56dd | 113 | magref[1] = 0.0; |
NaotoMorita | 38:acc7cdcf56dd | 114 | magref[2] = bz; |
NaotoMorita | 38:acc7cdcf56dd | 115 | } |
NaotoMorita | 38:acc7cdcf56dd | 116 | |
NaotoMorita | 38:acc7cdcf56dd | 117 | |
NaotoMorita | 38:acc7cdcf56dd | 118 | void getIMUval(){ |
NaotoMorita | 38:acc7cdcf56dd | 119 | // gx gy gz ax ay az |
NaotoMorita | 38:acc7cdcf56dd | 120 | accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); |
NaotoMorita | 38:acc7cdcf56dd | 121 | ax = ax - agoffset[0]; |
NaotoMorita | 38:acc7cdcf56dd | 122 | ay = ay - agoffset[1]; |
NaotoMorita | 38:acc7cdcf56dd | 123 | az = -az - agoffset[2]; |
NaotoMorita | 38:acc7cdcf56dd | 124 | gx = gx - agoffset[3]; |
NaotoMorita | 38:acc7cdcf56dd | 125 | gy = gy - agoffset[4]; |
NaotoMorita | 38:acc7cdcf56dd | 126 | gz = -gz - agoffset[5]; |
NaotoMorita | 38:acc7cdcf56dd | 127 | // 加速度値を分解能で割って加速度(G)に変換する |
NaotoMorita | 38:acc7cdcf56dd | 128 | acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g |
NaotoMorita | 38:acc7cdcf56dd | 129 | acc_y = float(ay) / ACCEL_SSF; |
NaotoMorita | 38:acc7cdcf56dd | 130 | acc_z = float(az) / ACCEL_SSF; |
NaotoMorita | 38:acc7cdcf56dd | 131 | // 角速度値を分解能で割って角速度(rad per sec)に変換する |
NaotoMorita | 38:acc7cdcf56dd | 132 | gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s) |
NaotoMorita | 38:acc7cdcf56dd | 133 | gyro_y = float(gy) / GYRO_SSF * 0.0174533f; |
NaotoMorita | 38:acc7cdcf56dd | 134 | gyro_z = float(gz) / GYRO_SSF * 0.0174533f; |
NaotoMorita | 38:acc7cdcf56dd | 135 | mag.getAxis(mdata); // flush the magnetmeter |
NaotoMorita | 38:acc7cdcf56dd | 136 | magval[0] = (mdata.x - magbias[0]); |
NaotoMorita | 38:acc7cdcf56dd | 137 | magval[1] = (mdata.y - magbias[1]); |
NaotoMorita | 38:acc7cdcf56dd | 138 | magval[2] = (mdata.z - magbias[2]); |
NaotoMorita | 38:acc7cdcf56dd | 139 | mag_x = -magval[0]/magbias[3]; |
NaotoMorita | 38:acc7cdcf56dd | 140 | mag_y = -magval[1]/magbias[3]; |
NaotoMorita | 38:acc7cdcf56dd | 141 | mag_z = -magval[2]/magbias[3]; |
NaotoMorita | 38:acc7cdcf56dd | 142 | |
NaotoMorita | 38:acc7cdcf56dd | 143 | accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z); |
NaotoMorita | 38:acc7cdcf56dd | 144 | magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z); |
NaotoMorita | 38:acc7cdcf56dd | 145 | accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]); |
NaotoMorita | 38:acc7cdcf56dd | 146 | magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]); |
NaotoMorita | 38:acc7cdcf56dd | 147 | calcMagRef(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm); |
NaotoMorita | 38:acc7cdcf56dd | 148 | } |
NaotoMorita | 38:acc7cdcf56dd | 149 | |
NaotoMorita | 38:acc7cdcf56dd | 150 | void updateBetweenMeasures(){ |
NaotoMorita | 38:acc7cdcf56dd | 151 | Matrix phi(4,4); |
NaotoMorita | 38:acc7cdcf56dd | 152 | phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt |
NaotoMorita | 38:acc7cdcf56dd | 153 | << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt |
NaotoMorita | 38:acc7cdcf56dd | 154 | << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt |
NaotoMorita | 38:acc7cdcf56dd | 155 | << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0; |
NaotoMorita | 38:acc7cdcf56dd | 156 | qhat = phi*qhat; |
NaotoMorita | 38:acc7cdcf56dd | 157 | |
NaotoMorita | 38:acc7cdcf56dd | 158 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 38:acc7cdcf56dd | 159 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 38:acc7cdcf56dd | 160 | |
NaotoMorita | 38:acc7cdcf56dd | 161 | float q0 = qhat.getNumber( 1, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 162 | float q1 = qhat.getNumber( 2, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 163 | float q2 = qhat.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 164 | float q3 = qhat.getNumber( 4, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 165 | |
NaotoMorita | 38:acc7cdcf56dd | 166 | Matrix B(4,3); |
NaotoMorita | 38:acc7cdcf56dd | 167 | B << q1 << q2 << q3 |
NaotoMorita | 38:acc7cdcf56dd | 168 | <<-q0 << q3 <<-q2 |
NaotoMorita | 38:acc7cdcf56dd | 169 | <<-q3 <<-q0 << q1 |
NaotoMorita | 38:acc7cdcf56dd | 170 | << q2 <<-q1 <<-q0; |
NaotoMorita | 38:acc7cdcf56dd | 171 | B *= 0.5f; |
NaotoMorita | 38:acc7cdcf56dd | 172 | Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); |
NaotoMorita | 38:acc7cdcf56dd | 173 | |
NaotoMorita | 38:acc7cdcf56dd | 174 | |
NaotoMorita | 38:acc7cdcf56dd | 175 | } |
NaotoMorita | 38:acc7cdcf56dd | 176 | |
NaotoMorita | 38:acc7cdcf56dd | 177 | void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){ |
NaotoMorita | 38:acc7cdcf56dd | 178 | |
NaotoMorita | 38:acc7cdcf56dd | 179 | Matrix u(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 180 | Matrix v(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 181 | |
NaotoMorita | 38:acc7cdcf56dd | 182 | u << u1 << u2 <<u3; |
NaotoMorita | 38:acc7cdcf56dd | 183 | v << v1 << v2 <<v3; |
NaotoMorita | 38:acc7cdcf56dd | 184 | |
NaotoMorita | 38:acc7cdcf56dd | 185 | float q0 = qhat.getNumber( 1, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 186 | float q1 = qhat.getNumber( 2, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 187 | float q2 = qhat.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 188 | float q3 = qhat.getNumber( 4, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 189 | |
NaotoMorita | 38:acc7cdcf56dd | 190 | Matrix A1(3,3); |
NaotoMorita | 38:acc7cdcf56dd | 191 | A1 << q0 << q3 << -q2 |
NaotoMorita | 38:acc7cdcf56dd | 192 | <<-q3 << q0 << q1 |
NaotoMorita | 38:acc7cdcf56dd | 193 | <<q2 <<-q1 <<q0; |
NaotoMorita | 38:acc7cdcf56dd | 194 | A1 *= 2.0f; |
NaotoMorita | 38:acc7cdcf56dd | 195 | |
NaotoMorita | 38:acc7cdcf56dd | 196 | Matrix A2(3,3); |
NaotoMorita | 38:acc7cdcf56dd | 197 | A2 << q1 << q2 << q3 |
NaotoMorita | 38:acc7cdcf56dd | 198 | << q2 <<-q1 << q0 |
NaotoMorita | 38:acc7cdcf56dd | 199 | << q3 <<-q0 <<-q1; |
NaotoMorita | 38:acc7cdcf56dd | 200 | A2 *= 2.0f; |
NaotoMorita | 38:acc7cdcf56dd | 201 | |
NaotoMorita | 38:acc7cdcf56dd | 202 | Matrix A3(3,3); |
NaotoMorita | 38:acc7cdcf56dd | 203 | A3 <<-q2 << q1 <<-q0 |
NaotoMorita | 38:acc7cdcf56dd | 204 | << q1 << q2 << q3 |
NaotoMorita | 38:acc7cdcf56dd | 205 | << q0 << q3 <<-q2; |
NaotoMorita | 38:acc7cdcf56dd | 206 | A3 *= 2.0f; |
NaotoMorita | 38:acc7cdcf56dd | 207 | |
NaotoMorita | 38:acc7cdcf56dd | 208 | Matrix A4(3,3); |
NaotoMorita | 38:acc7cdcf56dd | 209 | A4 <<-q3 << q0 << q1 |
NaotoMorita | 38:acc7cdcf56dd | 210 | <<-q0 <<-q3 << q2 |
NaotoMorita | 38:acc7cdcf56dd | 211 | << q1 << q2 << q3; |
NaotoMorita | 38:acc7cdcf56dd | 212 | A4 *= 2.0f; |
NaotoMorita | 38:acc7cdcf56dd | 213 | |
NaotoMorita | 38:acc7cdcf56dd | 214 | Matrix H(3,4); |
NaotoMorita | 38:acc7cdcf56dd | 215 | |
NaotoMorita | 38:acc7cdcf56dd | 216 | Matrix ab1(A1*u); |
NaotoMorita | 38:acc7cdcf56dd | 217 | Matrix ab2(A2*u); |
NaotoMorita | 38:acc7cdcf56dd | 218 | Matrix ab3(A3*u); |
NaotoMorita | 38:acc7cdcf56dd | 219 | Matrix ab4(A4*u); |
NaotoMorita | 38:acc7cdcf56dd | 220 | |
NaotoMorita | 38:acc7cdcf56dd | 221 | H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 ) |
NaotoMorita | 38:acc7cdcf56dd | 222 | << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) |
NaotoMorita | 38:acc7cdcf56dd | 223 | << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 224 | |
NaotoMorita | 38:acc7cdcf56dd | 225 | D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); |
NaotoMorita | 38:acc7cdcf56dd | 226 | D.add(1,2,2*(q1*q2 + q0*q3)); |
NaotoMorita | 38:acc7cdcf56dd | 227 | D.add(1,3,2*(q1*q3 - q0*q2)); |
NaotoMorita | 38:acc7cdcf56dd | 228 | D.add(2,1,2*(q1*q2 - q0*q3)); |
NaotoMorita | 38:acc7cdcf56dd | 229 | D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); |
NaotoMorita | 38:acc7cdcf56dd | 230 | D.add(2,3,2*(q2*q3 + q0*q1)); |
NaotoMorita | 38:acc7cdcf56dd | 231 | D.add(3,1,2*(q1*q3 + q0*q2)); |
NaotoMorita | 38:acc7cdcf56dd | 232 | D.add(3,2,2*(q2*q3 - q0*q1)); |
NaotoMorita | 38:acc7cdcf56dd | 233 | D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); |
NaotoMorita | 38:acc7cdcf56dd | 234 | |
NaotoMorita | 38:acc7cdcf56dd | 235 | |
NaotoMorita | 38:acc7cdcf56dd | 236 | Matrix K(4,3); |
NaotoMorita | 38:acc7cdcf56dd | 237 | K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); |
NaotoMorita | 38:acc7cdcf56dd | 238 | |
NaotoMorita | 38:acc7cdcf56dd | 239 | Matrix dq(4,1); |
NaotoMorita | 38:acc7cdcf56dd | 240 | dq = K*(v-D*u); |
NaotoMorita | 38:acc7cdcf56dd | 241 | qhat = qhat+dq; |
NaotoMorita | 38:acc7cdcf56dd | 242 | |
NaotoMorita | 38:acc7cdcf56dd | 243 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 38:acc7cdcf56dd | 244 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 38:acc7cdcf56dd | 245 | |
NaotoMorita | 38:acc7cdcf56dd | 246 | Matrix eye4(4,4); |
NaotoMorita | 38:acc7cdcf56dd | 247 | eye4 << 1 << 0 << 0 << 0 |
NaotoMorita | 38:acc7cdcf56dd | 248 | << 0 << 1 << 0 << 0 |
NaotoMorita | 38:acc7cdcf56dd | 249 | << 0 << 0 << 1 << 0 |
NaotoMorita | 38:acc7cdcf56dd | 250 | << 0 << 0 << 0 << 1; |
NaotoMorita | 38:acc7cdcf56dd | 251 | Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K); |
NaotoMorita | 38:acc7cdcf56dd | 252 | } |
NaotoMorita | 38:acc7cdcf56dd | 253 | |
NaotoMorita | 38:acc7cdcf56dd | 254 | void computeAngles() |
naker | 20:2c3f113a8e8f | 255 | { |
NaotoMorita | 38:acc7cdcf56dd | 256 | float q0 = qhat.getNumber( 1, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 257 | float q1 = qhat.getNumber( 2, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 258 | float q2 = qhat.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 259 | float q3 = qhat.getNumber( 4, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 260 | roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align; |
NaotoMorita | 38:acc7cdcf56dd | 261 | pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align; |
NaotoMorita | 38:acc7cdcf56dd | 262 | yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); |
NaotoMorita | 38:acc7cdcf56dd | 263 | } |
NaotoMorita | 38:acc7cdcf56dd | 264 | |
NaotoMorita | 38:acc7cdcf56dd | 265 | 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 | 266 | Matrix W1(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 267 | W1 << fb1 << fb2<< fb3; |
NaotoMorita | 38:acc7cdcf56dd | 268 | Matrix W2(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 269 | W2 << mb1 << mb2<< mb3; |
NaotoMorita | 38:acc7cdcf56dd | 270 | |
NaotoMorita | 38:acc7cdcf56dd | 271 | Matrix V1(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 272 | V1 << fn1 << fn2<< fn3; |
NaotoMorita | 38:acc7cdcf56dd | 273 | Matrix V2(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 274 | V2 << mn1 << mn2<< mn3; |
NaotoMorita | 38:acc7cdcf56dd | 275 | |
NaotoMorita | 38:acc7cdcf56dd | 276 | |
NaotoMorita | 38:acc7cdcf56dd | 277 | Matrix Ou2(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 278 | 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 | 279 | Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); |
NaotoMorita | 38:acc7cdcf56dd | 280 | Matrix Ou3(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 281 | 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 | 282 | Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); |
NaotoMorita | 38:acc7cdcf56dd | 283 | Matrix R2(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 284 | 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 | 285 | R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); |
NaotoMorita | 38:acc7cdcf56dd | 286 | Matrix R3(3,1); |
NaotoMorita | 38:acc7cdcf56dd | 287 | 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 | 288 | R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); |
NaotoMorita | 38:acc7cdcf56dd | 289 | |
NaotoMorita | 38:acc7cdcf56dd | 290 | Matrix Mou(3,3); |
NaotoMorita | 38:acc7cdcf56dd | 291 | Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 ) |
NaotoMorita | 38:acc7cdcf56dd | 292 | << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 ) |
NaotoMorita | 38:acc7cdcf56dd | 293 | << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 294 | Matrix Mr(3,3); |
NaotoMorita | 38:acc7cdcf56dd | 295 | Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 ) |
NaotoMorita | 38:acc7cdcf56dd | 296 | << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 ) |
NaotoMorita | 38:acc7cdcf56dd | 297 | << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 ); |
NaotoMorita | 38:acc7cdcf56dd | 298 | |
NaotoMorita | 38:acc7cdcf56dd | 299 | Matrix Cbn = Mr*MatrixMath::Transpose(Mou); |
NaotoMorita | 38:acc7cdcf56dd | 300 | |
NaotoMorita | 38:acc7cdcf56dd | 301 | float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 )); |
NaotoMorita | 38:acc7cdcf56dd | 302 | |
NaotoMorita | 38:acc7cdcf56dd | 303 | qhat.add(1,1,0.5*sqtrp1); |
NaotoMorita | 38:acc7cdcf56dd | 304 | qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1); |
NaotoMorita | 38:acc7cdcf56dd | 305 | qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1); |
NaotoMorita | 38:acc7cdcf56dd | 306 | qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1); |
NaotoMorita | 38:acc7cdcf56dd | 307 | |
NaotoMorita | 38:acc7cdcf56dd | 308 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 38:acc7cdcf56dd | 309 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 38:acc7cdcf56dd | 310 | } |
NaotoMorita | 38:acc7cdcf56dd | 311 | |
NaotoMorita | 38:acc7cdcf56dd | 312 | void calcDynAcc(){ |
NaotoMorita | 38:acc7cdcf56dd | 313 | dynacc_x = acc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]); |
NaotoMorita | 38:acc7cdcf56dd | 314 | dynacc_y = acc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]); |
NaotoMorita | 38:acc7cdcf56dd | 315 | dynacc_z = acc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]); |
NaotoMorita | 38:acc7cdcf56dd | 316 | } |
NaotoMorita | 38:acc7cdcf56dd | 317 | |
NaotoMorita | 38:acc7cdcf56dd | 318 | void execCalibration(){ |
NaotoMorita | 38:acc7cdcf56dd | 319 | pc.printf("\r\nEnter to Calibration Mode\r\n"); |
NaotoMorita | 38:acc7cdcf56dd | 320 | wait(5); |
NaotoMorita | 38:acc7cdcf56dd | 321 | pc.printf("\r\n Acc and Gyro Calibration Start\r\n"); |
NaotoMorita | 38:acc7cdcf56dd | 322 | int axs = 0; |
NaotoMorita | 38:acc7cdcf56dd | 323 | int ays = 0; |
NaotoMorita | 38:acc7cdcf56dd | 324 | int azs = 0; |
NaotoMorita | 38:acc7cdcf56dd | 325 | int gxs = 0; |
NaotoMorita | 38:acc7cdcf56dd | 326 | int gys = 0; |
NaotoMorita | 38:acc7cdcf56dd | 327 | int gzs = 0; |
NaotoMorita | 38:acc7cdcf56dd | 328 | int iter_n = 5000; |
NaotoMorita | 38:acc7cdcf56dd | 329 | for(int i = 0;i<iter_n ;i++){ |
NaotoMorita | 38:acc7cdcf56dd | 330 | accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); |
NaotoMorita | 38:acc7cdcf56dd | 331 | axs += ax; |
NaotoMorita | 38:acc7cdcf56dd | 332 | ays += ay; |
NaotoMorita | 38:acc7cdcf56dd | 333 | azs -= az; |
NaotoMorita | 38:acc7cdcf56dd | 334 | gxs += gx; |
NaotoMorita | 38:acc7cdcf56dd | 335 | gys += gy; |
NaotoMorita | 38:acc7cdcf56dd | 336 | gzs -= gz; |
NaotoMorita | 38:acc7cdcf56dd | 337 | wait(0.001); |
NaotoMorita | 38:acc7cdcf56dd | 338 | } |
NaotoMorita | 38:acc7cdcf56dd | 339 | axs = axs /iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 340 | ays = ays /iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 341 | azs = azs /iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 342 | gxs = gxs /iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 343 | gys = gys /iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 344 | gzs = gzs /iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 345 | pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); |
NaotoMorita | 38:acc7cdcf56dd | 346 | |
NaotoMorita | 38:acc7cdcf56dd | 347 | pc.printf("\r\n Mag Calibration Start\r\n"); |
NaotoMorita | 38:acc7cdcf56dd | 348 | float f = 0; |
NaotoMorita | 38:acc7cdcf56dd | 349 | while(1){ |
NaotoMorita | 38:acc7cdcf56dd | 350 | mag.getAxis(mdata); // flush the magnetmeter |
NaotoMorita | 38:acc7cdcf56dd | 351 | magval[0] = (mdata.x - magbias[0]); |
NaotoMorita | 38:acc7cdcf56dd | 352 | magval[1] = (mdata.y - magbias[1]); |
NaotoMorita | 38:acc7cdcf56dd | 353 | magval[2] = (mdata.z - magbias[2]); |
NaotoMorita | 38:acc7cdcf56dd | 354 | float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2]; |
NaotoMorita | 38:acc7cdcf56dd | 355 | float lr = 0.00001f; |
NaotoMorita | 38:acc7cdcf56dd | 356 | f = mag_r - magbias[3]*magbias[3]; |
NaotoMorita | 38:acc7cdcf56dd | 357 | magbias[0] = magbias[0] + 4 * lr * f * magval[0]; |
NaotoMorita | 38:acc7cdcf56dd | 358 | magbias[1] = magbias[1] + 4 * lr * f * magval[1]; |
NaotoMorita | 38:acc7cdcf56dd | 359 | magbias[2] = magbias[2] + 4 * lr * f * magval[2]; |
NaotoMorita | 38:acc7cdcf56dd | 360 | magbias[3] = magbias[3] + 4 * lr * f * magbias[3]; |
NaotoMorita | 38:acc7cdcf56dd | 361 | |
NaotoMorita | 38:acc7cdcf56dd | 362 | if(userButton.read() == 1){ |
NaotoMorita | 38:acc7cdcf56dd | 363 | break; |
NaotoMorita | 38:acc7cdcf56dd | 364 | } |
NaotoMorita | 38:acc7cdcf56dd | 365 | wait(0.001); |
NaotoMorita | 38:acc7cdcf56dd | 366 | } |
NaotoMorita | 38:acc7cdcf56dd | 367 | pc.printf("magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]); |
NaotoMorita | 38:acc7cdcf56dd | 368 | pc.printf("\r\n Refvec Calibration waiting\r\n"); |
NaotoMorita | 38:acc7cdcf56dd | 369 | wait(5); |
NaotoMorita | 38:acc7cdcf56dd | 370 | pc.printf("\r\n Calibration Start\r\n"); |
NaotoMorita | 38:acc7cdcf56dd | 371 | float arefs[3] = {0.0,0.0,0.0}; |
NaotoMorita | 38:acc7cdcf56dd | 372 | float mrefs[3] = {0.0,0.0,0.0}; |
naker | 41:4c02fcf105a9 | 373 | // 加速度のカリブレーション(結局使わなかった) |
naker | 41:4c02fcf105a9 | 374 | /* |
NaotoMorita | 38:acc7cdcf56dd | 375 | for(int i = 0;i<iter_n ;i++){ |
NaotoMorita | 38:acc7cdcf56dd | 376 | getIMUval(); |
NaotoMorita | 38:acc7cdcf56dd | 377 | arefs[0] += acc_x; |
NaotoMorita | 38:acc7cdcf56dd | 378 | arefs[1] += acc_y; |
NaotoMorita | 38:acc7cdcf56dd | 379 | arefs[2] += acc_z; |
NaotoMorita | 38:acc7cdcf56dd | 380 | mrefs[0] += mag_x; |
NaotoMorita | 38:acc7cdcf56dd | 381 | mrefs[1] += mag_y; |
NaotoMorita | 38:acc7cdcf56dd | 382 | mrefs[2] += mag_z; |
NaotoMorita | 38:acc7cdcf56dd | 383 | wait(0.001); |
NaotoMorita | 38:acc7cdcf56dd | 384 | } |
NaotoMorita | 38:acc7cdcf56dd | 385 | arefs[0] /= iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 386 | arefs[1] /= iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 387 | arefs[2] /= iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 388 | mrefs[0] /= iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 389 | mrefs[1] /= iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 390 | mrefs[2] /= iter_n; |
NaotoMorita | 38:acc7cdcf56dd | 391 | pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]); |
NaotoMorita | 38:acc7cdcf56dd | 392 | pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]); |
naker | 41:4c02fcf105a9 | 393 | */ |
NaotoMorita | 38:acc7cdcf56dd | 394 | while(1) {wait(1000);} |
naker | 20:2c3f113a8e8f | 395 | } |
NaotoMorita | 27:43bd0e444633 | 396 | |
naker | 22:da9893b71f24 | 397 | // 割り込まれた時点での出力(computeの結果)を返す関数 |
NaotoMorita | 27:43bd0e444633 | 398 | void calcServoOut(){ |
NaotoMorita | 28:fc6c46c1db65 | 399 | if(sbus.failSafe == false) { |
NaotoMorita | 28:fc6c46c1db65 | 400 | // sbusデータの読み込み |
NaotoMorita | 28:fc6c46c1db65 | 401 | for (int i =0 ; i < 16;i ++){ |
NaotoMorita | 35:4535af88f7bf | 402 | rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input |
NaotoMorita | 27:43bd0e444633 | 403 | } |
NaotoMorita | 28:fc6c46c1db65 | 404 | } |
NaotoMorita | 28:fc6c46c1db65 | 405 | |
NaotoMorita | 28:fc6c46c1db65 | 406 | pitchPID.setProcessValue(pitch); |
NaotoMorita | 28:fc6c46c1db65 | 407 | pitchratePID.setProcessValue(gyro_y); |
NaotoMorita | 28:fc6c46c1db65 | 408 | rollPID.setProcessValue(roll); |
NaotoMorita | 28:fc6c46c1db65 | 409 | rollratePID.setProcessValue(gyro_x); |
NaotoMorita | 38:acc7cdcf56dd | 410 | float de = pitchPID.compute()+pitchratePID.compute()-rc[1]+rc[0]; |
NaotoMorita | 36:e68ce293306e | 411 | float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1]; |
NaotoMorita | 28:fc6c46c1db65 | 412 | float dT = rc[2]; |
NaotoMorita | 28:fc6c46c1db65 | 413 | |
NaotoMorita | 28:fc6c46c1db65 | 414 | scaledServoOut[0]=de+da; |
NaotoMorita | 36:e68ce293306e | 415 | scaledServoOut[1]=-de+da; |
NaotoMorita | 28:fc6c46c1db65 | 416 | scaledMotorOut[0]= dT; |
NaotoMorita | 31:8d02f3b9a067 | 417 | |
NaotoMorita | 38:acc7cdcf56dd | 418 | float LP_servo = 0.2; |
NaotoMorita | 38:acc7cdcf56dd | 419 | float LP_motor = 0.2; |
NaotoMorita | 36:e68ce293306e | 420 | for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){ |
NaotoMorita | 36:e68ce293306e | 421 | servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i]; |
NaotoMorita | 28:fc6c46c1db65 | 422 | if(servoOut[i]<servoPwmMin) { |
NaotoMorita | 28:fc6c46c1db65 | 423 | servoOut[i] = servoPwmMin; |
NaotoMorita | 28:fc6c46c1db65 | 424 | }; |
NaotoMorita | 28:fc6c46c1db65 | 425 | if(servoOut[i]>servoPwmMax) { |
NaotoMorita | 28:fc6c46c1db65 | 426 | servoOut[i] = servoPwmMax; |
NaotoMorita | 28:fc6c46c1db65 | 427 | }; |
NaotoMorita | 28:fc6c46c1db65 | 428 | } |
NaotoMorita | 28:fc6c46c1db65 | 429 | |
NaotoMorita | 36:e68ce293306e | 430 | for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){ |
NaotoMorita | 36:e68ce293306e | 431 | motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i]; |
NaotoMorita | 28:fc6c46c1db65 | 432 | if(motorOut[i]<motorPwmMin) { |
NaotoMorita | 28:fc6c46c1db65 | 433 | motorOut[i] = motorPwmMin; |
NaotoMorita | 28:fc6c46c1db65 | 434 | }; |
NaotoMorita | 28:fc6c46c1db65 | 435 | if(motorOut[i]>motorPwmMax) { |
NaotoMorita | 28:fc6c46c1db65 | 436 | motorOut[i] = motorPwmMax; |
NaotoMorita | 28:fc6c46c1db65 | 437 | }; |
NaotoMorita | 28:fc6c46c1db65 | 438 | } |
NaotoMorita | 36:e68ce293306e | 439 | servoRight.pulsewidth_us(servoOut[0]); |
NaotoMorita | 36:e68ce293306e | 440 | servoLeft.pulsewidth_us(servoOut[1]); |
NaotoMorita | 36:e68ce293306e | 441 | servoThrust.pulsewidth_us(motorOut[0]); |
NaotoMorita | 32:c4f06cb0e1d6 | 442 | |
NaotoMorita | 36:e68ce293306e | 443 | if(loop_count > 10){ |
NaotoMorita | 31:8d02f3b9a067 | 444 | writeSdcard(); |
NaotoMorita | 36:e68ce293306e | 445 | loop_count = 1; |
NaotoMorita | 27:43bd0e444633 | 446 | }else{ |
NaotoMorita | 27:43bd0e444633 | 447 | loop_count +=1; |
naker | 26:62d9857eaecc | 448 | } |
naker | 22:da9893b71f24 | 449 | } |
cocorlow | 7:70161eb0f854 | 450 | |
NaotoMorita | 27:43bd0e444633 | 451 | int main() |
NaotoMorita | 36:e68ce293306e | 452 | { |
NaotoMorita | 28:fc6c46c1db65 | 453 | pitchPID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 454 | pitchratePID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 455 | rollPID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 456 | rollratePID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 457 | pitchPID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 458 | pitchratePID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 459 | rollPID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 460 | rollratePID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 461 | pitchPID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 28:fc6c46c1db65 | 462 | pitchratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 28:fc6c46c1db65 | 463 | rollPID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 29:34ee662f454e | 464 | rollratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 28:fc6c46c1db65 | 465 | pitchPID.setInputLimits(-pi,pi); |
NaotoMorita | 28:fc6c46c1db65 | 466 | pitchratePID.setInputLimits(-pi,pi); |
NaotoMorita | 28:fc6c46c1db65 | 467 | rollPID.setInputLimits(-pi,pi); |
NaotoMorita | 28:fc6c46c1db65 | 468 | rollratePID.setInputLimits(-pi,pi); |
NaotoMorita | 29:34ee662f454e | 469 | |
NaotoMorita | 36:e68ce293306e | 470 | servoRight.period_us(15000.0); |
NaotoMorita | 36:e68ce293306e | 471 | servoLeft.period_us(15000.0); |
NaotoMorita | 36:e68ce293306e | 472 | servoThrust.period_us(15000.0); |
NaotoMorita | 36:e68ce293306e | 473 | servoRight.pulsewidth_us(1500.0); |
NaotoMorita | 36:e68ce293306e | 474 | servoLeft.pulsewidth_us(1500.0); |
NaotoMorita | 36:e68ce293306e | 475 | servoThrust.pulsewidth_us(1100.0); |
NaotoMorita | 36:e68ce293306e | 476 | |
NaotoMorita | 38:acc7cdcf56dd | 477 | pc.baud(57600); |
NaotoMorita | 38:acc7cdcf56dd | 478 | sd.baud(57600); |
NaotoMorita | 38:acc7cdcf56dd | 479 | sd.printf("\r\n Program Start \r\n"); |
NaotoMorita | 36:e68ce293306e | 480 | accelgyro.initialize(); |
NaotoMorita | 36:e68ce293306e | 481 | //加速度計のフルスケールレンジを設定 |
NaotoMorita | 36:e68ce293306e | 482 | accelgyro.setFullScaleAccelRange(ACCEL_FSR); |
NaotoMorita | 38:acc7cdcf56dd | 483 | //角速度計のフルスケールレンジを設定 |
NaotoMorita | 36:e68ce293306e | 484 | accelgyro.setFullScaleGyroRange(GYRO_FSR); |
NaotoMorita | 36:e68ce293306e | 485 | //MPU6050のLPFを設定 |
NaotoMorita | 36:e68ce293306e | 486 | accelgyro.setDLPFMode(MPU6050_LPF); |
NaotoMorita | 38:acc7cdcf56dd | 487 | //地磁気 |
NaotoMorita | 38:acc7cdcf56dd | 488 | mag.enable(); |
NaotoMorita | 38:acc7cdcf56dd | 489 | |
NaotoMorita | 38:acc7cdcf56dd | 490 | if(userButton.read() == 0){ |
NaotoMorita | 38:acc7cdcf56dd | 491 | qhat << 1 << 0 << 0 << 0; |
NaotoMorita | 38:acc7cdcf56dd | 492 | |
NaotoMorita | 38:acc7cdcf56dd | 493 | Phat << 0.01 << 0 <<0 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 494 | << 0 << 0.01 <<0 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 495 | << 0 << 0 <<0.001 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 496 | << 0 << 0 << 0 << 0.001; |
NaotoMorita | 38:acc7cdcf56dd | 497 | |
NaotoMorita | 38:acc7cdcf56dd | 498 | Qgyro << 5.4732e-04 << 0 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 499 | << 0 <<5.4732e-04 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 500 | << 0 << 0 <<5.4732e-04; |
naker | 22:da9893b71f24 | 501 | |
NaotoMorita | 38:acc7cdcf56dd | 502 | Racc.add(1,1,1.0); |
NaotoMorita | 38:acc7cdcf56dd | 503 | Racc.add(2,2,1.0); |
NaotoMorita | 38:acc7cdcf56dd | 504 | Racc.add(3,3,1.0); |
NaotoMorita | 38:acc7cdcf56dd | 505 | |
NaotoMorita | 38:acc7cdcf56dd | 506 | Rmag << 1 << 0 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 507 | << 0 << 1 <<0 |
NaotoMorita | 38:acc7cdcf56dd | 508 | << 0 << 0 <<1; |
NaotoMorita | 38:acc7cdcf56dd | 509 | getIMUval(); |
NaotoMorita | 38:acc7cdcf56dd | 510 | 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 | 511 | for(int i = 0; i<1000 ;i++){ |
NaotoMorita | 38:acc7cdcf56dd | 512 | getIMUval(); |
NaotoMorita | 38:acc7cdcf56dd | 513 | val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm); |
NaotoMorita | 38:acc7cdcf56dd | 514 | } |
NaotoMorita | 38:acc7cdcf56dd | 515 | val_thmg /= 1000; |
NaotoMorita | 38:acc7cdcf56dd | 516 | LoopTicker PIDtick; |
NaotoMorita | 38:acc7cdcf56dd | 517 | PIDtick.attach(calcServoOut,PID_dt); |
NaotoMorita | 38:acc7cdcf56dd | 518 | |
NaotoMorita | 38:acc7cdcf56dd | 519 | t.start(); |
NaotoMorita | 28:fc6c46c1db65 | 520 | |
NaotoMorita | 38:acc7cdcf56dd | 521 | while(1) { |
NaotoMorita | 38:acc7cdcf56dd | 522 | float tstart = t.read(); |
NaotoMorita | 38:acc7cdcf56dd | 523 | //姿勢角を更新 |
NaotoMorita | 38:acc7cdcf56dd | 524 | getIMUval(); |
NaotoMorita | 38:acc7cdcf56dd | 525 | updateBetweenMeasures(); |
NaotoMorita | 38:acc7cdcf56dd | 526 | calcDynAcc(); |
NaotoMorita | 38:acc7cdcf56dd | 527 | float th_mg = acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm); |
NaotoMorita | 38:acc7cdcf56dd | 528 | float dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z); |
NaotoMorita | 38:acc7cdcf56dd | 529 | |
NaotoMorita | 38:acc7cdcf56dd | 530 | //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref)); |
NaotoMorita | 38:acc7cdcf56dd | 531 | int ang_th = abs(th_mg-val_thmg)<0.005; |
NaotoMorita | 38:acc7cdcf56dd | 532 | int dyn_th = dynaccnorm < 0.01/4; |
NaotoMorita | 38:acc7cdcf56dd | 533 | int norm_th = abs(accnorm-accrefnorm)< 0.01/6.0; |
NaotoMorita | 38:acc7cdcf56dd | 534 | //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 | 38:acc7cdcf56dd | 535 | if(ang_th+dyn_th+norm_th>1){ |
NaotoMorita | 38:acc7cdcf56dd | 536 | updateAcrossMeasures(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); |
NaotoMorita | 38:acc7cdcf56dd | 537 | updateAcrossMeasures(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); |
NaotoMorita | 38:acc7cdcf56dd | 538 | } |
NaotoMorita | 38:acc7cdcf56dd | 539 | computeAngles(); |
NaotoMorita | 38:acc7cdcf56dd | 540 | PIDtick.loop(); |
NaotoMorita | 38:acc7cdcf56dd | 541 | |
NaotoMorita | 38:acc7cdcf56dd | 542 | float tend = t.read(); |
NaotoMorita | 38:acc7cdcf56dd | 543 | att_dt = (tend-tstart); |
NaotoMorita | 38:acc7cdcf56dd | 544 | } |
NaotoMorita | 38:acc7cdcf56dd | 545 | }else{ |
NaotoMorita | 38:acc7cdcf56dd | 546 | execCalibration(); |
naker | 20:2c3f113a8e8f | 547 | } |
NaotoMorita | 0:6b18a09a6628 | 548 | } |