3/24 計器
Dependencies: SDFileSystem mbed
Fork of keiki2016ver2 by
Diff: main.cpp
- Revision:
- 1:5ec2409854da
- Parent:
- 0:085b2c5e3254
- Child:
- 2:5a1a638f5fe6
--- a/main.cpp Tue Feb 23 13:05:06 2016 +0000 +++ b/main.cpp Wed Feb 24 10:22:59 2016 +0000 @@ -1,3 +1,4 @@ +//計器プログラム #include "mbed.h" #include "Fusokukei.h" #include "MPU6050.h" @@ -5,7 +6,7 @@ #define KX_VALUE_MIN 0.4 #define KX_VALUE_MAX 0.8 -#define SOUDA_DATAS_NUM 14 +#define SOUDA_DATAS_NUM 13 #define WRITE_DATAS_NUM 28 #define MPU_LOOP_TIME 0.01 #define AIR_LOOP_TIME 0.01 @@ -15,7 +16,7 @@ Serial pc(USBTX,USBRX); Serial soudaSerial(p13,p14); -Serial twelite(p9,p10); +Serial twe(p9,p10); Ticker writeDatasTicker; InterruptIn FusokukeiPin(p30); @@ -36,6 +37,7 @@ DigitalOut RollAlarmR(p20); DigitalOut RollAlarmL(p19); DigitalOut led(LED1); +DigitalOut led2(LED2); SDFileSystem sd(p5, p6, p7, p8, "sd"); FILE* fp; @@ -65,11 +67,12 @@ } void init(){ - soudaSerial.attach(DataReceiveFromSouda, Serial::RxIrq); - //wait(0.01); + twe.baud(115200); FusokukeiInit(); MpuInit(); - // SdInit(); + SdInit(); + writeDatasTicker.attach(&WriteDatas,0.25); + //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq); } void FusokukeiInit(){ @@ -123,14 +126,13 @@ } 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 > 200) { // update LCD once per half-second independent of read rate + if (delt_t > 500) { // 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; - pc.printf("p:%f,r:%f,y:%f\n\r",pitch,roll,yaw); myled= !myled; count = t.read_ms(); sum = 0; @@ -149,31 +151,38 @@ } void DataReceiveFromSouda(){ - int soudaDatasIndex = 0; - while(soudaSerial.readable()){ - soudaDatas[soudaDatasIndex++] = soudaSerial.getc(); + led2 = !led2; + pc.printf("received\n\r"); + for(int i = 0; i < SOUDA_DATAS_NUM; i++){ + soudaDatas[i] = soudaSerial.getc(); } } void WriteDatas(){ - //for(int i = 0; i < WRITE_DATAS_NUM; i++){ -// pc.printf("%i\t",writeDatas[i]); -// if(i % 7 == 0){ -// pc.printf("\n\r"); -// } -// } - //pc.printf("p:%f,r:%f,y:%f\n\r",pitch,roll,yaw); - // pc.printf("gx:%f,gy:%f,gz:%f\n\r",gx,gy,gz); - // pc.printf("angleX:%f,angleY:%f,angleZ:%f\n\r",calcKXdeg(gx),calcKXdeg(gy),calcKXdeg(gz)); - //pc.printf("as:%f\n\r",airSpeed); - //fp = fopen("/sd/mydir/sdtest.csv", "a"); -// if(fp == NULL) { -// error("Could not open file for write\n"); -// } -// 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); + 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); + 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]); + } + 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); } float calcKXdeg(float x){ @@ -207,13 +216,11 @@ } int main(){ - pc.printf("test\n\r"); init(); while(1){ mpuProcessing(); kxRead(); - WriteDatas(); RollAlarm(); - wait(0.01); + DataReceiveFromSouda(); } } \ No newline at end of file