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 250
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         }
00187     }
00188     fprintf(fp,"\n\r");
00189     fclose(fp);
00190 }
00191 
00192 void WriteDatas(){
00193     int i;
00194     for(i = 0; i < SOUDA_DATAS_NUM; i++){
00195         //writeDatas[write_datas_index][i] = 0.0;    
00196         writeDatas[write_datas_index][i] = (float)soudaDatas[i];
00197     }
00198     writeDatas[write_datas_index][i++] = calcKXdeg(kx_X.read());
00199     writeDatas[write_datas_index][i++] = calcKXdeg(kx_Y.read());
00200     writeDatas[write_datas_index][i++] = calcKXdeg(kx_Z.read());
00201     writeDatas[write_datas_index][i++] = pitch;
00202     writeDatas[write_datas_index][i++] = roll;
00203     writeDatas[write_datas_index][i++] = yaw;
00204     writeDatas[write_datas_index][i++] = airSpeed;
00205     //writeDatas[write_datas_index][i++] = writeTimer.read();
00206     //for(i = 0; i < WRITE_DATAS_NUM; i++){
00207 //        pc.printf("%f   ", writeDatas[write_datas_index][i]);
00208 //        twe.printf("%f,", writeDatas[write_datas_index][i]);
00209 //    }
00210 //    pc.printf("\n\r");
00211 //    twe.printf("\n\r");
00212     if(write_datas_index == SD_WRITE_NUM-1){
00213         SDprintf();
00214         write_datas_index=0;
00215     }
00216     else{
00217         write_datas_index++;
00218     } 
00219     for(int i = 0; i < SOUDA_DATAS_NUM; i++){
00220         pc.printf("%i   ",soudaDatas[i]);
00221         twe.printf("%i,",soudaDatas[i]);
00222     }
00223     //pc.printf("\n\r");
00224     twe.printf("%f,%f,%f,",pitch,roll,yaw);
00225     twe.printf("%f,%f,%f,",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
00226     twe.printf("%f,\r\n",airSpeed); 
00227     pc.printf("%f,%f,%f\n\r",pitch,roll,yaw);
00228     //pc.printf("%f,%f,%f\n\r",calcKXdeg(kx_X.read()),calcKXdeg(KX_Y),calcKXdeg(KX_Z));   
00229     pc.printf("%f\n\r",airSpeed);
00230     //SDprintf();
00231 }
00232 
00233 void WriteDatasF(){
00234     pc.printf("airSpeed:%f\n\r",airSpeed);
00235 }
00236 
00237 float calcKXdeg(float x){
00238     return -310.54*x+156.65;
00239 }
00240 
00241 float calcAttackAngle(){
00242     return pitch-calcKXdeg(kx_Z.read());
00243 }
00244 
00245 void RollAlarm(){ 
00246     if((roll < 0) && (roll > ROLL_L_MAX_DEG-180)){
00247         RollAlarmL = 1;
00248     }
00249     else{
00250         RollAlarmL = 0;
00251     }
00252     
00253     if((roll > 0) && (roll < 180-ROLL_R_MAX_DEG)){
00254         RollAlarmR = 1;
00255     }
00256     else{
00257         RollAlarmR = 0;
00258     }
00259 }
00260 
00261 void WriteServo(){
00262     //kisokuServo.pulsewidth(calcPulse(airSpeed*10));
00263     kisokuServo.pulsewidth(calcPulse(9*airSpeed));
00264     if(pitch<0){
00265         geikakuServo.pulsewidth(calcPulse(0));
00266     }
00267     else{
00268         geikakuServo.pulsewidth(calcPulse(abs(pitch*90/13.0)));
00269     }
00270     //pc.printf("a:%f",airSpeed);
00271     //pc.printf("p:%f\r\n",pitch);
00272     //kisokuServo.pulsewidth(calcPulse(0));
00273     //geikakuServo.pulsewidth(calcPulse(0));
00274 }
00275 
00276 int main(){
00277     init();
00278     while(1){
00279         pc.printf("test\n\r");
00280         mpuProcessing();
00281         RollAlarm();
00282         DataReceiveFromSouda();
00283         WriteDatas();
00284         WriteServo();
00285     }
00286 }