Diff: main.cpp

Revision:

42:24c93e42c3f4

Parent:

40:869f3791a3e2

--- a/main.cpp	Tue Mar 23 06:37:24 2021 +0000
+++ b/main.cpp	Thu Mar 25 02:55:00 2021 +0000
@@ -96,6 +96,53 @@
 int agoffset[6] = {0, 0, 0, -123, -575, -1};
 float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077};
 
+// eepromのread writeのためのunionを定義
+int address = 0xAE; // EEPROMの3つの入力が全て+より
+int pointeraddress = 0;
+
+I2C i2c(PB_9,PB_8);  // sda, scl
+union U{
+ int i[8]; // 0: flag(>1で正常に書き込まれた状態) 1 ~ 3: 磁場の中心座標, 4: 磁場の半径、 5~7; mag, 8~10: acc bias
+ char c[32]; // floatはcharの4倍
+};
+int gxs = 0;
+int gys = 0;
+int gzs = 0;
+
+
+
+// eeprom書き込み・読み込みに必要な関数
+void writeEEPROM(int address, unsigned int eeaddress, char *data, int size)
+{
+    char i2cBuffer[size + 2];
+    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
+    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
+ 
+    for (int i = 0; i < size; i++) {
+        i2cBuffer[i + 2] = data[i];
+    }
+ 
+    int result = i2c.write(address, i2cBuffer, size + 2, false);
+    //sleep_ms(500);
+}
+ 
+// this function has no read limit
+void readEEPROM(int address, unsigned int eeaddress, char *data, int size)
+{
+    char i2cBuffer[2];
+    i2cBuffer[0] = (unsigned char)(eeaddress >> 8); // MSB
+    i2cBuffer[1] = (unsigned char)(eeaddress & 0xFF); // LSB
+ 
+    // Reset eeprom pointer address
+    int result = i2c.write(address, i2cBuffer, 2, false);
+    
+    //sleep_ms(500);
+ 
+    // Read eeprom
+    i2c.read(address, data, size);
+    //sleep_ms(500);
+}
+
 void writeSdcard()
 {
     //magcal.getExtremes(&magmin[0],&magmax[0]);
@@ -350,82 +397,119 @@
      dynacc_z = LPacc_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);
+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;
+  gxs = 0;
+  gys = 0;
+  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;
     }
-    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);
+    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);}
+  pc.printf("\r Writing to EEPROM...\r\n");
+  U transfer_data;
+  transfer_data.i[0] = 2;
+  for (int i = 1; i < 5; i++) {
+    if (!isnan(magbias[i - 1]))
+      transfer_data.i[i] = (int)magbias[i - 1];  // intに丸めた値を送る。
+    else {
+      pc.printf("mag bias is NOT transferred\n");
+      transfer_data.i[i] = 100;
     }
-    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);
+  }
+  // gxs,gys,gzsを送る
+  int gxyzs[3] = {gxs, gys, gzs};
+  for (int i = 5; i < 8; i++) {
+    if (!isnan(gxyzs[i - 5]))
+      transfer_data.i[i] = gxyzs[i - 5];
+    else {
+      pc.printf("gxyzs is NOT transferred\n");
+      transfer_data.i[i] = 0;
     }
-    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);}
+  }
+   
+  for (int i = 0; i < 8; i++) {
+    pc.printf("transfer_data[%d]: %\d\n", i, transfer_data.i[i]);
+  }
+  writeEEPROM(address, pointeraddress, transfer_data.c, 32);
+  wait(3);
+  U read_test;
+  readEEPROM(address, pointeraddress, read_test.c, 32);
+  wait(3);
+  for (int i = 0 ; i < 8; i ++){
+      pc.printf("transfer_data[%d]: %d\n",i, read_test.i[i]); 
+  }
 }
 
+
 // 割り込まれた時点での出力(computeの結果）を返す関数
 void calcServoOut(){
     if(sbus.failSafe == false) {
@@ -482,13 +566,25 @@
 }
 
 int main()
-{    
+{
+    pc.baud(57600);
+    U read_data; // eepromからの読み込み
+    readEEPROM(address, pointeraddress, read_data.c, 32);
+    pc.printf("%d",read_data.i[0]); // for debug
+    if(read_data.i[0] < 0){
+        pc.printf("No Data Stored in EEPROM!!!");
+        execCalibration();
+    }
+    for (int i = 1 ; i < 5; i ++) magbias[i] = read_data.i[i];
+    gxs = read_data.i[5];
+    gys = read_data.i[6];
+    gzs = read_data.i[7];
     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);
@@ -507,7 +603,6 @@
     servoLeft.pulsewidth_us(1500.0); 
     servoThrust.pulsewidth_us(1100.0);
     
-    pc.baud(57600);
     sd.baud(57600);
     sd.printf("\r\n Program Start \r\n");
     accelgyro.initialize();