2018年度計器mbed用プログラム
Dependencies: BufferedSoftSerial2 INA226_ver1 mbed-rtos mbed SDFileSystem-RTOS
Fork of keiki2017 by
Diff: main.cpp
- Revision:
- 32:fe006d07c1ea
- Parent:
- 30:66fa18093418
--- a/main.cpp Fri Feb 17 09:06:58 2017 +0000 +++ b/main.cpp Fri Feb 17 12:14:23 2017 +0000 @@ -1,6 +1,7 @@ //計器プログラム #include "mbed.h" +#define __mbed_h__ #include "rtos.h" #include "Fusokukei.h" //#include "xMPU6050.h" @@ -83,8 +84,8 @@ SDFileSystem sd(p5, p6, p7, p8, "sd"); FILE* fp; -PwmOut kisokuServo(p26); -PwmOut geikakuServo(p22); +//PwmOut kisokuServo(p26); +//PwmOut geikakuServo(p22); char soudaDatas[SOUDA_DATAS_NUM]; float writeDatas[SD_WRITE_NUM][WRITE_DATAS_NUM]; @@ -103,6 +104,7 @@ void DataReceiveFromSouda(void const *argument); void SDprintf(void const *argument); void WriteDatas(); +void calcPusle(int deg); //float calcAttackAngle(); //float calcKXdeg(float x); @@ -142,8 +144,8 @@ twe.baud(115200); Android.baud(9600); soudaSerial.baud(9600); - kisokuServo.period_ms(INIT_SERVO_PERIOD_MS); - geikakuServo.period_ms(INIT_SERVO_PERIOD_MS); +// kisokuServo.period_ms(INIT_SERVO_PERIOD_MS); +// geikakuServo.period_ms(INIT_SERVO_PERIOD_MS); FusokukeiInit(); // MpuInit(); //mpuProcessing内で // SonarInit(); @@ -155,10 +157,6 @@ FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME); } -double calcPulse(int deg){ - return (0.0006+(deg/180.0)*(0.00235-0.00045)); -} - void SdInit(){ mkdir("/sd/mydir", 0777); fp = fopen("/sd/mydir/sdtest2.csv", "w"); @@ -248,12 +246,15 @@ //} void mpuProcessing(void const *argument){ using namespace MPU6050DMP6; + led1 = 1; setup(); -// do{ -// for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i]; + int j=0; + do{ + for(int i = 0; i<3; i++) pre_ypr[i] = ypr[i]; getYPR(); -// Thread::wait(0.05); -// }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003) ); + Thread::wait(0.05); + j++; + }while( (pre_ypr[1]-ypr[1]<0.0003)&&(pre_ypr[2]-ypr[2]<0.0003)&&j<20 ); for(int i = 0; i<3; i++) offset_ypr[i] = ypr[i]; // MpuInit(); while(1){ @@ -348,20 +349,20 @@ } for(int i = 0; i < SOUDA_DATAS_NUM; i++){ // pc.printf("%i ",soudaDatas[i]); - ssMutex.lock(); +// ssMutex.lock(); twe.printf("%i,",soudaDatas[i]); - ssMutex.unlock(); +// ssMutex.unlock(); } if(Android.writeable()){ Android.printf("%f,%f,%f",airSpeed,roll,0); } - ssMutex.lock(); +// 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)); twe.printf("%f,\r\n",airSpeed); - ssMutex.unlock(); +// ssMutex.unlock(); // 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); @@ -369,6 +370,10 @@ pc.printf("%d\n\r",write_datas_index); } +double calcPulse(int deg){ + return (0.0006+(deg/180.0)*(0.00235-0.00045)); +} + void WriteDatasF(){ pc.printf("airSpeed:%f\n\r",airSpeed); }