5/27 計器プログラム
Dependencies: SDFileSystem mbed
Fork of keiki2016_513 by
Diff: main.cpp
- Revision:
- 3:8dc516be2e7e
- Parent:
- 2:5a1a638f5fe6
- Child:
- 4:a863a092141c
diff -r 5a1a638f5fe6 -r 8dc516be2e7e main.cpp --- a/main.cpp Thu Feb 25 09:13:46 2016 +0000 +++ b/main.cpp Thu Mar 24 06:42:22 2016 +0000 @@ -7,17 +7,20 @@ #define KX_VALUE_MIN 0.4 #define KX_VALUE_MAX 0.8 #define SOUDA_DATAS_NUM 13 -#define WRITE_DATAS_NUM 28 +#define WRITE_DATAS_NUM 21 #define MPU_LOOP_TIME 0.01 #define AIR_LOOP_TIME 0.01 -#define WRITE_DATAS_LOOP_TIME 0.5 -#define ROLL_R_MAX_DEG 5 -#define ROLL_L_MAX_DEG 5 +#define WRITE_DATAS_LOOP_TIME 1 +#define ROLL_R_MAX_DEG 2 +#define ROLL_L_MAX_DEG 2 +#define MPU_DELT_MIN 500 +#define SD_WRITE_NUM 10 Serial pc(USBTX,USBRX); Serial soudaSerial(p13,p14); Serial twe(p9,p10); Ticker writeDatasTicker; +Timer writeTimer; InterruptIn FusokukeiPin(p30); Ticker FusokukeiTicker; @@ -33,6 +36,7 @@ AnalogIn kx_X(p17); AnalogIn kx_Y(p16); AnalogIn kx_Z(p15); +float KX_X,KX_Y,KX_Z; DigitalOut RollAlarmR(p20); DigitalOut RollAlarmL(p19); @@ -43,7 +47,8 @@ FILE* fp; char soudaDatas[SOUDA_DATAS_NUM]; -char writeDatas[WRITE_DATAS_NUM]; +float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM]; +volatile int write_datas_index = 0; void air_countUp(); void call_calcAirSpeed(); @@ -68,10 +73,11 @@ void init(){ twe.baud(115200); + writeTimer.start(); FusokukeiInit(); MpuInit(); SdInit(); - writeDatasTicker.attach(&WriteDatas,0.25); + //writeDatasTicker.attach(&WriteDatas,1); //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq); } @@ -102,41 +108,42 @@ 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 > 800) { // update LCD once per half-second independent of read rate - 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; + 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; + pc.printf("%f,%f,%f\n\r",pitch,roll,yaw); + myled= !myled; + count = t.read_ms(); + sum = 0; + sumCount = 0; } } @@ -158,31 +165,69 @@ } } -void WriteDatas(){ - pc.printf("write\n\r"); - for(int i = 0; i < SOUDA_DATAS_NUM; i++){ - pc.printf("%i ",soudaDatas[i]); - twe.printf("%i ",soudaDatas[i]); - } - pc.printf("\n\r"); - twe.printf("\n\r"); - twe.printf("%f,%f,%f\n\r",pitch,roll,yaw); - // twe.printf("%f,%f,%f\n\r",gx,gy,gz); -// twe.printf("%f\n\r",airSpeed); - pc.printf("%f,%f,%f\n\r",pitch,roll,yaw); - pc.printf("%f,%f,%f\n\r",gx,gy,gz); - pc.printf("%f\n\r",airSpeed); +void SDprintf(){ fp = fopen("/sd/mydir/sdtest.csv", "a"); if(fp == NULL) { error("Could not open file for write\n"); } - for(int i = 0; i < SOUDA_DATAS_NUM; i++){ - fprintf(fp,"%i ",soudaDatas[i]); + for(int i = 0; i < SD_WRITE_NUM; i++){ + for(int j = 0; j < WRITE_DATAS_NUM; j++){ + fprintf(fp,"%f ", writeDatas[i][j]); + if(i%7==0){ + fprintf(fp,"\n\r"); + } + } + } + //for(int i = 0; i < SOUDA_DATAS_NUM; i++){ +// fprintf(fp,"%i ",soudaDatas[i]); +// } +// fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw); +// fprintf(fp, "gx:%f,gy:%f,gz:%f\n",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); +// fprintf(fp, "as:%f\n",airSpeed); + fclose(fp); +} + +void WriteDatas(){ + int i; + for(i = 0; i < SOUDA_DATAS_NUM; i++){ + writeDatas[write_datas_index][i] = (float)soudaDatas[i]; } - fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw); - fprintf(fp, "gx:%f,gy:%f,gz:%f\n",gx,gy,gz); - fprintf(fp, "as:%f\n",airSpeed); - fclose(fp); + writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read()); + writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read()); + writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read()); + 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++] = writeTimer.read(); + for(i = 0; i < WRITE_DATAS_NUM; i++){ + pc.printf("%f ", writeDatas[write_datas_index][i]); + twe.printf("%f ", writeDatas[write_datas_index][i]); + if(i%7==0){ + pc.printf("\n\r"); + twe.printf("\n\r"); + } + } + if(write_datas_index == SD_WRITE_NUM){ + SDprintf(); + write_datas_index=0; + } + else{ + write_datas_index++; + } + //for(int i = 0; i < SOUDA_DATAS_NUM; i++){ +// pc.printf("%i ",soudaDatas[i]); +// twe.printf("%i ",soudaDatas[i]); +// } +// pc.printf("\n\r"); +// twe.printf("\n\r"); +// twe.printf("%f,%f,%f\n\r",pitch,roll,yaw); +// twe.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); +// twe.printf("%f\n\r",airSpeed); +// pc.printf("%f,%f,%f\n\r",pitch,roll,yaw); +// pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z)); +// pc.printf("%f\n\r",airSpeed); +// SDprintf(); } float calcKXdeg(float x){ @@ -193,12 +238,6 @@ return pitch-calcKXdeg(kx_Z.read()); } -void kxRead(){ - gx = kx_X.read(); - gy = kx_Y.read(); - gz = kx_Z.read(); -} - void RollAlarm(){ if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){ RollAlarmL = 1; @@ -219,8 +258,8 @@ init(); while(1){ mpuProcessing(); - kxRead(); RollAlarm(); DataReceiveFromSouda(); + WriteDatas(); } } \ No newline at end of file