克輝 水上 / Mbed 2 deprecated kiritannpokaihou

きりたんぽの開放プログラム

Dependencies:   BMP180 MPU6050 SDFileSystem mbed

Fork of SDFileSystem_HelloWorld by mbed official

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?

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