Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
main.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-03-03
- Revision:
- 37:dad55cf05e3d
- Parent:
- 36:e68ce293306e
- Child:
- 38:acc7cdcf56dd
File content as of revision 37:dad55cf05e3d:
#include "mbed.h" #include "PIDcontroller.h" #include "SBUS.hpp" //#include <MadgwickAHRS.h> #include "LoopTicker.hpp" #include "MPU6050.h" #include <I2Cdev.h> #include "FastPWM.h" #include "Matrix.h" #include "MatrixMath.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 GYRO_FSR MPU6050_GYRO_FS_250 #define GYRO_SSF 131.0 #define MPU6050_LPF MPU6050_DLPF_BW_256 MPU6050 accelgyro; //Madgwick MadgwickFilter; SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); Serial sd(PG_14,PG_9); DigitalOut led1(LED1); DigitalOut led3(LED3); FastPWM servoRight(PE_9); FastPWM servoLeft(PE_11); FastPWM servoThrust(PA_0); const double PID_dt = 0.015; PID pitchPID(5.0, 0,0,PID_dt); //rad PID pitchratePID(0.01, 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 Timer t; Matrix qhat(4,1); Matrix Phat(4,4); Matrix Qgyro(3,3); Matrix Racc(3,3); int loop_count = 1; float att_dt = 0.01; float rc[16]; float roll_ac; double pitch = 0.0; double roll = 0.0; double 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; 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}; const double pitch_align = 0.0; const double roll_align = -0.0; 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() { pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f); } // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut(){ if(sbus.failSafe == false) { // sbusデータの読み込み for (int i =0 ; i < 16;i ++){ rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input } } //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 da = rollPID.compute()+rollratePID.compute()+rc[0]+rc[1]; float dT = rc[2]; scaledServoOut[0]=de+da; scaledServoOut[1]=-de+da; scaledMotorOut[0]= dT; double LP_servo = 0.2; double 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) { servoOut[i] = servoPwmMin; }; if(servoOut[i]>servoPwmMax) { servoOut[i] = servoPwmMax; }; } for(int i = 0;i<sizeof(motorOut)/sizeof(motorOut[0]) ;i++){ motorOut[i] = LP_motor*(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax))+(1.0-LP_motor)*motorOut[i]; if(motorOut[i]<motorPwmMin) { motorOut[i] = motorPwmMin; }; if(motorOut[i]>motorPwmMax) { motorOut[i] = motorPwmMax; }; } servoRight.pulsewidth_us(servoOut[0]); servoLeft.pulsewidth_us(servoOut[1]); servoThrust.pulsewidth_us(motorOut[0]); if(loop_count > 10){ writeSdcard(); loop_count = 1; }else{ loop_count +=1; } } 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); pitchratePID.setSetPoint(0.0); rollPID.setSetPoint(0.0); rollratePID.setSetPoint(0.0); pitchPID.setBias(0.0); pitchratePID.setBias(0.0); rollPID.setBias(0.0); rollratePID.setBias(0.0); pitchPID.setOutputLimits(-1.0,1.0); pitchratePID.setOutputLimits(-1.0,1.0); rollPID.setOutputLimits(-1.0,1.0); rollratePID.setOutputLimits(-1.0,1.0); pitchPID.setInputLimits(-pi,pi); pitchratePID.setInputLimits(-pi,pi); rollPID.setInputLimits(-pi,pi); rollratePID.setInputLimits(-pi,pi); servoRight.period_us(15000.0); servoLeft.period_us(15000.0); servoThrust.period_us(15000.0); servoRight.pulsewidth_us(1500.0); 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); 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結果を格納する float tend = t.read(); att_dt = (tend-tstart); //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz } }