Diff: main.cpp

Revision:

0:010a686dbe95

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 16 16:17:13 2021 +0000
@@ -0,0 +1,173 @@
+#include "mbed.h"
+#include "mpu9250_i2c.h"
+#include "BMP180.h"
+#include "millis.h"
+#include "IM920.h"
+#include "GPS.h"
+#include "math.h"
+#define mpu_SDA PB_7
+#define mpu_SCL PB_6
+I2C i2c(PB_7, PB_6);
+BMP180 bmp180(&i2c);
+I2C i2cBus(mpu_SDA, mpu_SCL);
+mpu9250 mpu(i2cBus, AD0_HIGH);
+DigitalIn enable(PA_8);
+PwmOut PWM1(PB_0);
+PwmOut PWM2(PB_1); 
+//SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
+IM920 im920(PA_2, PA_3, PF_0, PB_3);
+GPS gps(PA_9, PA_10);
+Serial  pc(USBTX, USBRX);
+bool flightpin();
+void getmpu(float &ax,float &ay,float &az,float &gx,float &gy,float &gz,float &mx,float &my,float &mz);
+void getbmp(int &pressure,float &temp,float &altitude,float &l);
+void imSend(char *send);
+void sendDatas(float latitude, float longtitude, float altitude, float time);
+void getGPS();
+//void getgps(float &longtitude,float &latitude);
+char sendData[256]; //送るデータのchar型配列(im920はchar型でしか送れない。)
+int main(){
+    int sequence=0;
+    int maxaltitude=0;
+    int pressure;
+    bool flightpinattached=false;
+    bool launched=false;
+    float ax,ay,az,gx,gy,gz,mx,my,mz;
+    float altitude,temp,l;
+    float longtitude,latitude;
+    
+    PWM1.period_us(20000);
+    PWM1.pulsewidth_us(500);
+    PWM2.period_us(20000); 
+    PWM2.pulsewidth_us(500);
+    
+    //sd.mount();
+    //FATFS fs;
+    //f_mount(&fs,"",0);
+    //FIL fp;
+    //f_open(&fp,"TEST.TXT",FA_CREATE_ALWAYS | FA_WRITE);
+    
+    while(launched==false){
+        getmpu(ax,ay,az,gx,gy,gz,mx,my,mz);
+        enable.mode(PullUp);
+        while(1) {
+         if(enable) {
+            flightpinattached=true;
+        }
+        
+         if(flightpinattached==true||(ax*ax+ay*ay+az*az)>=2.0*2.0){
+            launched=true;
+         }
+       }
+    }
+       
+    
+    while(sequence!=3){
+        getmpu(ax,ay,az,gx,gy,gz,mx,my,mz);
+        getbmp(pressure,temp,altitude,l);
+        //getgps(longtitude,latitude);
+        
+        if(maxaltitude<altitude){
+            maxaltitude=altitude;
+        }  
+        
+        switch(sequence){
+            case 0:if(launched==true){
+                millisStart();
+                sequence=1;
+                //f_printf(&fp,"%f,%f,%f,%f,%f",ax,ay,az,longtitude,latitude);
+                }
+                break;
+            case 1:if((millis()>15||maxaltitude-altitude>10)&&launched==true){
+                PWM1.pulsewidth_us(1200);
+                PWM2.pulsewidth_us(1200);
+                sequence=2;
+                //fprintf(fp,"%f,%f,%f",ax,ay,az);
+                //f_printf(&fp,"%f,%f,%f",ax,ay,az);
+                }
+                break;
+            case 2:if(millis()>60){
+                sequence=3;
+                //sd.unmount();
+                //f_close(&fp);
+                }
+                break;
+        }
+    }
+}
+void getmpu(float &ax,float &ay,float &az,float &gx,float &gy,float &gz,float &mx,float &my,float &mz){
+    float acc[3]={};
+    float gyro[3]={};
+    float mag[3]={};
+    
+    pc.baud(115200);
+    mpu.setAccLPF(NO_USE);
+    mpu.setAcc(_16G);
+    
+    mpu.getAcc(acc);
+    mpu.getGyro(gyro);
+    mpu.getMag(mag);
+    
+    ax=acc[0];
+    ay=acc[1];
+    az=acc[2];
+    gx=gyro[0];
+    gy=gyro[1];
+    gz=gyro[2];
+    mx=mag[0];
+    my=mag[1];
+    mz=mag[2];  
+}
+//void getbmp(double &altitude,double &pressure){
+  //  double temp,altitudebig;
+    //bmp180.startTemperature();
+    //bmp180.startPressure(BMP180::ULTRA_LOW_POWER);    
+    //double l = (1012.25 / (pressure/100) );    
+    //double i = temp + 273.15;
+    //altitudebig = pow(double(l), double((1 / 5.257) - 1)) * (i)*(1 / 0.0065);      
+    //altitude=altitudebig/10000;      
+//}
+void getbmp(int &pressure,float &temp,float &altitude,float &l){
+    
+    if (bmp180.init() != 0) {
+            printf("");
+        } else {
+            printf("");
+        }
+    bmp180.startTemperature();
+    wait_ms(5);    
+    if(bmp180.getTemperature(&temp) != 0) {
+        printf("");
+    }
+    bmp180.startPressure(BMP180::ULTRA_LOW_POWER);
+    wait_ms(10);   
+    
+    if(bmp180.getPressure(&pressure) != 0) {
+        printf("");
+    }
+    float t_press = float(pressure)/100;
+    l = (1012.25 / t_press );  
+    float i = temp + 273.15;
+    altitude = (pow(double(l), double(1 / 5.257)) - 1) * i / 0.0065;
+}
+//void getgps(float &longtitude,float &latitude){
+  //  gps.GetData();
+    //longtitude=gps.longtitude;
+    //latitude=gps.latitude;
+//}
+void imSend(char *send){//無線で送信する関数
+    im920.send(send,strlen(send)+1);
+    pc.printf(send);
+    pc.printf("\r\n");
+}
+void sendDatas(float latitude, float longtitude, float altitude, float time){//データを文字列に変換してimSendを呼び出して送信する関数
+        sprintf(sendData,"data1,%.3f,%.3f,%.3f,%.3f", latitude, longtitude, altitude, time);
+        imSend(sendData);
+}
+void getGPS(){//GPSの値を取得してsendDatesに値を入れる関数
+    //NVIC_SetPriority(UART2_IRQn,0); //割り込み優先順位(必要に応じて)
+    gps.GetData();
+    if(gps.readable == true){
+       sendDatas(gps.latitude, gps.longtitude, gps.altitude, gps.time);
+    }
+}
\ No newline at end of file