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
Diff: setup.cpp
- Revision:
- 70:99f974d8960e
- Parent:
- 68:b9f6938fab9d
- Child:
- 73:84ffa0166e6c
diff -r 401c5d4454fc -r 99f974d8960e setup.cpp --- a/setup.cpp Mon Jun 28 01:45:12 2021 +0000 +++ b/setup.cpp Tue Jun 29 08:07:55 2021 +0000 @@ -4,17 +4,27 @@ { pitchPID.setSetPoint(0.0); pitchratePID.setSetPoint(0.0); + rollPID.setSetPoint(0.0); + rollratePID.setSetPoint(0.0); pitchPID.setBias(0.0); - pitchratePID.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); - pitchPID.setInputLimits(-M_PI, M_PI); - pitchratePID.setInputLimits(-M_PI, M_PI); + rollPID.setOutputLimits(-1.0,1.0); + rollratePID.setOutputLimits(-1.0,1.0); + pitchPID.setInputLimits(-M_PI,M_PI); + pitchratePID.setInputLimits(-M_PI,M_PI); + rollPID.setInputLimits(-M_PI,M_PI); + rollratePID.setInputLimits(-M_PI,M_PI); - elevServo.period_us(15000.0); - elevServo.pulsewidth_us(1500.0); - rudServo.period_us(15000.0); - rudServo.pulsewidth_us(1500.0); + 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); accelgyro.initialize(); //加速度計のフルスケールレンジを設定 @@ -86,28 +96,30 @@ pc.serial.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); pc.serial.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); - pc.serial.printf("Mag Calibration Start\r\n"); + pc.serial.printf("Acc Scale and Mag Calibration Start\r\n"); + for(int i = 0;i<3;i++){ + accMin[i] = -1.0f; + accMax[i] = 1.0f; + } - float inputMag[3]; - float outputMag[3]; - while(1) - { - mag_sensor.getAxis(mdata); // flush the magnetmeter - inputMag[0] = mdata.x; - inputMag[1] = mdata.y; - inputMag[2] = mdata.z; - magCalibrator.run(inputMag,outputMag); - if(userButton.read() == 1) - { - break; - } - wait(0.001); - } + accMax[2] = accScaleCalibrate(5); + accMin[0] = accScaleCalibrate(1); + accMax[0] =accScaleCalibrate(2); + accMin[1] =accScaleCalibrate(3); + accMax[1] =accScaleCalibrate(4); + accMin[2] =accScaleCalibrate(6); + magCalibrator.getExtremes(magbiasMin,magbiasMax); pc.serial.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); pc.serial.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); magCalibrator.setExtremes(magbiasMin,magbiasMax); + pc.serial.printf("accMin : %f, %f, %f\r\n", accMin[0], accMin[1], accMin[2]); + pc.serial.printf("accMax : %f, %f, %f\r\n", accMax[0], accMax[1], accMax[2]); + + pc.serial.printf("Keep Level \r\n"); + wait(5); + pc.serial.printf("Calculating pitch/roll Offset \r\n"); //姿勢オフセットを計算 rpy_align.y = 0.0f*M_PI/180.0f; @@ -137,18 +149,149 @@ ekf.computeAngles(rpy, rpy_align); if(i>199) { - ave_pitch += rpy.y; - ave_roll += rpy.x; + ave_pitch += rpy.x; + ave_roll += rpy.y; } wait(0.001); float tend = _t.read(); att_dt = (tend-tstart); } - pc.serial.printf("aliginment data : %f(pitch deg) %f(roll deg)\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI); + pc.serial.printf("aliginment data(rpy.x, rpy.y, rpy.z) : %ff*M_PI/180.0f, %ff*M_PI/180.0f, 0.0f*M_PI/180.0f\r\n",ave_pitch/2000.0f*180.0f/M_PI,ave_roll/2000.0f*180.0f/M_PI); + + + pc.serial.printf("Calibration Complete\r\n"); while(1) { wait(1000); } +} + +float accScaleCalibrate(int attNo) +{ + //attNo 1:Right down (acc.x Negative) + //attNo 2:Left down (acc.x Positive) + //attNo 3:Nose down (acc.y Negative) + //attNo 4:Tail down (acc.y Positive) + //attNo 5:Level (acc.z Positive) + //attNo 6:upside down (acc.z Negative) + //acc scale calibration + switch(attNo){ + case 1: + pc.serial.printf("Right down (acc.x Negative)\r\n"); + break; + case 2: + pc.serial.printf("Left down (acc.x Positive)\r\n"); + break; + case 3: + pc.serial.printf("Nose down (acc.y Negative)\r\n"); + break; + case 4: + pc.serial.printf("Tail down (acc.y Positive)\r\n"); + break; + case 5: + pc.serial.printf("Level (acc.z Positive)\r\n"); + break; + case 6: + pc.serial.printf("Upside down (acc.z Negative)\r\n"); + break; + default : + pc.serial.printf("error"); + break; + } + + while(1){ + double accx = 0.0; + double accy = 0.0; + double accz = 0.0; + for(int i = 0;i<100 ;i++) + { + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + accx += ax/ ACCEL_SSF; + accy += ay/ ACCEL_SSF; + accz += az/ ACCEL_SSF; + wait(0.01); + } + bool breakFlag = false; + switch(attNo){ + case 1: + if(abs(accx/100.0+1.0)<0.1){ + breakFlag = true; + }; + break; + case 2: + if(abs(accx/100.0-1.0)<0.1){ + breakFlag = true; + }; + break; + case 3: + if(abs(accy/100.0+1.0)<0.1){ + breakFlag = true; + }; + break; + case 4: + if(abs(accy/100.0-1.0)<0.1){ + breakFlag = true; + }; + break; + case 5: + if(abs(accz/100.0-1.0)<0.1){ + breakFlag = true; + }; + break; + case 6: + if(abs(accz/100.0+1.0)<0.1){ + breakFlag = true; + }; + break; + } + if(breakFlag){break;}; + pc.serial.printf("acc %f %f %f\r\n", accx/100.0,accy/100.0,accz/100.0); + } + pc.serial.printf("Keep it hold\r\n"); + int iter_n = 1000; + double accx = 0.0; + double accy = 0.0; + double accz = 0.0; + for(int i = 0;i<iter_n ;i++) + { + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + accx += ax/ ACCEL_SSF; + accy += ay/ ACCEL_SSF; + accz += az/ ACCEL_SSF; + + float inputMag[3]; + float outputMag[3]; + mag_sensor.getAxis(mdata); // flush the magnetmeter + inputMag[0] = mdata.x; + inputMag[1] = mdata.y; + inputMag[2] = mdata.z; + magCalibrator.run(inputMag,outputMag); + + wait(0.001); + } + float returnval = 0.0f; + switch(attNo){ + case 1: + returnval = accx/1000.0f; + break; + case 2: + returnval = accx/1000.0f; + break; + case 3: + returnval = accy/1000.0f; + break; + case 4: + returnval = accy/1000.0f; + break; + case 5: + returnval = accz/1000.0f; + break; + case 6: + returnval = accz/1000.0f; + break; + } + + return returnval; } \ No newline at end of file