2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
Diff: main.cpp
- Branch:
- Android?????????
- Revision:
- 19:fa3f9ba17af8
- Parent:
- 15:6966299bea4c
- Parent:
- 18:0a0c4277d960
- Child:
- 21:8802034b7810
- Child:
- 24:483dd4e61ead
--- a/main.cpp Sat Nov 26 01:12:10 2016 +0000 +++ b/main.cpp Sat Dec 17 09:12:39 2016 +0000 @@ -1,4 +1,5 @@ //計器プログラム + #include "mbed.h" #include "Fusokukei.h" #include "MPU6050.h" @@ -15,7 +16,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 @@ -23,8 +23,8 @@ //Ticker cadenceTicker; Serial pc(USBTX,USBRX); +Serial Android(p13,p14); BufferedSoftSerial twe(p9,p10); -//Serial soudaSerial(p13,p14); BufferedSoftSerial soudaSerial(p17,p18); //Ticker writeDatasTicker; Timer writeTimer; @@ -34,8 +34,6 @@ Fusokukei air; volatile int air_kaitensu= 0; -float sum = 0; -uint32_t sumCount = 0; MPU6050 mpu6050; Timer t; Ticker mpu6050Ticker; @@ -65,10 +63,6 @@ void call_calcAirSpeed(); void init(); void FusokukeiInit(); -void MpuInit(); -void mpuProcessing(); -//void cadenceRead(); -//void cadenceInit(); void SdInit(); void DataReceiveFromSouda(); void WriteDatas(); @@ -90,16 +84,13 @@ void init(){ twe.baud(19200); + Android.baud(9600); soudaSerial.baud(9600); -// cadenceTicker.attach(&cadenceDataReceive,0.1); - //writeTimer.start(); 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); } void FusokukeiInit(){ @@ -107,72 +98,10 @@ 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"); @@ -185,17 +114,12 @@ void DataReceiveFromSouda(){ led2 = !led2; - //pc.printf("received\n\r"); -// bool kaigyo=0; for(int i = 0; i < SOUDA_DATAS_NUM; i++){ if(soudaSerial.readable()) { soudaDatas[i] = (char)soudaSerial.getc(); if(soudaDatas[i]==';') i=-1; -// else pc.printf("%5d:%3d",i,soudaDatas[i]); -// kaigyo =1; }else i--; } -// if(kaigyo) pc.printf("\n\r"); } void SDprintf(){ @@ -247,6 +171,10 @@ // 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); +} twe.printf("%f,%f,%f,",pitch,roll,yaw); twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); twe.printf("%f,\r\n",airSpeed); @@ -275,7 +203,6 @@ else{ RollAlarmL = 0; } - if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){ RollAlarmR = 1; } @@ -285,7 +212,6 @@ } void WriteServo(){ - //kisokuServo.pulsewidth(calcPulse(airSpeed*10)); kisokuServo.pulsewidth(calcPulse(9*airSpeed)); if(pitch<0){ geikakuServo.pulsewidth(calcPulse(0)); @@ -293,19 +219,13 @@ else{ geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0))); } - //pc.printf("a:%f",airSpeed); - //pc.printf("p:%f\r\n",pitch); - //kisokuServo.pulsewidth(calcPulse(0)); - //geikakuServo.pulsewidth(calcPulse(0)); } int main(){ init(); while(1){ pc.printf("test\n\r"); -// if(test.readable()) pc.printf("%d ",test.getc()); -// pc.printf("<-softserial\n\r"); - mpuProcessing(); + mpu6050.mpuProcessing(t); RollAlarm(); DataReceiveFromSouda(); // cadence.readData();