2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
Diff: main.cpp
- Revision:
- 29:2da9b8d03c0b
- Parent:
- 26:50272431cd1e
- Child:
- 30:66fa18093418
--- a/main.cpp Wed Jan 25 12:24:10 2017 +0000 +++ b/main.cpp Fri Feb 17 07:38:36 2017 +0000 @@ -3,7 +3,8 @@ #include "mbed.h" #include "rtos.h" #include "Fusokukei.h" -#include "MPU6050.h" +//#include "xMPU6050.h" +#include "MPU6050_DMP6.h" #include "SDFileSystem.h" #include "BufferedSoftSerial.h" #include "Cadence.h" @@ -56,7 +57,10 @@ float sum = 0; uint32_t sumCount = 0; -MPU6050 mpu6050; +//MPU6050 mpu6050; +float yaw,pitch,roll; +float pre_ypr[3]; +float offset_ypr[3]; Timer t; // //AnalogIn kx_X(p17); @@ -73,7 +77,7 @@ DigitalOut RollAlarmR(p20); DigitalOut RollAlarmL(p19); -//DigitalOut led(LED1); +//DigitalOut led(LED1);//for mpu DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); @@ -117,6 +121,7 @@ void call_calcAirSpeed(){ air.calcAirSpeed(air_kaitensu); air_kaitensu = 0; + led3 = !led3; } //void sonarInterruptStart(){ @@ -147,7 +152,7 @@ } void FusokukeiInit(){ - FusokukeiPin.rise(air_countUp); + FusokukeiPin.rise(&air_countUp); FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME); } @@ -163,27 +168,35 @@ } fprintf(fp, "Hello fun SD Card World!\n\r"); fclose(fp); -// Thread sd_thread(&SDprintf); } + 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 - Thread::wait(1); - mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values - Thread::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 - Thread::wait(2); - } else { - } - } else { - pc.printf("MPU6050 not ready...\n\r"); - //pc.printf("out\n\r"); // Loop forever if communication doesn't happen - } +// 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 +// Thread::wait(1); +// mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values +// Thread::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 +// Thread::wait(2); +// } else { +// } +// } else { +// pc.printf("MPU6050 not ready...\n\r"); +// //pc.printf("out\n\r"); // Loop forever if communication doesn't happen +// } + using namespace MPU6050DMP6; + setup(); + do{ + for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i]; + getYPR(); + Thread::wait(0.01); + }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) ); + for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i]; } //void SonarInit(){ @@ -197,53 +210,73 @@ for(int i =0; i<20; i++){ sonarV += sonarPin.read(); Thread::wait(0.01); +// wait(0.01); } sonarDist = (sonarV/20)*2062.5; Thread::wait(0.01); +// wait(0.01); } } +//void mpuProcessing(void const *argument){ +// MpuInit(); +// while(1){ +// Thread::wait(0.1); +// 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 mpuProcessing(void const *argument){ - MpuInit(); + using namespace MPU6050DMP6; + setup(); + do{ + for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i]; + getYPR(); + Thread::wait(0.01); + }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) ); + for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i]; +// MpuInit(); while(1){ - Thread::wait(0.1); - 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; - } + getYPR(); + yaw = (ypr[0] - offset_ypr[0]) *180/M_PI; + pitch = (ypr[1] - offset_ypr[1]) *180/M_PI; + roll = (ypr[2] - offset_ypr[2]) *180/M_PI; + Thread::wait(0.01); } } @@ -297,12 +330,12 @@ //writeDatas[write_datas_index][i] = 0.0; writeDatas[write_datas_index][i] = (float)soudaDatas[i+6]; } - writeDatas[write_datas_index][i++] = cadence.cadence; - writeDatas[write_datas_index][i++] = sonarDist; writeDatas[write_datas_index][i++] = pitch; writeDatas[write_datas_index][i++] = roll; writeDatas[write_datas_index][i++] = yaw; writeDatas[write_datas_index][i++] = airSpeed; + writeDatas[write_datas_index][i++] = sonarDist; + writeDatas[write_datas_index][i++] = cadence.cadence; //writeDatas[write_datas_index][i++] = writeTimer.read(); for(i = 0; i < WRITE_DATAS_NUM; i++){ pc.printf("%f ", writeDatas[write_datas_index][i]); @@ -319,13 +352,11 @@ twe.printf("%i,",soudaDatas[i]); ssMutex.unlock(); } -// twe.printf("%f\n\r",cadence.cadence); -// pc.printf("%f\n\r",cadence.cadence); - //pc.printf("\n\r"); - -if(Android.writeable()){ - Android.printf("%f,%f,%f",airSpeed,roll,0); -} + + if(Android.writeable()){ + Android.printf("%f,%f,%f",airSpeed,roll,0); + } + ssMutex.lock(); twe.printf("%f,%f,%f,",pitch,roll,yaw); // twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); @@ -341,7 +372,7 @@ void WriteDatasF(){ pc.printf("airSpeed:%f\n\r",airSpeed); } -// + //float calcKXdeg(float x){ // return -310.54*x+156.65; //} @@ -367,7 +398,7 @@ -int main(){ +int main(){ Thread sd_thread(&SDprintf);//必ずmain内で Thread sonar_thread(&sonarCalc); Thread mpu_thread(&mpuProcessing); @@ -376,10 +407,10 @@ init(); while(1){ pc.printf("test\n\r"); -// mpuProcessing(); RollAlarm(); // DataReceiveFromSouda(); // cadence.readData(); WriteDatas(); +// mpuProcessing(); } } \ No newline at end of file