Diff: setup.cpp

Revision:

70:99f974d8960e

Parent:

68:b9f6938fab9d

Child:

73:84ffa0166e6c

--- 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