kato shunsuke / Mbed 2 deprecated IZU2022

code for IZU2022

Dependencies:   mbed library

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?

UserRevisionLine numberNew 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 }