2017年度の製作を開始します。
Dependencies: BufferedSoftSerial2 SDFileSystem-RTOS mbed mbed-rtos INA226_ver1
Fork of keiki2016ver5 by
Diff: main.cpp
- Branch:
- 7_????????????
- Revision:
- 17:0a0c4277d960
- Parent:
- 11:90e2b070f337
- Child:
- 18:fa3f9ba17af8
--- a/main.cpp Thu Oct 27 15:56:19 2016 +0000 +++ b/main.cpp Fri Oct 28 15:08:14 2016 +0000 @@ -1,4 +1,5 @@ //計器プログラム + #include "mbed.h" #include "Fusokukei.h" #include "MPU6050.h" @@ -13,7 +14,6 @@ #define WRITE_DATAS_LOOP_TIME 1 #define ROLL_R_MAX_DEG 2 #define ROLL_L_MAX_DEG 2 -#define MPU_DELT_MIN 250 #define SD_WRITE_NUM 10 #define INIT_SERVO_PERIOD_MS 20 @@ -28,8 +28,7 @@ Fusokukei air; volatile int air_kaitensu= 0; -float sum = 0; -uint32_t sumCount = 0; + MPU6050 mpu6050; Timer t; Ticker mpu6050Ticker; @@ -58,8 +57,6 @@ void call_calcAirSpeed(); void init(); void FusokukeiInit(); -void MpuInit(); -void mpuProcessing(); void SdInit(); void DataReceiveFromSouda(); void WriteDatas(); @@ -81,7 +78,7 @@ kisokuServo.period_ms(INIT_SERVO_PERIOD_MS); geikakuServo.period_ms(INIT_SERVO_PERIOD_MS); FusokukeiInit(); - MpuInit(); + mpu6050.MPUInit(t); SdInit(); //writeDatasTicker.attach(&WriteDatas,1); //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq); @@ -92,71 +89,13 @@ FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME); } -void MpuInit(){ - i2c.frequency(400000); // use fast (400 kHz) I2C - t.start(); - uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 - if (whoami == 0x68) { // WHO_AM_I should always be 0x68 - wait(1); - mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values - wait(1); - if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { - mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration - mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers - mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature - wait(2); - } else { - } - } else { - //pc.printf("out\n\r"); // Loop forever if communication doesn't happen - } -} + double calcPulse(int deg){ return (0.0006+(deg/180.0)*(0.00235-0.00045)); } -void mpuProcessing(){ - if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt - mpu6050.readAccelData(accelCount); // Read the x/y/z adc values - mpu6050.getAres(); - ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set - ay = (float)accelCount[1]*aRes - accelBias[1]; - az = (float)accelCount[2]*aRes - accelBias[2]; - mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values - mpu6050.getGres(); - gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set - gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; - gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; - tempCount = mpu6050.readTempData(); // Read the x/y/z adc values - temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade - } - Now = t.read_us(); - deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update - lastUpdate = Now; - sum += deltat; - sumCount++; - if(lastUpdate - firstUpdate > 10000000.0f) { - beta = 0.04; // decrease filter gain after stabilized - zeta = 0.015; // increasey bias drift gain after stabilized - } - mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); - delt_t = t.read_ms() - count; - if (delt_t > MPU_DELT_MIN) { - yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); - pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); - roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - pitch *= 180.0f / PI; - yaw *= 180.0f / PI; - roll *= 180.0f / PI; - myled= !myled; - count = t.read_ms(); - sum = 0; - sumCount = 0; - } -} - void SdInit(){ mkdir("/sd/mydir", 0777); fp = fopen("/sd/mydir/sdtest2.csv", "w"); @@ -277,7 +216,7 @@ init(); while(1){ pc.printf("test\n\r"); - mpuProcessing(); + mpu6050.mpuProcessing(t); RollAlarm(); DataReceiveFromSouda(); WriteDatas();