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:
- 61:c05353850017
- Parent:
- 60:b89ac376fba3
- Child:
- 65:ea184054e659
--- a/setup.cpp Wed Jun 02 06:14:47 2021 +0000 +++ b/setup.cpp Thu Jun 03 11:28:13 2021 +0000 @@ -70,40 +70,42 @@ wait(3); for (int i = 0 ; i < N_EEPROM; i ++) { - magbias[0] = float(read_bias.i[1])/1000.0f; - magbias[1] = float(read_bias.i[2])/1000.0f; - magbias[2] = float(read_bias.i[3])/1000.0f; - magbias[3] = float(read_bias.i[4])/1000.0f; - } + magbiasMin[0] = float(read_bias.i[1])/1000.0f; + magbiasMin[1] = float(read_bias.i[2])/1000.0f; + magbiasMin[2] = float(read_bias.i[3])/1000.0f; + magbiasMax[0] = float(read_bias.i[4])/1000.0f; + magbiasMax[1] = float(read_bias.i[5])/1000.0f; + magbiasMax[2] = float(read_bias.i[6])/1000.0f; + } + magCalibrator.setExtremes(magbiasMin,magbiasMax); }else{ pc.printf("Preset Initial Magbias \r\n"); } - pc.printf("Initial Magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2], magbias[3]); + magCalibrator.getExtremes(magbiasMin,magbiasMax); + pc.printf("Initial Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); + pc.printf("Initial Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); + pc.printf("Mag Calibration Start\r\n"); - - float f = 0; - + float inputMag[3]; + float outputMag[3]; while(1) { mag_sensor.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.000001f; - 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]; + inputMag[0] = mdata.x; + inputMag[1] = mdata.y; + inputMag[2] = mdata.z; + magCalibrator.run(inputMag,outputMag); if(userButton.read() == 1) { break; } wait(0.001); } - pc.printf("Magbias : %f, %f, %f, %f\r\n", magbias[0], magbias[1], magbias[2], magbias[3]); + magCalibrator.getExtremes(magbiasMin,magbiasMax); + pc.printf("Magbias (Min) : %f, %f, %f\r\n", magbiasMin[0], magbiasMin[1], magbiasMin[2]); + pc.printf("Magbias (Max) : %f, %f, %f\r\n", magbiasMax[0], magbiasMax[1], magbiasMax[2]); + magCalibrator.getExtremes(magbiasMin,magbiasMax); pc.printf("Determine Position of MBED\r\n"); wait(1); pc.printf("Press the user button\r\n"); @@ -184,39 +186,49 @@ U transfer_data; transfer_data.i[0] = pos_tail; - for (int i = 1; i < 5; i++) + for (int i = 0; i < 3; i++) { - if (!isnan(magbias[i - 1])) - transfer_data.i[i] = int(magbias[i - 1]*1000); // intに丸めた値を送る。 + if (!isnan(magbiasMax[i])) + transfer_data.i[i+1] = int(magbiasMin[i]*1000); // intに丸めた値を送る。 else { pc.printf("Mag bias is NOT transferred\n"); - transfer_data.i[i] = 100; + transfer_data.i[i+1] = 100; + } + } + for (int i = 0; i < 3; i++) + { + if (!isnan(magbiasMax[i])) + transfer_data.i[i+4] = int(magbiasMax[i]*1000); // intに丸めた値を送る。 + else + { + pc.printf("Mag bias is NOT transferred\n"); + transfer_data.i[i+4] = 100; } } // gxs,gys,gzsを送る int gxyzs[3] = {gxs, gys, gzs}; - for (int i = 5; i < 8; i++) + for (int i = 0; i < 3; i++) { - if (!isnan(gxyzs[i - 5])) - transfer_data.i[i] = gxyzs[i - 5]; + if (!isnan(gxyzs[i])) + transfer_data.i[i+7] = gxyzs[i]; else { pc.printf("gxyzs is NOT transferred\n"); - transfer_data.i[i] = 0; + transfer_data.i[i+7] = 0; } } // ave_pitch,ave_rollを送る int ave_pr[2] = {ave_pitch*100, ave_roll*100}; - for (int i = 8; i < 10; i++) + for (int i = 0; i < 2; i++) { - if (!isnan(ave_pr[i - 8])) - transfer_data.i[i] = ave_pr[i - 8]; + if (!isnan(ave_pr[i])) + transfer_data.i[i+10] = ave_pr[i]; else { pc.printf("alignment data is NOT transferred\n"); - transfer_data.i[i] = 0; + transfer_data.i[i+10] = 0; } } // transfer_data.i[10] = tail_address[pos_tail];