main.cpp

00001 //計器プログラム
00002 #include "mbed.h"
00003 #include "Fusokukei.h"
00004 #include "MPU6050.h"
00005 #include "SDFileSystem.h"
00006 
00007 #define KX_VALUE_MIN 0.4
00008 #define KX_VALUE_MAX 0.8
00009 #define SOUDA_DATAS_NUM 13
00010 #define WRITE_DATAS_NUM 28
00011 #define MPU_LOOP_TIME 0.01
00012 #define AIR_LOOP_TIME 0.01
00013 #define WRITE_DATAS_LOOP_TIME 0.5
00014 #define ROLL_R_MAX_DEG 5
00015 #define ROLL_L_MAX_DEG 5
00016 
00017 Serial pc(USBTX,USBRX);
00018 Serial soudaSerial(p13,p14);
00019 Serial twe(p9,p10);
00020 Ticker writeDatasTicker;
00021 
00022 InterruptIn FusokukeiPin(p30);
00023 Ticker FusokukeiTicker;
00024 Fusokukei air;
00025 volatile int air_kaitensu= 0;
00026 
00027 float sum = 0;
00028 uint32_t sumCount = 0;
00029 MPU6050 mpu6050;
00030 Timer t;
00031 Ticker mpu6050Ticker;
00032 
00033 AnalogIn kx_X(p17);
00034 AnalogIn kx_Y(p16);
00035 AnalogIn kx_Z(p15);
00036 
00037 DigitalOut RollAlarmR(p20);
00038 DigitalOut RollAlarmL(p19);
00039 DigitalOut led(LED1);
00040 DigitalOut led2(LED2);
00041 
00042 SDFileSystem sd(p5, p6, p7, p8, "sd");
00043 FILE* fp;
00044 
00045 char soudaDatas[SOUDA_DATAS_NUM];
00046 char writeDatas[WRITE_DATAS_NUM];
00047 
00048 void air_countUp();
00049 void call_calcAirSpeed();
00050 void init();
00051 void FusokukeiInit();
00052 void MpuInit();
00053 void mpuProcessing();
00054 void SdInit();
00055 void DataReceiveFromSouda();
00056 void WriteDatas();
00057 float calcAttackAngle();
00058 float calcKXdeg(float x);
00059 
00060 void air_countUp(){
00061     air_kaitensu++;
00062 }
00063 
00064 void call_calcAirSpeed(){
00065     air.calcAirSpeed(air_kaitensu);
00066     air_kaitensu = 0;
00067 }
00068 
00069 void init(){
00070     twe.baud(115200);
00071     FusokukeiInit();
00072     MpuInit(); 
00073     SdInit();
00074     writeDatasTicker.attach(&WriteDatas,0.25);
00075     //soudaSerial.attach(&DataReceiveFromSouda, Serial::RxIrq);
00076 }
00077 
00078 void FusokukeiInit(){
00079     FusokukeiPin.rise(air_countUp);
00080     FusokukeiTicker.attach(&call_calcAirSpeed, AIR_LOOP_TIME);
00081 }
00082 
00083 void MpuInit(){
00084     i2c.frequency(400000);  // use fast (400 kHz) I2C
00085     t.start();
00086     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00087     if (whoami == 0x68) { // WHO_AM_I should always be 0x68
00088         wait(1);
00089         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00090         wait(1);
00091         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) {
00092             mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00093             mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00094             mpu6050.initMPU6050(); //pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00095             wait(2);
00096         } else {
00097         }
00098     } else {
00099         pc.printf("out\n\r"); // Loop forever if communication doesn't happen
00100     }   
00101 }
00102 
00103 void mpuProcessing(){
00104     if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00105             mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
00106             mpu6050.getAres();
00107             ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00108             ay = (float)accelCount[1]*aRes - accelBias[1];
00109             az = (float)accelCount[2]*aRes - accelBias[2];
00110             mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
00111             mpu6050.getGres();
00112             gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
00113             gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
00114             gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
00115             tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
00116             temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
00117         }
00118         Now = t.read_us();
00119         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00120         lastUpdate = Now;
00121         sum += deltat;
00122         sumCount++;
00123         if(lastUpdate - firstUpdate > 10000000.0f) {
00124             beta = 0.04;  // decrease filter gain after stabilized
00125             zeta = 0.015; // increasey bias drift gain after stabilized
00126         }
00127         mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
00128         delt_t = t.read_ms() - count;
00129         if (delt_t > 800) { // update LCD once per half-second independent of read rate
00130             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]);
00131             pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00132             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]);
00133             pitch *= 180.0f / PI;
00134             yaw   *= 180.0f / PI;
00135             roll  *= 180.0f / PI;
00136             myled= !myled;
00137             count = t.read_ms();
00138             sum = 0;
00139             sumCount = 0;
00140     }
00141 }
00142 
00143 void SdInit(){
00144     mkdir("/sd/mydir", 0777);
00145     fp = fopen("/sd/mydir/sdtest2.csv", "w");
00146     if(fp == NULL) {
00147         error("Could not open file for write\n");
00148     }
00149     fprintf(fp, "Hello fun SD Card World!\n\r");
00150     fclose(fp);
00151 }
00152 
00153 void DataReceiveFromSouda(){
00154     led2 = !led2;
00155     pc.printf("received\n\r");
00156     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
00157         soudaDatas[i] = soudaSerial.getc();
00158     }
00159 }
00160 
00161 void WriteDatas(){
00162     pc.printf("write\n\r");
00163     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
00164         pc.printf("%i   ",soudaDatas[i]);
00165         twe.printf("%i  ",soudaDatas[i]);
00166     }
00167     pc.printf("\n\r");
00168     twe.printf("\n\r");
00169     twe.printf("%f,%f,%f\n\r",pitch,roll,yaw);
00170    // twe.printf("%f,%f,%f\n\r",gx,gy,gz);   
00171 //    twe.printf("%f\n\r",airSpeed); 
00172     pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
00173     pc.printf("%f,%f,%f\n\r",gx,gy,gz);   
00174     pc.printf("%f\n\r",airSpeed);
00175     fp = fopen("/sd/mydir/sdtest.csv", "a");
00176     if(fp == NULL) {
00177         error("Could not open file for write\n");
00178     }
00179     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
00180         fprintf(fp,"%i   ",soudaDatas[i]);
00181     }
00182     fprintf(fp, "p:%f,r:%f,y:%f\n",pitch,roll,yaw);
00183     fprintf(fp, "gx:%f,gy:%f,gz:%f\n",gx,gy,gz);
00184     fprintf(fp, "as:%f\n",airSpeed);
00185     fclose(fp);
00186 }
00187 
00188 float calcKXdeg(float x){
00189     return -310.54*x+156.65;
00190 }
00191 
00192 float calcAttackAngle(){
00193     return pitch-calcKXdeg(kx_Z.read());
00194 }
00195 
00196 void kxRead(){
00197     gx = kx_X.read();
00198     gy = kx_Y.read();
00199     gz = kx_Z.read();
00200 }
00201 
00202 void RollAlarm(){ 
00203     if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
00204         RollAlarmL = 1;
00205     }
00206     else{
00207         RollAlarmL = 0;
00208     }
00209     
00210     if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
00211         RollAlarmR = 1;
00212     }
00213     else{
00214         RollAlarmR = 0;
00215     }
00216 }
00217 
00218 int main(){
00219     init();
00220     while(1){
00221         mpuProcessing();
00222         kxRead();
00223         RollAlarm();
00224         DataReceiveFromSouda();
00225     }
00226 }