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