きりたんぽの開放プログラム
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of SDFileSystem_HelloWorld by
main.cpp@2:4cdbd4b3722d, 2018-07-31 (annotated)
- Committer:
- mizukamikatsuki
- Date:
- Tue Jul 31 11:43:36 2018 +0000
- Revision:
- 2:4cdbd4b3722d
- Parent:
- 0:bdbd3d6fc5d5
?????????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mbed_official | 0:bdbd3d6fc5d5 | 1 | #include "mbed.h" |
mizukamikatsuki | 2:4cdbd4b3722d | 2 | #include "math.h" |
mizukamikatsuki | 2:4cdbd4b3722d | 3 | #include "MPU6050.h" |
mizukamikatsuki | 2:4cdbd4b3722d | 4 | #include "BMP180.h" |
mbed_official | 0:bdbd3d6fc5d5 | 5 | #include "SDFileSystem.h" |
mizukamikatsuki | 2:4cdbd4b3722d | 6 | |
mizukamikatsuki | 2:4cdbd4b3722d | 7 | #define p0 1013.25f |
mizukamikatsuki | 2:4cdbd4b3722d | 8 | #define JUDGE_ACC 1.0 |
mizukamikatsuki | 2:4cdbd4b3722d | 9 | #define JUDGE_ALT 1.0 |
mizukamikatsuki | 2:4cdbd4b3722d | 10 | #define RATE_GET 20 |
mizukamikatsuki | 2:4cdbd4b3722d | 11 | #define RATE_OPEN 20 |
mizukamikatsuki | 2:4cdbd4b3722d | 12 | #define NUM_DATA 20 |
mizukamikatsuki | 2:4cdbd4b3722d | 13 | #define NUMBER 5 |
mizukamikatsuki | 2:4cdbd4b3722d | 14 | #define TIME_OPEN 14 |
mizukamikatsuki | 2:4cdbd4b3722d | 15 | #define TIME_DROP 104 |
mizukamikatsuki | 2:4cdbd4b3722d | 16 | |
mizukamikatsuki | 2:4cdbd4b3722d | 17 | enum PHASE{SETUP,LAUNCH,RISE,DROP,LAND} Phase; |
mizukamikatsuki | 2:4cdbd4b3722d | 18 | |
mizukamikatsuki | 2:4cdbd4b3722d | 19 | Serial twe(p13,p14); |
mizukamikatsuki | 2:4cdbd4b3722d | 20 | Serial pc(USBTX,USBRX); |
mizukamikatsuki | 2:4cdbd4b3722d | 21 | MPU6050 mpu(p9,p10); |
mizukamikatsuki | 2:4cdbd4b3722d | 22 | BMP180 bmp(p9, p10); |
mizukamikatsuki | 2:4cdbd4b3722d | 23 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board |
mizukamikatsuki | 2:4cdbd4b3722d | 24 | Serial gps(p28,p27); |
mizukamikatsuki | 2:4cdbd4b3722d | 25 | PwmOut servo1(p21); |
mizukamikatsuki | 2:4cdbd4b3722d | 26 | PwmOut servo2(p25); |
mizukamikatsuki | 2:4cdbd4b3722d | 27 | DigitalOut fte(p5); |
mizukamikatsuki | 2:4cdbd4b3722d | 28 | DigitalOut buzz(p6); |
mizukamikatsuki | 2:4cdbd4b3722d | 29 | DigitalOut led1(LED1); |
mizukamikatsuki | 2:4cdbd4b3722d | 30 | DigitalOut led2(LED2); |
mizukamikatsuki | 2:4cdbd4b3722d | 31 | DigitalOut led3(LED3); |
mizukamikatsuki | 2:4cdbd4b3722d | 32 | DigitalOut led4(LED4); |
mizukamikatsuki | 2:4cdbd4b3722d | 33 | |
mizukamikatsuki | 2:4cdbd4b3722d | 34 | |
mizukamikatsuki | 2:4cdbd4b3722d | 35 | |
mizukamikatsuki | 2:4cdbd4b3722d | 36 | Timer timer_open; |
mizukamikatsuki | 2:4cdbd4b3722d | 37 | Timer timer_log; |
mizukamikatsuki | 2:4cdbd4b3722d | 38 | Ticker tic_open; |
mizukamikatsuki | 2:4cdbd4b3722d | 39 | Ticker tic_log; |
mizukamikatsuki | 2:4cdbd4b3722d | 40 | Ticker tic_heat; |
mizukamikatsuki | 2:4cdbd4b3722d | 41 | Ticker tic_buzz; |
mizukamikatsuki | 2:4cdbd4b3722d | 42 | |
mizukamikatsuki | 2:4cdbd4b3722d | 43 | |
mizukamikatsuki | 2:4cdbd4b3722d | 44 | FILE *bp; |
mizukamikatsuki | 2:4cdbd4b3722d | 45 | FILE *lfp; |
mizukamikatsuki | 2:4cdbd4b3722d | 46 | LocalFileSystem local("local"); |
mizukamikatsuki | 2:4cdbd4b3722d | 47 | |
mizukamikatsuki | 2:4cdbd4b3722d | 48 | void _open(); |
mizukamikatsuki | 2:4cdbd4b3722d | 49 | void _getData(); |
mizukamikatsuki | 2:4cdbd4b3722d | 50 | int _input(char cha); |
mizukamikatsuki | 2:4cdbd4b3722d | 51 | void _heater(); |
mizukamikatsuki | 2:4cdbd4b3722d | 52 | void _buzzer(); |
mizukamikatsuki | 2:4cdbd4b3722d | 53 | char comh; |
mizukamikatsuki | 2:4cdbd4b3722d | 54 | char comb; |
mizukamikatsuki | 2:4cdbd4b3722d | 55 | float _median(float data[], int num); |
mizukamikatsuki | 2:4cdbd4b3722d | 56 | float getAlt(float press, float temp); |
mizukamikatsuki | 2:4cdbd4b3722d | 57 | float _DMS2DEG(float raw_data); |
mizukamikatsuki | 2:4cdbd4b3722d | 58 | |
mizukamikatsuki | 2:4cdbd4b3722d | 59 | float Alt_gnd; |
mizukamikatsuki | 2:4cdbd4b3722d | 60 | float med[NUMBER]; |
mizukamikatsuki | 2:4cdbd4b3722d | 61 | float alt[NUMBER]; |
mizukamikatsuki | 2:4cdbd4b3722d | 62 | float med_alt; |
mizukamikatsuki | 2:4cdbd4b3722d | 63 | float max_alt=-1000; |
mizukamikatsuki | 2:4cdbd4b3722d | 64 | float mae_alt=5000; |
mizukamikatsuki | 2:4cdbd4b3722d | 65 | float med_g; |
mizukamikatsuki | 2:4cdbd4b3722d | 66 | float pressure,temperature,altitude; |
mizukamikatsuki | 2:4cdbd4b3722d | 67 | float Time; |
mizukamikatsuki | 2:4cdbd4b3722d | 68 | float Data[2][NUM_DATA][7]={0}; |
mizukamikatsuki | 2:4cdbd4b3722d | 69 | char gps_data[256]; |
mizukamikatsuki | 2:4cdbd4b3722d | 70 | int cnt_gps; |
mizukamikatsuki | 2:4cdbd4b3722d | 71 | int Cnt; |
mizukamikatsuki | 2:4cdbd4b3722d | 72 | int cnt_alt=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 73 | int cnt_d=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 74 | int Cnt_Data; |
mizukamikatsuki | 2:4cdbd4b3722d | 75 | int Cnt_GPS=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 76 | bool row; |
mizukamikatsuki | 2:4cdbd4b3722d | 77 | |
mizukamikatsuki | 2:4cdbd4b3722d | 78 | int main(){ |
mizukamikatsuki | 2:4cdbd4b3722d | 79 | twe.baud(115200); |
mizukamikatsuki | 2:4cdbd4b3722d | 80 | mpu.setAcceleroRange(3); |
mizukamikatsuki | 2:4cdbd4b3722d | 81 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
mizukamikatsuki | 2:4cdbd4b3722d | 82 | mkdir("/sd/mydir",0777); |
mizukamikatsuki | 2:4cdbd4b3722d | 83 | bp=fopen("/sd/mydir/data.bin","ab"); |
mizukamikatsuki | 2:4cdbd4b3722d | 84 | fclose(bp); |
mizukamikatsuki | 2:4cdbd4b3722d | 85 | lfp=fopen("/local/data.txt","a"); |
mizukamikatsuki | 2:4cdbd4b3722d | 86 | fclose(lfp); |
mizukamikatsuki | 2:4cdbd4b3722d | 87 | servo1.period_ms(20); |
mizukamikatsuki | 2:4cdbd4b3722d | 88 | servo2.period_ms(20); |
mizukamikatsuki | 2:4cdbd4b3722d | 89 | led1=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 90 | led2=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 91 | led3=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 92 | twe.printf("HELLO!\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 93 | led1=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 94 | led2=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 95 | led3=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 96 | led4=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 97 | fte=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 98 | buzz=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 99 | pc.printf("hello\r\n"); |
mbed_official | 0:bdbd3d6fc5d5 | 100 | |
mizukamikatsuki | 2:4cdbd4b3722d | 101 | while(1){ |
mizukamikatsuki | 2:4cdbd4b3722d | 102 | char cha = twe.getc(); |
mizukamikatsuki | 2:4cdbd4b3722d | 103 | if(_input(cha)==-1){ |
mizukamikatsuki | 2:4cdbd4b3722d | 104 | timer_log.start(); |
mizukamikatsuki | 2:4cdbd4b3722d | 105 | tic_log.attach(&_getData, 1.0/RATE_GET); |
mizukamikatsuki | 2:4cdbd4b3722d | 106 | break; |
mizukamikatsuki | 2:4cdbd4b3722d | 107 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 108 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 109 | tic_heat.attach(&_heater,0.1); |
mizukamikatsuki | 2:4cdbd4b3722d | 110 | led1=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 111 | led2=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 112 | led3=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 113 | led4=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 114 | Phase = SETUP; |
mizukamikatsuki | 2:4cdbd4b3722d | 115 | tic_open.attach(&_open, 1.0/RATE_OPEN); |
mizukamikatsuki | 2:4cdbd4b3722d | 116 | |
mizukamikatsuki | 2:4cdbd4b3722d | 117 | while(1){ |
mizukamikatsuki | 2:4cdbd4b3722d | 118 | |
mizukamikatsuki | 2:4cdbd4b3722d | 119 | |
mizukamikatsuki | 2:4cdbd4b3722d | 120 | if(gps.readable()){ |
mizukamikatsuki | 2:4cdbd4b3722d | 121 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
mizukamikatsuki | 2:4cdbd4b3722d | 122 | cnt_gps = 0; |
mizukamikatsuki | 2:4cdbd4b3722d | 123 | memset(gps_data,'\0',256); |
mizukamikatsuki | 2:4cdbd4b3722d | 124 | }else if(gps_data[cnt_gps] == '\r'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 125 | float world_time, lon_east, lat_north; |
mizukamikatsuki | 2:4cdbd4b3722d | 126 | int rlock, sat_num; |
mizukamikatsuki | 2:4cdbd4b3722d | 127 | char lat,lon; |
mizukamikatsuki | 2:4cdbd4b3722d | 128 | if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ |
mizukamikatsuki | 2:4cdbd4b3722d | 129 | if(rlock==1){ |
mizukamikatsuki | 2:4cdbd4b3722d | 130 | lat_north = _DMS2DEG(lat_north); |
mizukamikatsuki | 2:4cdbd4b3722d | 131 | lon_east = _DMS2DEG(lon_east); |
mizukamikatsuki | 2:4cdbd4b3722d | 132 | twe.printf("%s\r\n",gps_data); |
mizukamikatsuki | 2:4cdbd4b3722d | 133 | twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); |
mizukamikatsuki | 2:4cdbd4b3722d | 134 | }else{ |
mizukamikatsuki | 2:4cdbd4b3722d | 135 | twe.printf("%s\r\n",gps_data); |
mizukamikatsuki | 2:4cdbd4b3722d | 136 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 137 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 138 | }else{ |
mizukamikatsuki | 2:4cdbd4b3722d | 139 | cnt_gps++; |
mizukamikatsuki | 2:4cdbd4b3722d | 140 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 141 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 142 | |
mizukamikatsuki | 2:4cdbd4b3722d | 143 | |
mizukamikatsuki | 2:4cdbd4b3722d | 144 | if(timer_log.read()>=30.0*60.0) timer_log.reset(); |
mbed_official | 0:bdbd3d6fc5d5 | 145 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 146 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 147 | |
mizukamikatsuki | 2:4cdbd4b3722d | 148 | |
mizukamikatsuki | 2:4cdbd4b3722d | 149 | |
mizukamikatsuki | 2:4cdbd4b3722d | 150 | void _open(){ |
mizukamikatsuki | 2:4cdbd4b3722d | 151 | switch(Phase){ |
mizukamikatsuki | 2:4cdbd4b3722d | 152 | case SETUP: |
mizukamikatsuki | 2:4cdbd4b3722d | 153 | for(Cnt=0;Cnt<10;Cnt++){ |
mizukamikatsuki | 2:4cdbd4b3722d | 154 | bmp.ReadData(&temperature,&pressure); |
mizukamikatsuki | 2:4cdbd4b3722d | 155 | altitude = getAlt(pressure,temperature); |
mizukamikatsuki | 2:4cdbd4b3722d | 156 | alt[Cnt]=altitude; |
mizukamikatsuki | 2:4cdbd4b3722d | 157 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 158 | Alt_gnd = _median(alt,10); |
mizukamikatsuki | 2:4cdbd4b3722d | 159 | lfp=fopen("/local/data.txt", "a"); |
mizukamikatsuki | 2:4cdbd4b3722d | 160 | fprintf(lfp,"地上高度:%f\r\n",Alt_gnd); |
mizukamikatsuki | 2:4cdbd4b3722d | 161 | fclose(lfp); |
mizukamikatsuki | 2:4cdbd4b3722d | 162 | Phase = LAUNCH; |
mizukamikatsuki | 2:4cdbd4b3722d | 163 | twe.printf("Phase=LAUNCH\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 164 | pc.printf("Phase=LAUNCH\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 165 | break; |
mizukamikatsuki | 2:4cdbd4b3722d | 166 | |
mizukamikatsuki | 2:4cdbd4b3722d | 167 | case LAUNCH: |
mizukamikatsuki | 2:4cdbd4b3722d | 168 | |
mizukamikatsuki | 2:4cdbd4b3722d | 169 | float a[3],g; |
mizukamikatsuki | 2:4cdbd4b3722d | 170 | int i=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 171 | for(i=0;i<NUMBER;i++){ |
mizukamikatsuki | 2:4cdbd4b3722d | 172 | mpu.getAccelero(a); |
mizukamikatsuki | 2:4cdbd4b3722d | 173 | g=sqrt(pow(a[0],2)+(a[1],2)+(a[2],2))/9.81; |
mizukamikatsuki | 2:4cdbd4b3722d | 174 | med[i]=g; |
mizukamikatsuki | 2:4cdbd4b3722d | 175 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 176 | med_g=_median(med,NUMBER); |
mizukamikatsuki | 2:4cdbd4b3722d | 177 | twe.printf("%f\n\r",med_g); |
mizukamikatsuki | 2:4cdbd4b3722d | 178 | if(med_g>JUDGE_ACC){ |
mizukamikatsuki | 2:4cdbd4b3722d | 179 | tic_heat.detach(); |
mizukamikatsuki | 2:4cdbd4b3722d | 180 | timer_open.start(); |
mizukamikatsuki | 2:4cdbd4b3722d | 181 | pc.printf("発射\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 182 | twe.printf("発射\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 183 | led1=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 184 | led4=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 185 | Phase=RISE; |
mizukamikatsuki | 2:4cdbd4b3722d | 186 | twe.printf("PHASE=RISE\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 187 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 188 | led1=!led1; |
mizukamikatsuki | 2:4cdbd4b3722d | 189 | break; |
mizukamikatsuki | 2:4cdbd4b3722d | 190 | |
mizukamikatsuki | 2:4cdbd4b3722d | 191 | case RISE: |
mizukamikatsuki | 2:4cdbd4b3722d | 192 | int j=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 193 | for(j=0;j<NUMBER;j++){ |
mizukamikatsuki | 2:4cdbd4b3722d | 194 | bmp.ReadData(&temperature,&pressure); |
mizukamikatsuki | 2:4cdbd4b3722d | 195 | altitude = getAlt(pressure,temperature); |
mizukamikatsuki | 2:4cdbd4b3722d | 196 | alt[j]=altitude; |
mizukamikatsuki | 2:4cdbd4b3722d | 197 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 198 | med_alt=_median(alt,NUMBER); |
mizukamikatsuki | 2:4cdbd4b3722d | 199 | twe.printf("%f, %f\r\n",max_alt,med_alt); |
mizukamikatsuki | 2:4cdbd4b3722d | 200 | if(med_alt>max_alt){ |
mizukamikatsuki | 2:4cdbd4b3722d | 201 | max_alt=med_alt; |
mizukamikatsuki | 2:4cdbd4b3722d | 202 | cnt_alt=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 203 | }else if(max_alt-med_alt>JUDGE_ALT){ |
mizukamikatsuki | 2:4cdbd4b3722d | 204 | cnt_alt++; |
mizukamikatsuki | 2:4cdbd4b3722d | 205 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 206 | twe.printf("%d\n\r",cnt_alt); |
mizukamikatsuki | 2:4cdbd4b3722d | 207 | if(cnt_alt>=3 || timer_open.read()>=TIME_OPEN){ |
mizukamikatsuki | 2:4cdbd4b3722d | 208 | pc.printf("頂点\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 209 | twe.printf("頂点\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 210 | lfp = fopen("/local/data.txt", "a"); |
mizukamikatsuki | 2:4cdbd4b3722d | 211 | fprintf(lfp,"最高高度:%f\r\n",max_alt); |
mizukamikatsuki | 2:4cdbd4b3722d | 212 | fclose(lfp); |
mizukamikatsuki | 2:4cdbd4b3722d | 213 | servo1.pulsewidth(0.0015); |
mizukamikatsuki | 2:4cdbd4b3722d | 214 | servo2.pulsewidth(0.0006); |
mizukamikatsuki | 2:4cdbd4b3722d | 215 | led2=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 216 | Phase=DROP; |
mizukamikatsuki | 2:4cdbd4b3722d | 217 | twe.printf("PHASE=DROP"); |
mizukamikatsuki | 2:4cdbd4b3722d | 218 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 219 | led2=!led2; |
mizukamikatsuki | 2:4cdbd4b3722d | 220 | break; |
mizukamikatsuki | 2:4cdbd4b3722d | 221 | |
mizukamikatsuki | 2:4cdbd4b3722d | 222 | case DROP: |
mizukamikatsuki | 2:4cdbd4b3722d | 223 | int k=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 224 | for(k=0;k<NUMBER;k++){ |
mizukamikatsuki | 2:4cdbd4b3722d | 225 | bmp.ReadData(&temperature,&pressure); |
mizukamikatsuki | 2:4cdbd4b3722d | 226 | altitude = getAlt(pressure,temperature); |
mizukamikatsuki | 2:4cdbd4b3722d | 227 | alt[k]=altitude; |
mizukamikatsuki | 2:4cdbd4b3722d | 228 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 229 | med_alt=_median(alt,NUMBER); |
mizukamikatsuki | 2:4cdbd4b3722d | 230 | mae_alt=med_alt; |
mizukamikatsuki | 2:4cdbd4b3722d | 231 | if(mae_alt-med_alt<=1 && mae_alt-med_alt>=-1){ |
mizukamikatsuki | 2:4cdbd4b3722d | 232 | cnt_d++; |
mizukamikatsuki | 2:4cdbd4b3722d | 233 | }else{ |
mizukamikatsuki | 2:4cdbd4b3722d | 234 | cnt_d=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 235 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 236 | twe.printf("%d\n\r",cnt_d); |
mizukamikatsuki | 2:4cdbd4b3722d | 237 | if(cnt_d==3 || timer_open.read()>TIME_DROP){ |
mizukamikatsuki | 2:4cdbd4b3722d | 238 | pc.printf("着水\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 239 | twe.printf("着水\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 240 | timer_open.stop(); |
mizukamikatsuki | 2:4cdbd4b3722d | 241 | tic_buzz.attach(&_buzzer,0.1); |
mizukamikatsuki | 2:4cdbd4b3722d | 242 | Phase=LAND; |
mizukamikatsuki | 2:4cdbd4b3722d | 243 | pc.printf("Judge DROP\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 244 | twe.printf("FINISH\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 245 | led3=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 246 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 247 | led3=!led3; |
mizukamikatsuki | 2:4cdbd4b3722d | 248 | break; |
mizukamikatsuki | 2:4cdbd4b3722d | 249 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 250 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 251 | |
mizukamikatsuki | 2:4cdbd4b3722d | 252 | void _getData(){ |
mizukamikatsuki | 2:4cdbd4b3722d | 253 | Data[row][Cnt_Data][0] = timer_log.read(); |
mizukamikatsuki | 2:4cdbd4b3722d | 254 | mpu.getAccelero(&Data[row][Cnt_Data][1]); |
mizukamikatsuki | 2:4cdbd4b3722d | 255 | bmp.ReadData(&Data[row][Cnt_Data][4],&Data[row][Cnt_Data][5]); |
mizukamikatsuki | 2:4cdbd4b3722d | 256 | Data[row][Cnt_Data][6] = (pow((p0/Data[row][Cnt_Data][5]), (1.0f/5.257f))-1.0f)*(Data[row][Cnt_Data][4]+273.15f)/0.0065f; |
mizukamikatsuki | 2:4cdbd4b3722d | 257 | Cnt_Data++; |
mizukamikatsuki | 2:4cdbd4b3722d | 258 | /*Log*/ |
mizukamikatsuki | 2:4cdbd4b3722d | 259 | if(Cnt_Data==NUM_DATA){ |
mizukamikatsuki | 2:4cdbd4b3722d | 260 | Cnt_Data = 0; |
mizukamikatsuki | 2:4cdbd4b3722d | 261 | row =! row; |
mizukamikatsuki | 2:4cdbd4b3722d | 262 | bp = fopen("/sd/mydir/data.bin","ab"); |
mizukamikatsuki | 2:4cdbd4b3722d | 263 | fwrite(&Data[!row][0][0],sizeof(float),7*NUM_DATA,bp); |
mizukamikatsuki | 2:4cdbd4b3722d | 264 | fclose(bp); |
mizukamikatsuki | 2:4cdbd4b3722d | 265 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 266 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 267 | |
mizukamikatsuki | 2:4cdbd4b3722d | 268 | float _DMS2DEG(float raw_data){ |
mizukamikatsuki | 2:4cdbd4b3722d | 269 | int d=(int)(raw_data/100); |
mizukamikatsuki | 2:4cdbd4b3722d | 270 | float m=(raw_data-(float)d*100); |
mizukamikatsuki | 2:4cdbd4b3722d | 271 | return (float)d+m/60; |
mizukamikatsuki | 2:4cdbd4b3722d | 272 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 273 | |
mizukamikatsuki | 2:4cdbd4b3722d | 274 | float getAlt(float press, float temp){ |
mizukamikatsuki | 2:4cdbd4b3722d | 275 | return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; |
mizukamikatsuki | 2:4cdbd4b3722d | 276 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 277 | |
mizukamikatsuki | 2:4cdbd4b3722d | 278 | float _median(float data[], int num){ |
mizukamikatsuki | 2:4cdbd4b3722d | 279 | float *data_cpy, ans; |
mizukamikatsuki | 2:4cdbd4b3722d | 280 | data_cpy = new float[num]; |
mizukamikatsuki | 2:4cdbd4b3722d | 281 | memcpy(data_cpy,data,sizeof(float)*num); |
mbed_official | 0:bdbd3d6fc5d5 | 282 | |
mizukamikatsuki | 2:4cdbd4b3722d | 283 | for(int i=0; i<num; i++){ |
mizukamikatsuki | 2:4cdbd4b3722d | 284 | for(int j=0; j<num-i-1; j++){ |
mizukamikatsuki | 2:4cdbd4b3722d | 285 | if(data_cpy[j]>data_cpy[j+1]){ |
mizukamikatsuki | 2:4cdbd4b3722d | 286 | float buff = data_cpy[j+1]; |
mizukamikatsuki | 2:4cdbd4b3722d | 287 | data_cpy[j+1] = data_cpy[j]; |
mizukamikatsuki | 2:4cdbd4b3722d | 288 | data_cpy[j] = buff; |
mizukamikatsuki | 2:4cdbd4b3722d | 289 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 290 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 291 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 292 | if(num%2!=0) ans = data_cpy[num/2]; |
mizukamikatsuki | 2:4cdbd4b3722d | 293 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
mizukamikatsuki | 2:4cdbd4b3722d | 294 | delete[] data_cpy; |
mizukamikatsuki | 2:4cdbd4b3722d | 295 | return ans; |
mizukamikatsuki | 2:4cdbd4b3722d | 296 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 297 | |
mizukamikatsuki | 2:4cdbd4b3722d | 298 | int _input(char cha){ |
mizukamikatsuki | 2:4cdbd4b3722d | 299 | if(cha=='F'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 300 | twe.printf("flight mode on\r\n"); |
mizukamikatsuki | 2:4cdbd4b3722d | 301 | return -1; |
mizukamikatsuki | 2:4cdbd4b3722d | 302 | }else if(cha=='U'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 303 | servo1.pulsewidth(0.0015); |
mizukamikatsuki | 2:4cdbd4b3722d | 304 | servo2.pulsewidth(0.0006); |
mizukamikatsuki | 2:4cdbd4b3722d | 305 | }else if(cha=='L'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 306 | servo1.pulsewidth(0.0008); |
mizukamikatsuki | 2:4cdbd4b3722d | 307 | servo2.pulsewidth(0.0014); |
mizukamikatsuki | 2:4cdbd4b3722d | 308 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 309 | return 0; |
mbed_official | 0:bdbd3d6fc5d5 | 310 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 311 | |
mizukamikatsuki | 2:4cdbd4b3722d | 312 | void _heater(){ |
mizukamikatsuki | 2:4cdbd4b3722d | 313 | if(twe.readable()){ |
mizukamikatsuki | 2:4cdbd4b3722d | 314 | char comh=twe.getc(); |
mizukamikatsuki | 2:4cdbd4b3722d | 315 | if(comh=='H'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 316 | fte=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 317 | led4=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 318 | }else if(comh=='C'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 319 | fte=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 320 | led4=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 321 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 322 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 323 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 324 | |
mizukamikatsuki | 2:4cdbd4b3722d | 325 | void _buzzer(){ |
mizukamikatsuki | 2:4cdbd4b3722d | 326 | if(twe.readable()){ |
mizukamikatsuki | 2:4cdbd4b3722d | 327 | char comb=twe.getc(); |
mizukamikatsuki | 2:4cdbd4b3722d | 328 | if(comb=='N'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 329 | buzz=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 330 | led4=1; |
mizukamikatsuki | 2:4cdbd4b3722d | 331 | }else if(comb=='S'){ |
mizukamikatsuki | 2:4cdbd4b3722d | 332 | buzz=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 333 | led4=0; |
mizukamikatsuki | 2:4cdbd4b3722d | 334 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 335 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 336 | |
mizukamikatsuki | 2:4cdbd4b3722d | 337 | } |
mizukamikatsuki | 2:4cdbd4b3722d | 338 |