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:
- 36:e68ce293306e
- Parent:
- 35:4535af88f7bf
- Child:
- 37:dad55cf05e3d
diff -r 4535af88f7bf -r e68ce293306e main.cpp --- a/main.cpp Tue Mar 02 05:01:20 2021 +0000 +++ b/main.cpp Wed Mar 03 03:34:11 2021 +0000 @@ -1,11 +1,14 @@ #include "mbed.h" #include "PIDcontroller.h" #include "SBUS.hpp" -#include <MadgwickAHRS.h> +//#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 @@ -17,24 +20,30 @@ #define MPU6050_LPF MPU6050_DLPF_BW_256 MPU6050 accelgyro; -Madgwick MadgwickFilter; +//Madgwick MadgwickFilter; SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); Serial sd(PG_14,PG_9); DigitalOut led1(LED1); DigitalOut led3(LED3); -PwmOut servoRight(PE_9); -PwmOut servoLeft(PE_11); -PwmOut servoThrust(PA_0); -const double PID_dt = 1.0/200; -PID pitchPID(3.0, 0,0,PID_dt); //rad +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(3.0,0.0,0.0,PID_dt); +PID rollPID(5.0,0.0,0.0,PID_dt); PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s Timer t; -int loop_count = 0; +Matrix qhat(4,1); +Matrix Phat(4,4); +Matrix Racc(3,3); + + +int loop_count = 1; +float att_dt = 0.01; float rc[16]; float roll_ac; double pitch = 0.0; @@ -45,15 +54,15 @@ double acc_x,acc_y,acc_z; double gyro_x,gyro_y,gyro_z; int out1, out2; -float scaledServoOut[3]= {0,0,0}; +float scaledServoOut[3]= {0,0}; float scaledMotorOut[1]= {-1}; -float servoOut[3] = {0.0015,0.0015,0.0015}; -float motorOut[1] = {0.00095}; -float servoPwmMax = 0.002; -float servoPwmMin = 0.0012; -float motorPwmMax = 0.002; -float motorPwmMin = 0.00095; -int offsetVal[6] = {0,0,0,0,0,0}; +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; @@ -66,7 +75,7 @@ void writeSdcard() { - pc.printf("motor: %f, servo1: %f, servo2: %f \r\n",motorOut[0],servoOut[0],servoOut[1]); + pc.printf("gx: %d, gy: %d, gz: %d roll: %f, pitch: %f \r\n",gx,gy,gz,roll* 57.29578f,pitch* 57.29578f); } // 割り込まれた時点での出力(computeの結果)を返す関数 @@ -78,22 +87,25 @@ } } - pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; - roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; + + //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]; - float da = rollPID.compute()+rollratePID.compute()+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]; scaledServoOut[0]=de+da; - scaledServoOut[1]=de-da; + scaledServoOut[1]=-de+da; scaledMotorOut[0]= dT; - for(int i = 0;i<4;i++){ - servoOut[i] = float(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax)); + 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; }; @@ -102,8 +114,8 @@ }; } - for(int i = 0;i<1;i++){ - motorOut[i] = float(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax)); + 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; }; @@ -111,14 +123,13 @@ motorOut[i] = motorPwmMax; }; } - - servoRight.pulsewidth(servoOut[0]); - servoLeft.pulsewidth(servoOut[1]); - servoThrust.pulsewidth(motorOut[0]); + servoRight.pulsewidth_us(servoOut[0]); + servoLeft.pulsewidth_us(servoOut[1]); + servoThrust.pulsewidth_us(motorOut[0]); - if(loop_count > int(1/PID_dt/10.0)){ + if(loop_count > 10){ writeSdcard(); - loop_count = 0; + loop_count = 1; }else{ loop_count +=1; } @@ -136,27 +147,53 @@ 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); + //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); } void updateBetweenMeasures(){ - + // 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 = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g + acc_y = ay / ACCEL_SSF; + acc_z = 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; + + + 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); + +} + +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() -{ - 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); - +{ pitchPID.setSetPoint(0.0); pitchratePID.setSetPoint(0.0); rollPID.setSetPoint(0.0); @@ -174,19 +211,50 @@ rollPID.setInputLimits(-pi,pi); rollratePID.setInputLimits(-pi,pi); - servoRight.period(0.02f); - servoLeft.period(0.02f); - servoThrust.period(0.02f); - MadgwickFilter.begin(100); //サンプリング周波数Hza + 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; + + 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(); + //updateAttitude(); + updateBetweenMeasures(); + computeAngles(); PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する float tend = t.read(); - MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz + att_dt = (tend-tstart); + //MadgwickFilter.begin(1.0f/att_dt); //サンプリング周波数Hz } } \ No newline at end of file