solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 38:acc7cdcf56dd
- Parent:
- 37:dad55cf05e3d
- Child:
- 40:869f3791a3e2
- Child:
- 41:4c02fcf105a9
diff -r dad55cf05e3d -r acc7cdcf56dd main.cpp --- a/main.cpp Wed Mar 03 09:32:10 2021 +0000 +++ b/main.cpp Mon Mar 22 02:34:47 2021 +0000 @@ -1,82 +1,394 @@ #include "mbed.h" #include "PIDcontroller.h" #include "SBUS.hpp" -//#include <MadgwickAHRS.h> #include "LoopTicker.hpp" #include "MPU6050.h" -#include <I2Cdev.h> +#include "MAG3110.h" +#include "I2Cdev.h" #include "FastPWM.h" #include "Matrix.h" #include "MatrixMath.h" -#include <math.h> +#include "math.h" #define MPU6050_PWR_MGMT_1 0x6B #define MPU_ADDRESS 0x68 #define pi 3.141562 #define ACCEL_FSR MPU6050_ACCEL_FS_8 -#define ACCEL_SSF 36500.0 +#define ACCEL_SSF 4096.0 #define GYRO_FSR MPU6050_GYRO_FS_250 #define GYRO_SSF 131.0 #define MPU6050_LPF MPU6050_DLPF_BW_256 +#define gyro_th 20.0 +#define PID_dt 0.015 +#define servoPwmMax 1800.0 +#define servoPwmMin 1200.0 +#define motorPwmMax 2000.0 +#define motorPwmMin 1100.0 +#define pitch_align 8.0*3.141562/180.0 +#define roll_align -2.8*3.141562/180.0 MPU6050 accelgyro; -//Madgwick MadgwickFilter; +MAG3110 mag(PB_9,PB_8); SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); Serial sd(PG_14,PG_9); -DigitalOut led1(LED1); -DigitalOut led3(LED3); - +DigitalIn userButton(USER_BUTTON); FastPWM servoRight(PE_9); FastPWM servoLeft(PE_11); -FastPWM servoThrust(PA_0); -const double PID_dt = 0.015; +FastPWM servoThrust(PE_13); PID pitchPID(5.0, 0,0,PID_dt); //rad -PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s +PID pitchratePID(0.5, 0.0, 0.0, PID_dt);//rad/s PID rollPID(5.0,0.0,0.0,PID_dt); -PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s +PID rollratePID(0.5, 0.0, 0.0, PID_dt);//rad/s Timer t; Matrix qhat(4,1); Matrix Phat(4,4); Matrix Qgyro(3,3); Matrix Racc(3,3); - +Matrix Rmag(3,3); +Matrix D(3,3); -int loop_count = 1; +int loop_count = 0; float att_dt = 0.01; float rc[16]; -float roll_ac; -double pitch = 0.0; -double roll = 0.0; -double yaw = 0.0; +float pitch = 0.0; +float roll = 0.0; +float yaw = 0.0; int16_t ax, ay, az; int16_t gx, gy, gz; -double acc_x,acc_y,acc_z; -double gyro_x,gyro_y,gyro_z; +MotionSensorDataUnits mdata; +float magval[3] = {1.0,0.0,0.0}; +float acc_x,acc_y,acc_z; +float dynacc_x,dynacc_y,dynacc_z; +float gyro_x,gyro_y,gyro_z; +float mag_x,mag_y,mag_z; int out1, out2; float scaledServoOut[3]= {0,0}; float scaledMotorOut[1]= {-1}; float servoOut[3] = {1500.0,1500.0}; float motorOut[1] = {1100.0}; -float servoPwmMax = 1800.0; -float servoPwmMin = 1200.0; -float motorPwmMax = 2000.0; -float motorPwmMin = 1100.0; -int offsetVal[6] = {0,0,0,-600,570,265}; + +float accnorm; +float magnorm; +float accrefnorm; +float magrefnorm; + +float val_thmg = 0.0; +float accref[3] = {0, 0, -0.98}; +float magref[3] = {0.65, 0, 0.75}; + +int agoffset[6] = {0, 0, 0, -123, -575, -1}; +float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077}; -const double pitch_align = 0.0; -const double roll_align = -0.0; - +void writeSdcard() +{ + //magcal.getExtremes(&magmin[0],&magmax[0]); + //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]); + //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]); + 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); + //pc.printf("%d \r\n",userButton.read()); + //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); + //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); + //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); + //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); + //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); +} float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } -void writeSdcard() +void calcMagRef(float mx, float my, float mz){ + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2)); + float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1)); + float bx = sqrt(hx * hx + hy * hy); + float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2)); + magref[0] = bx; + magref[1] = 0.0; + magref[2] = bz; +} + + +void getIMUval(){ + // gx gy gz ax ay az + accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); + ax = ax - agoffset[0]; + ay = ay - agoffset[1]; + az = -az - agoffset[2]; + gx = gx - agoffset[3]; + gy = gy - agoffset[4]; + gz = -gz - agoffset[5]; + // 加速度値を分解能で割って加速度(G)に変換する + acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g + acc_y = float(ay) / ACCEL_SSF; + acc_z = float(az) / ACCEL_SSF; + // 角速度値を分解能で割って角速度(rad per sec)に変換する + gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s) + gyro_y = float(gy) / GYRO_SSF * 0.0174533f; + gyro_z = float(gz) / GYRO_SSF * 0.0174533f; + mag.getAxis(mdata); // flush the magnetmeter + magval[0] = (mdata.x - magbias[0]); + magval[1] = (mdata.y - magbias[1]); + magval[2] = (mdata.z - magbias[2]); + mag_x = -magval[0]/magbias[3]; + mag_y = -magval[1]/magbias[3]; + mag_z = -magval[2]/magbias[3]; + + accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z); + magnorm = sqrt(mag_x*mag_x+mag_y*mag_y+mag_z*mag_z); + accrefnorm = sqrt(accref[0]*accref[0]+accref[1]*accref[1]+accref[2]*accref[2]); + magrefnorm = sqrt(magref[0]*magref[0]+magref[1]*magref[1]+magref[2]*magref[2]); + calcMagRef(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm); +} + +void updateBetweenMeasures(){ + Matrix phi(4,4); + phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt + << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt + << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt + << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0; + qhat = phi*qhat; + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix B(4,3); + B << q1 << q2 << q3 + <<-q0 << q3 <<-q2 + <<-q3 <<-q0 << q1 + << q2 <<-q1 <<-q0; + B *= 0.5f; + Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); + + +} + +void updateAcrossMeasures(float v1,float v2, float v3, float u1, float u2, float u3, Matrix R){ + + Matrix u(3,1); + Matrix v(3,1); + + u << u1 << u2 <<u3; + v << v1 << v2 <<v3; + + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix A1(3,3); + A1 << q0 << q3 << -q2 + <<-q3 << q0 << q1 + <<q2 <<-q1 <<q0; + A1 *= 2.0f; + + Matrix A2(3,3); + A2 << q1 << q2 << q3 + << q2 <<-q1 << q0 + << q3 <<-q0 <<-q1; + A2 *= 2.0f; + + Matrix A3(3,3); + A3 <<-q2 << q1 <<-q0 + << q1 << q2 << q3 + << q0 << q3 <<-q2; + A3 *= 2.0f; + + Matrix A4(3,3); + A4 <<-q3 << q0 << q1 + <<-q0 <<-q3 << q2 + << q1 << q2 << q3; + A4 *= 2.0f; + + Matrix H(3,4); + + Matrix ab1(A1*u); + Matrix ab2(A2*u); + Matrix ab3(A3*u); + Matrix ab4(A4*u); + + H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 ) + << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) + << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); + + D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); + D.add(1,2,2*(q1*q2 + q0*q3)); + D.add(1,3,2*(q1*q3 - q0*q2)); + D.add(2,1,2*(q1*q2 - q0*q3)); + D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); + D.add(2,3,2*(q2*q3 + q0*q1)); + D.add(3,1,2*(q1*q3 + q0*q2)); + D.add(3,2,2*(q2*q3 - q0*q1)); + D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); + + + Matrix K(4,3); + K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); + + Matrix dq(4,1); + dq = K*(v-D*u); + qhat = qhat+dq; + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); + + Matrix eye4(4,4); + eye4 << 1 << 0 << 0 << 0 + << 0 << 1 << 0 << 0 + << 0 << 0 << 1 << 0 + << 0 << 0 << 0 << 1; + Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K); +} + +void computeAngles() { - pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f); + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align; + pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align; + yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); +} + +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){ + Matrix W1(3,1); + W1 << fb1 << fb2<< fb3; + Matrix W2(3,1); + W2 << mb1 << mb2<< mb3; + + Matrix V1(3,1); + V1 << fn1 << fn2<< fn3; + Matrix V2(3,1); + V2 << mn1 << mn2<< mn3; + + + Matrix Ou2(3,1); + 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 ); + Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); + Matrix Ou3(3,1); + 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 ); + Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); + Matrix R2(3,1); + 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 ); + R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); + Matrix R3(3,1); + 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 ); + R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); + + Matrix Mou(3,3); + Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 ) + << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 ) + << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 ); + Matrix Mr(3,3); + Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 ) + << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 ) + << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 ); + + Matrix Cbn = Mr*MatrixMath::Transpose(Mou); + + float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 )); + + qhat.add(1,1,0.5*sqtrp1); + qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1); + qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1); + qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1); + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); +} + +void calcDynAcc(){ + dynacc_x = acc_x-(D.getNumber( 1, 1 )*accref[0]+D.getNumber( 1, 2 )*accref[1]+D.getNumber( 1, 3 )*accref[2]); + dynacc_y = acc_y-(D.getNumber( 2, 1 )*accref[0]+D.getNumber( 2, 2 )*accref[1]+D.getNumber( 2, 3 )*accref[2]); + dynacc_z = acc_z-(D.getNumber( 3, 1 )*accref[0]+D.getNumber( 3, 2 )*accref[1]+D.getNumber( 3, 3 )*accref[2]); +} + +void execCalibration(){ + pc.printf("\r\nEnter to Calibration Mode\r\n"); + wait(5); + pc.printf("\r\n Acc and Gyro Calibration Start\r\n"); + int axs = 0; + int ays = 0; + int azs = 0; + int gxs = 0; + int gys = 0; + int gzs = 0; + int iter_n = 5000; + for(int i = 0;i<iter_n ;i++){ + accelgyro.getMotion6(&ay, &ax, &az, &gy, &gx, &gz); + axs += ax; + ays += ay; + azs -= az; + gxs += gx; + gys += gy; + gzs -= gz; + wait(0.001); + } + axs = axs /iter_n; + ays = ays /iter_n; + azs = azs /iter_n; + gxs = gxs /iter_n; + gys = gys /iter_n; + gzs = gzs /iter_n; + pc.printf("\r\nagoffset : 0, 0, 0, %d, %d, %d \r\n",gxs,gys,gzs); + + pc.printf("\r\n Mag Calibration Start\r\n"); + float f = 0; + while(1){ + mag.getAxis(mdata); // flush the magnetmeter + magval[0] = (mdata.x - magbias[0]); + magval[1] = (mdata.y - magbias[1]); + magval[2] = (mdata.z - magbias[2]); + float mag_r = magval[0]*magval[0] + magval[1]*magval[1] + magval[2]*magval[2]; + float lr = 0.00001f; + f = mag_r - magbias[3]*magbias[3]; + magbias[0] = magbias[0] + 4 * lr * f * magval[0]; + magbias[1] = magbias[1] + 4 * lr * f * magval[1]; + magbias[2] = magbias[2] + 4 * lr * f * magval[2]; + magbias[3] = magbias[3] + 4 * lr * f * magbias[3]; + + if(userButton.read() == 1){ + break; + } + wait(0.001); + } + pc.printf("magbias : %f, %f, %f, %f\r\n",magbias[0],magbias[1],magbias[2],magbias[3]); + pc.printf("\r\n Refvec Calibration waiting\r\n"); + wait(5); + pc.printf("\r\n Calibration Start\r\n"); + float arefs[3] = {0.0,0.0,0.0}; + float mrefs[3] = {0.0,0.0,0.0}; + for(int i = 0;i<iter_n ;i++){ + getIMUval(); + arefs[0] += acc_x; + arefs[1] += acc_y; + arefs[2] += acc_z; + mrefs[0] += mag_x; + mrefs[1] += mag_y; + mrefs[2] += mag_z; + wait(0.001); + } + arefs[0] /= iter_n; + arefs[1] /= iter_n; + arefs[2] /= iter_n; + mrefs[0] /= iter_n; + mrefs[1] /= iter_n; + mrefs[2] /= iter_n; + pc.printf("\r\naccreg : %f, %f, %f\r\n",arefs[0],arefs[1],arefs[2]); + pc.printf("\r\nmagreg : %f, %f, %f\r\n",mrefs[0],mrefs[1],mrefs[2]); + while(1) {wait(1000);} } // 割り込まれた時点での出力(computeの結果)を返す関数 @@ -88,14 +400,11 @@ } } - - //pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; - //roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; pitchPID.setProcessValue(pitch); pitchratePID.setProcessValue(gyro_y); rollPID.setProcessValue(roll); rollratePID.setProcessValue(gyro_x); - float de = -pitchPID.compute()-pitchratePID.compute()-rc[1]+rc[0]; + float de = pitchPID.compute()+pitchratePID.compute()-rc[1]+rc[0]; float da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1]; float dT = rc[2]; @@ -103,8 +412,8 @@ scaledServoOut[1]=-de+da; scaledMotorOut[0]= dT; - double LP_servo = 0.2; - double LP_motor = 0.2; + float LP_servo = 0.2; + float LP_motor = 0.2; for(int i = 0;i<sizeof(servoOut)/sizeof(servoOut[0]);i++){ servoOut[i] = LP_servo*(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax))+(1.0-LP_servo)*servoOut[i]; if(servoOut[i]<servoPwmMin) { @@ -136,156 +445,6 @@ } } -void updateAttitude(){ - // gx gy gz ax ay az - accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); - // 加速度値を分解能で割って加速度(G)に変換する - acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g - acc_y = ay / ACCEL_SSF; - acc_z = az / ACCEL_SSF; - // 角速度値を分解能で割って角速度(deg per sec)に変換する - gyro_x = gx / GYRO_SSF; // (deg/s) - gyro_y = gy / GYRO_SSF; - gyro_z = gz / GYRO_SSF; - //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 - //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); -} - -void getIMUval(){ - // gx gy gz ax ay az - accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); - ax = ax - offsetVal[0]; - ay = ay - offsetVal[1]; - az = az - offsetVal[2]; - gx = gx - offsetVal[3]; - gy = gy - offsetVal[4]; - gz = gz - offsetVal[5]; - // 加速度値を分解能で割って加速度(G)に変換する - acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g - acc_y = float(ay) / ACCEL_SSF; - acc_z = float(az) / ACCEL_SSF; - // 角速度値を分解能で割って角速度(rad per sec)に変換する - gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s) - gyro_y = float(gy) / GYRO_SSF * 0.0174533f; - gyro_z = float(gz) / GYRO_SSF * 0.0174533f; -} - -void updateBetweenMeasures(){ - - getIMUval(); - - Matrix phi(4,4); - phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt - << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt - << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt - << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0; - qhat = phi*qhat; - - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); - qhat *= (1.0f/ qnorm); - - float q0 = qhat.getNumber( 1, 1 ); - float q1 = qhat.getNumber( 2, 1 ); - float q2 = qhat.getNumber( 3, 1 ); - float q3 = qhat.getNumber( 4, 1 ); - - Matrix B(4,3); - B << q1 << q2 << q3 - <<-q0 << q3 <<-q2 - <<-q3 <<-q0 << q1 - << q2 <<-q1 <<-q0; - B *= 0.5f; - Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); - - -} - -void updateAcrossMeasures(){ - getIMUval(); - - Matrix uacc(3,1); - Matrix vacc(3,1); - - uacc << 0 << 0 << -1; - float accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z); - vacc << acc_x/accnorm << acc_y/accnorm << acc_z/accnorm; - - float q0 = qhat.getNumber( 1, 1 ); - float q1 = qhat.getNumber( 2, 1 ); - float q2 = qhat.getNumber( 3, 1 ); - float q3 = qhat.getNumber( 4, 1 ); - - Matrix A1(3,3); - A1 << q0 << q3 << -q2 - <<-q3 << q0 << q1 - <<q2 <<-q1 <<q0; - A1 *= 2.0f; - - Matrix A2(3,3); - A2 << q1 << q2 << q3 - << q2 <<-q1 << q0 - << q3 <<-q0 <<-q1; - A2 *= 2.0f; - - Matrix A3(3,3); - A3 <<-q2 << q1 <<-q0 - << q1 << q2 << q3 - << q0 << q3 <<-q2; - A3 *= 2.0f; - - Matrix A4(3,3); - A4 <<-q3 << q0 << q1 - <<-q0 <<-q3 << q2 - << q1 << q2 << q3; - A4 *= 2.0f; - - Matrix H(3,4); - - Matrix ab1(A1*uacc); - Matrix ab2(A2*uacc); - Matrix ab3(A3*uacc); - Matrix ab4(A4*uacc); - - H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 ) - << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) - << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); - - Matrix D(3,3); - D << q0*q0 + q1*q1 - q2*q2 - q3*q3 - << 2*(q1*q2 + q0*q3) - << 2*(q1*q3 - q0*q2) - << 2*(q1*q2 - q0*q3) - << q0*q0 - q1*q1 + q2*q2 - q3*q3 - << 2*(q2*q3 + q0*q1) - << 2*(q1*q3 + q0*q2) - << 2*(q2*q3 - q0*q1) - << q0*q0 - q1*q1 - q2*q2 + q3*q3; - - Matrix err(3,1); - err = vacc-D*uacc; - - Matrix K(4,3); - K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Racc); - - Matrix dq(4,1); - dq = K*err; - qhat = qhat-dq; - - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); - qhat *= (1.0f/ qnorm); -} - -void computeAngles() -{ - float q0 = qhat.getNumber( 1, 1 ); - float q1 = qhat.getNumber( 2, 1 ); - float q2 = qhat.getNumber( 3, 1 ); - float q3 = qhat.getNumber( 4, 1 ); - roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2); - pitch = asinf(-2.0f * (q1*q3 - q0*q2)); - yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); -} - int main() { pitchPID.setSetPoint(0.0); @@ -312,48 +471,75 @@ servoLeft.pulsewidth_us(1500.0); servoThrust.pulsewidth_us(1100.0); - qhat << 1 << 0 << 0 << 0; - - Phat << 0.0001 << 0 <<0 <<0 - << 0 << 0.0001 <<0 <<0 - << 0 << 0 <<0.0001 <<0 - << 0 << 0 << 0 << 0.0001; - - Qgyro << 0.0001 << 0 <<0 - << 0 << 0.0001 <<0 - << 0 << 0 <<0.0001; - - Racc << 0.0001 << 0 <<0 - << 0 << 0.0001 <<0 - << 0 << 0 <<0.0001; - - - LoopTicker PIDtick; - PIDtick.attach(calcServoOut,PID_dt); - pc.baud(115200); - //sd.baud(115200); + pc.baud(57600); + sd.baud(57600); + sd.printf("\r\n Program Start \r\n"); accelgyro.initialize(); //加速度計のフルスケールレンジを設定 accelgyro.setFullScaleAccelRange(ACCEL_FSR); - //■角速度計のフルスケールレンジを設定 + //角速度計のフルスケールレンジを設定 accelgyro.setFullScaleGyroRange(GYRO_FSR); //MPU6050のLPFを設定 accelgyro.setDLPFMode(MPU6050_LPF); - //MadgwickFilter.begin(100); //サンプリング周波数Hza - t.start(); - //servoRight.write(0.0f); - while(1) { - float tstart = t.read(); - //姿勢角を更新 - //updateAttitude(); - updateBetweenMeasures(); - updateAcrossMeasures(); - computeAngles(); - PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する + //地磁気 + mag.enable(); + + if(userButton.read() == 0){ + qhat << 1 << 0 << 0 << 0; + + Phat << 0.01 << 0 <<0 <<0 + << 0 << 0.01 <<0 <<0 + << 0 << 0 <<0.001 <<0 + << 0 << 0 << 0 << 0.001; + + Qgyro << 5.4732e-04 << 0 <<0 + << 0 <<5.4732e-04 <<0 + << 0 << 0 <<5.4732e-04; - float tend = t.read(); - att_dt = (tend-tstart); - //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz + Racc.add(1,1,1.0); + Racc.add(2,2,1.0); + Racc.add(3,3,1.0); + + Rmag << 1 << 0 <<0 + << 0 << 1 <<0 + << 0 << 0 <<1; + getIMUval(); + 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); + for(int i = 0; i<1000 ;i++){ + getIMUval(); + val_thmg += acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm); + } + val_thmg /= 1000; + LoopTicker PIDtick; + PIDtick.attach(calcServoOut,PID_dt); + + t.start(); + while(1) { + float tstart = t.read(); + //姿勢角を更新 + getIMUval(); + updateBetweenMeasures(); + calcDynAcc(); + float th_mg = acos((mag_x*acc_x+mag_y*acc_y+mag_z*acc_z)/magnorm/accnorm); + float dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z); + + //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref)); + int ang_th = abs(th_mg-val_thmg)<0.005; + int dyn_th = dynaccnorm < 0.01/4; + int norm_th = abs(accnorm-accrefnorm)< 0.01/6.0; + //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)); + if(ang_th+dyn_th+norm_th>1){ + updateAcrossMeasures(mag_x/magnorm, mag_y/magnorm, mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); + updateAcrossMeasures(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); + } + computeAngles(); + PIDtick.loop(); + + float tend = t.read(); + att_dt = (tend-tstart); + } + }else{ + execCalibration(); } } \ No newline at end of file