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: 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();