kato shunsuke
/
IZU2022
code for IZU2022
main.cpp@0:010a686dbe95, 2021-12-16 (annotated)
- Committer:
- katoshunsuke
- Date:
- Thu Dec 16 16:17:13 2021 +0000
- Revision:
- 0:010a686dbe95
change for use;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
katoshunsuke | 0:010a686dbe95 | 1 | #include "mbed.h" |
katoshunsuke | 0:010a686dbe95 | 2 | #include "mpu9250_i2c.h" |
katoshunsuke | 0:010a686dbe95 | 3 | #include "BMP180.h" |
katoshunsuke | 0:010a686dbe95 | 4 | #include "millis.h" |
katoshunsuke | 0:010a686dbe95 | 5 | #include "IM920.h" |
katoshunsuke | 0:010a686dbe95 | 6 | #include "GPS.h" |
katoshunsuke | 0:010a686dbe95 | 7 | #include "math.h" |
katoshunsuke | 0:010a686dbe95 | 8 | #define mpu_SDA PB_7 |
katoshunsuke | 0:010a686dbe95 | 9 | #define mpu_SCL PB_6 |
katoshunsuke | 0:010a686dbe95 | 10 | I2C i2c(PB_7, PB_6); |
katoshunsuke | 0:010a686dbe95 | 11 | BMP180 bmp180(&i2c); |
katoshunsuke | 0:010a686dbe95 | 12 | I2C i2cBus(mpu_SDA, mpu_SCL); |
katoshunsuke | 0:010a686dbe95 | 13 | mpu9250 mpu(i2cBus, AD0_HIGH); |
katoshunsuke | 0:010a686dbe95 | 14 | DigitalIn enable(PA_8); |
katoshunsuke | 0:010a686dbe95 | 15 | PwmOut PWM1(PB_0); |
katoshunsuke | 0:010a686dbe95 | 16 | PwmOut PWM2(PB_1); |
katoshunsuke | 0:010a686dbe95 | 17 | //SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd"); |
katoshunsuke | 0:010a686dbe95 | 18 | IM920 im920(PA_2, PA_3, PF_0, PB_3); |
katoshunsuke | 0:010a686dbe95 | 19 | GPS gps(PA_9, PA_10); |
katoshunsuke | 0:010a686dbe95 | 20 | Serial pc(USBTX, USBRX); |
katoshunsuke | 0:010a686dbe95 | 21 | bool flightpin(); |
katoshunsuke | 0:010a686dbe95 | 22 | void getmpu(float &ax,float &ay,float &az,float &gx,float &gy,float &gz,float &mx,float &my,float &mz); |
katoshunsuke | 0:010a686dbe95 | 23 | void getbmp(int &pressure,float &temp,float &altitude,float &l); |
katoshunsuke | 0:010a686dbe95 | 24 | void imSend(char *send); |
katoshunsuke | 0:010a686dbe95 | 25 | void sendDatas(float latitude, float longtitude, float altitude, float time); |
katoshunsuke | 0:010a686dbe95 | 26 | void getGPS(); |
katoshunsuke | 0:010a686dbe95 | 27 | //void getgps(float &longtitude,float &latitude); |
katoshunsuke | 0:010a686dbe95 | 28 | char sendData[256]; //送るデータのchar型配列(im920はchar型でしか送れない。) |
katoshunsuke | 0:010a686dbe95 | 29 | int main(){ |
katoshunsuke | 0:010a686dbe95 | 30 | int sequence=0; |
katoshunsuke | 0:010a686dbe95 | 31 | int maxaltitude=0; |
katoshunsuke | 0:010a686dbe95 | 32 | int pressure; |
katoshunsuke | 0:010a686dbe95 | 33 | bool flightpinattached=false; |
katoshunsuke | 0:010a686dbe95 | 34 | bool launched=false; |
katoshunsuke | 0:010a686dbe95 | 35 | float ax,ay,az,gx,gy,gz,mx,my,mz; |
katoshunsuke | 0:010a686dbe95 | 36 | float altitude,temp,l; |
katoshunsuke | 0:010a686dbe95 | 37 | float longtitude,latitude; |
katoshunsuke | 0:010a686dbe95 | 38 | |
katoshunsuke | 0:010a686dbe95 | 39 | PWM1.period_us(20000); |
katoshunsuke | 0:010a686dbe95 | 40 | PWM1.pulsewidth_us(500); |
katoshunsuke | 0:010a686dbe95 | 41 | PWM2.period_us(20000); |
katoshunsuke | 0:010a686dbe95 | 42 | PWM2.pulsewidth_us(500); |
katoshunsuke | 0:010a686dbe95 | 43 | |
katoshunsuke | 0:010a686dbe95 | 44 | //sd.mount(); |
katoshunsuke | 0:010a686dbe95 | 45 | //FATFS fs; |
katoshunsuke | 0:010a686dbe95 | 46 | //f_mount(&fs,"",0); |
katoshunsuke | 0:010a686dbe95 | 47 | //FIL fp; |
katoshunsuke | 0:010a686dbe95 | 48 | //f_open(&fp,"TEST.TXT",FA_CREATE_ALWAYS | FA_WRITE); |
katoshunsuke | 0:010a686dbe95 | 49 | |
katoshunsuke | 0:010a686dbe95 | 50 | while(launched==false){ |
katoshunsuke | 0:010a686dbe95 | 51 | getmpu(ax,ay,az,gx,gy,gz,mx,my,mz); |
katoshunsuke | 0:010a686dbe95 | 52 | enable.mode(PullUp); |
katoshunsuke | 0:010a686dbe95 | 53 | while(1) { |
katoshunsuke | 0:010a686dbe95 | 54 | if(enable) { |
katoshunsuke | 0:010a686dbe95 | 55 | flightpinattached=true; |
katoshunsuke | 0:010a686dbe95 | 56 | } |
katoshunsuke | 0:010a686dbe95 | 57 | |
katoshunsuke | 0:010a686dbe95 | 58 | if(flightpinattached==true||(ax*ax+ay*ay+az*az)>=2.0*2.0){ |
katoshunsuke | 0:010a686dbe95 | 59 | launched=true; |
katoshunsuke | 0:010a686dbe95 | 60 | } |
katoshunsuke | 0:010a686dbe95 | 61 | } |
katoshunsuke | 0:010a686dbe95 | 62 | } |
katoshunsuke | 0:010a686dbe95 | 63 | |
katoshunsuke | 0:010a686dbe95 | 64 | |
katoshunsuke | 0:010a686dbe95 | 65 | while(sequence!=3){ |
katoshunsuke | 0:010a686dbe95 | 66 | getmpu(ax,ay,az,gx,gy,gz,mx,my,mz); |
katoshunsuke | 0:010a686dbe95 | 67 | getbmp(pressure,temp,altitude,l); |
katoshunsuke | 0:010a686dbe95 | 68 | //getgps(longtitude,latitude); |
katoshunsuke | 0:010a686dbe95 | 69 | |
katoshunsuke | 0:010a686dbe95 | 70 | if(maxaltitude<altitude){ |
katoshunsuke | 0:010a686dbe95 | 71 | maxaltitude=altitude; |
katoshunsuke | 0:010a686dbe95 | 72 | } |
katoshunsuke | 0:010a686dbe95 | 73 | |
katoshunsuke | 0:010a686dbe95 | 74 | switch(sequence){ |
katoshunsuke | 0:010a686dbe95 | 75 | case 0:if(launched==true){ |
katoshunsuke | 0:010a686dbe95 | 76 | millisStart(); |
katoshunsuke | 0:010a686dbe95 | 77 | sequence=1; |
katoshunsuke | 0:010a686dbe95 | 78 | //f_printf(&fp,"%f,%f,%f,%f,%f",ax,ay,az,longtitude,latitude); |
katoshunsuke | 0:010a686dbe95 | 79 | } |
katoshunsuke | 0:010a686dbe95 | 80 | break; |
katoshunsuke | 0:010a686dbe95 | 81 | case 1:if((millis()>15||maxaltitude-altitude>10)&&launched==true){ |
katoshunsuke | 0:010a686dbe95 | 82 | PWM1.pulsewidth_us(1200); |
katoshunsuke | 0:010a686dbe95 | 83 | PWM2.pulsewidth_us(1200); |
katoshunsuke | 0:010a686dbe95 | 84 | sequence=2; |
katoshunsuke | 0:010a686dbe95 | 85 | //fprintf(fp,"%f,%f,%f",ax,ay,az); |
katoshunsuke | 0:010a686dbe95 | 86 | //f_printf(&fp,"%f,%f,%f",ax,ay,az); |
katoshunsuke | 0:010a686dbe95 | 87 | } |
katoshunsuke | 0:010a686dbe95 | 88 | break; |
katoshunsuke | 0:010a686dbe95 | 89 | case 2:if(millis()>60){ |
katoshunsuke | 0:010a686dbe95 | 90 | sequence=3; |
katoshunsuke | 0:010a686dbe95 | 91 | //sd.unmount(); |
katoshunsuke | 0:010a686dbe95 | 92 | //f_close(&fp); |
katoshunsuke | 0:010a686dbe95 | 93 | } |
katoshunsuke | 0:010a686dbe95 | 94 | break; |
katoshunsuke | 0:010a686dbe95 | 95 | } |
katoshunsuke | 0:010a686dbe95 | 96 | } |
katoshunsuke | 0:010a686dbe95 | 97 | } |
katoshunsuke | 0:010a686dbe95 | 98 | void getmpu(float &ax,float &ay,float &az,float &gx,float &gy,float &gz,float &mx,float &my,float &mz){ |
katoshunsuke | 0:010a686dbe95 | 99 | float acc[3]={}; |
katoshunsuke | 0:010a686dbe95 | 100 | float gyro[3]={}; |
katoshunsuke | 0:010a686dbe95 | 101 | float mag[3]={}; |
katoshunsuke | 0:010a686dbe95 | 102 | |
katoshunsuke | 0:010a686dbe95 | 103 | pc.baud(115200); |
katoshunsuke | 0:010a686dbe95 | 104 | mpu.setAccLPF(NO_USE); |
katoshunsuke | 0:010a686dbe95 | 105 | mpu.setAcc(_16G); |
katoshunsuke | 0:010a686dbe95 | 106 | |
katoshunsuke | 0:010a686dbe95 | 107 | mpu.getAcc(acc); |
katoshunsuke | 0:010a686dbe95 | 108 | mpu.getGyro(gyro); |
katoshunsuke | 0:010a686dbe95 | 109 | mpu.getMag(mag); |
katoshunsuke | 0:010a686dbe95 | 110 | |
katoshunsuke | 0:010a686dbe95 | 111 | ax=acc[0]; |
katoshunsuke | 0:010a686dbe95 | 112 | ay=acc[1]; |
katoshunsuke | 0:010a686dbe95 | 113 | az=acc[2]; |
katoshunsuke | 0:010a686dbe95 | 114 | gx=gyro[0]; |
katoshunsuke | 0:010a686dbe95 | 115 | gy=gyro[1]; |
katoshunsuke | 0:010a686dbe95 | 116 | gz=gyro[2]; |
katoshunsuke | 0:010a686dbe95 | 117 | mx=mag[0]; |
katoshunsuke | 0:010a686dbe95 | 118 | my=mag[1]; |
katoshunsuke | 0:010a686dbe95 | 119 | mz=mag[2]; |
katoshunsuke | 0:010a686dbe95 | 120 | } |
katoshunsuke | 0:010a686dbe95 | 121 | //void getbmp(double &altitude,double &pressure){ |
katoshunsuke | 0:010a686dbe95 | 122 | // double temp,altitudebig; |
katoshunsuke | 0:010a686dbe95 | 123 | //bmp180.startTemperature(); |
katoshunsuke | 0:010a686dbe95 | 124 | //bmp180.startPressure(BMP180::ULTRA_LOW_POWER); |
katoshunsuke | 0:010a686dbe95 | 125 | //double l = (1012.25 / (pressure/100) ); |
katoshunsuke | 0:010a686dbe95 | 126 | //double i = temp + 273.15; |
katoshunsuke | 0:010a686dbe95 | 127 | //altitudebig = pow(double(l), double((1 / 5.257) - 1)) * (i)*(1 / 0.0065); |
katoshunsuke | 0:010a686dbe95 | 128 | //altitude=altitudebig/10000; |
katoshunsuke | 0:010a686dbe95 | 129 | //} |
katoshunsuke | 0:010a686dbe95 | 130 | void getbmp(int &pressure,float &temp,float &altitude,float &l){ |
katoshunsuke | 0:010a686dbe95 | 131 | |
katoshunsuke | 0:010a686dbe95 | 132 | if (bmp180.init() != 0) { |
katoshunsuke | 0:010a686dbe95 | 133 | printf(""); |
katoshunsuke | 0:010a686dbe95 | 134 | } else { |
katoshunsuke | 0:010a686dbe95 | 135 | printf(""); |
katoshunsuke | 0:010a686dbe95 | 136 | } |
katoshunsuke | 0:010a686dbe95 | 137 | bmp180.startTemperature(); |
katoshunsuke | 0:010a686dbe95 | 138 | wait_ms(5); |
katoshunsuke | 0:010a686dbe95 | 139 | if(bmp180.getTemperature(&temp) != 0) { |
katoshunsuke | 0:010a686dbe95 | 140 | printf(""); |
katoshunsuke | 0:010a686dbe95 | 141 | } |
katoshunsuke | 0:010a686dbe95 | 142 | bmp180.startPressure(BMP180::ULTRA_LOW_POWER); |
katoshunsuke | 0:010a686dbe95 | 143 | wait_ms(10); |
katoshunsuke | 0:010a686dbe95 | 144 | |
katoshunsuke | 0:010a686dbe95 | 145 | if(bmp180.getPressure(&pressure) != 0) { |
katoshunsuke | 0:010a686dbe95 | 146 | printf(""); |
katoshunsuke | 0:010a686dbe95 | 147 | } |
katoshunsuke | 0:010a686dbe95 | 148 | float t_press = float(pressure)/100; |
katoshunsuke | 0:010a686dbe95 | 149 | l = (1012.25 / t_press ); |
katoshunsuke | 0:010a686dbe95 | 150 | float i = temp + 273.15; |
katoshunsuke | 0:010a686dbe95 | 151 | altitude = (pow(double(l), double(1 / 5.257)) - 1) * i / 0.0065; |
katoshunsuke | 0:010a686dbe95 | 152 | } |
katoshunsuke | 0:010a686dbe95 | 153 | //void getgps(float &longtitude,float &latitude){ |
katoshunsuke | 0:010a686dbe95 | 154 | // gps.GetData(); |
katoshunsuke | 0:010a686dbe95 | 155 | //longtitude=gps.longtitude; |
katoshunsuke | 0:010a686dbe95 | 156 | //latitude=gps.latitude; |
katoshunsuke | 0:010a686dbe95 | 157 | //} |
katoshunsuke | 0:010a686dbe95 | 158 | void imSend(char *send){//無線で送信する関数 |
katoshunsuke | 0:010a686dbe95 | 159 | im920.send(send,strlen(send)+1); |
katoshunsuke | 0:010a686dbe95 | 160 | pc.printf(send); |
katoshunsuke | 0:010a686dbe95 | 161 | pc.printf("\r\n"); |
katoshunsuke | 0:010a686dbe95 | 162 | } |
katoshunsuke | 0:010a686dbe95 | 163 | void sendDatas(float latitude, float longtitude, float altitude, float time){//データを文字列に変換してimSendを呼び出して送信する関数 |
katoshunsuke | 0:010a686dbe95 | 164 | sprintf(sendData,"data1,%.3f,%.3f,%.3f,%.3f", latitude, longtitude, altitude, time); |
katoshunsuke | 0:010a686dbe95 | 165 | imSend(sendData); |
katoshunsuke | 0:010a686dbe95 | 166 | } |
katoshunsuke | 0:010a686dbe95 | 167 | void getGPS(){//GPSの値を取得してsendDatesに値を入れる関数 |
katoshunsuke | 0:010a686dbe95 | 168 | //NVIC_SetPriority(UART2_IRQn,0); //割り込み優先順位(必要に応じて) |
katoshunsuke | 0:010a686dbe95 | 169 | gps.GetData(); |
katoshunsuke | 0:010a686dbe95 | 170 | if(gps.readable == true){ |
katoshunsuke | 0:010a686dbe95 | 171 | sendDatas(gps.latitude, gps.longtitude, gps.altitude, gps.time); |
katoshunsuke | 0:010a686dbe95 | 172 | } |
katoshunsuke | 0:010a686dbe95 | 173 | } |