201903_ISE
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp@6:f17310205c1f, 2019-02-12 (annotated)
- Committer:
- Yukina
- Date:
- Tue Feb 12 11:20:33 2019 +0000
- Revision:
- 6:f17310205c1f
- Parent:
- 5:f6e956e8a060
- Child:
- 7:9953d922499d
add madgwick
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mikawataru | 0:0ff20d8e9090 | 1 | #include "mbed.h" |
Yukina | 4:4b3ae90ec778 | 2 | #include "MPU6050.h" |
mikawataru | 0:0ff20d8e9090 | 3 | #include "BMP180.h" |
Yukina | 5:f6e956e8a060 | 4 | #include "Kalman.h" |
Yukina | 5:f6e956e8a060 | 5 | #include "MadgwickAHRS.h" |
mikawataru | 0:0ff20d8e9090 | 6 | |
Yukina | 5:f6e956e8a060 | 7 | /*しきい値など*/ |
Yukina | 5:f6e956e8a060 | 8 | #define ACC_JUDGE_LAUNCH 1.5 |
Yukina | 5:f6e956e8a060 | 9 | #define TIME_BURNING 6 |
Yukina | 5:f6e956e8a060 | 10 | #define ALT_JUDGE_FIRE 1 |
Yukina | 5:f6e956e8a060 | 11 | #define ALT_JUDGE_OPEN 2 |
Yukina | 5:f6e956e8a060 | 12 | #define ANGLE_JUDGE_FIRE 90 |
Yukina | 5:f6e956e8a060 | 13 | #define CNT_JUDGE 3 |
Yukina | 5:f6e956e8a060 | 14 | #define TIME_JUDGE_CNT 1.5 |
Yukina | 5:f6e956e8a060 | 15 | #define NUM_CNT_MEDIAN 10 |
Yukina | 5:f6e956e8a060 | 16 | #define RATE_GPS 1 |
Yukina | 5:f6e956e8a060 | 17 | #define RATE_DATA 2 |
Yukina | 5:f6e956e8a060 | 18 | #define p0 1013.25f |
Yukina | 5:f6e956e8a060 | 19 | #define RadToDeg 57.295779513082320876798154814105 |
Yukina | 5:f6e956e8a060 | 20 | |
Yukina | 5:f6e956e8a060 | 21 | MPU6050 mpu(dp5,dp27); |
Yukina | 5:f6e956e8a060 | 22 | BMP180 bmp(dp5,dp27); |
Yukina | 5:f6e956e8a060 | 23 | KalmanFilter gKfx, gKfy; |
Yukina | 5:f6e956e8a060 | 24 | Madgwick MadgwickFilter; |
Yukina | 5:f6e956e8a060 | 25 | Serial pc(dp16,dp15); |
Yukina | 5:f6e956e8a060 | 26 | Timer timer_open; |
Yukina | 5:f6e956e8a060 | 27 | Timer timer_data; |
Yukina | 5:f6e956e8a060 | 28 | Ticker tic_data; |
Yukina | 5:f6e956e8a060 | 29 | Ticker tic_gps; |
mikawataru | 0:0ff20d8e9090 | 30 | |
Yukina | 5:f6e956e8a060 | 31 | /*自作関数*/ |
Yukina | 5:f6e956e8a060 | 32 | float _getAlt(); |
Yukina | 5:f6e956e8a060 | 33 | float _median(float data[],int num); |
Yukina | 5:f6e956e8a060 | 34 | void _SendData(); |
Yukina | 5:f6e956e8a060 | 35 | void _SendGPS(); |
Yukina | 5:f6e956e8a060 | 36 | float _DMS2DEG(float raw_data); |
Yukina | 4:4b3ae90ec778 | 37 | |
Yukina | 5:f6e956e8a060 | 38 | enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; |
Yukina | 4:4b3ae90ec778 | 39 | |
Yukina | 5:f6e956e8a060 | 40 | /*グローバル変数*/ |
Yukina | 5:f6e956e8a060 | 41 | //カルマン用 |
Yukina | 5:f6e956e8a060 | 42 | float gCalibrateX; |
Yukina | 5:f6e956e8a060 | 43 | float gCalibrateY; |
Yukina | 5:f6e956e8a060 | 44 | float gPrevMicros; |
Yukina | 5:f6e956e8a060 | 45 | float degRoll,degPitch; |
mikawataru | 0:0ff20d8e9090 | 46 | |
Yukina | 4:4b3ae90ec778 | 47 | int main(){ |
Yukina | 5:f6e956e8a060 | 48 | /*ローカル変数*/ |
Yukina | 5:f6e956e8a060 | 49 | float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs; |
Yukina | 5:f6e956e8a060 | 50 | float alt_buff[10],alt_md,alt_max; |
Yukina | 5:f6e956e8a060 | 51 | float time_judge; |
Yukina | 5:f6e956e8a060 | 52 | int cnt_data=0,cnt_judge=0; |
Yukina | 5:f6e956e8a060 | 53 | |
Yukina | 5:f6e956e8a060 | 54 | /*センサの初期化等*/ |
Yukina | 5:f6e956e8a060 | 55 | pc.baud(9600); |
Yukina | 5:f6e956e8a060 | 56 | mpu.setAcceleroRange(3); |
mikawataru | 0:0ff20d8e9090 | 57 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
Yukina | 5:f6e956e8a060 | 58 | |
Yukina | 5:f6e956e8a060 | 59 | /*初期位置の設定*/ |
Yukina | 5:f6e956e8a060 | 60 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 61 | mpu.getGyro(gyro); |
Yukina | 5:f6e956e8a060 | 62 | degRoll = atan2(acc[1], acc[2]) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 63 | degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 64 | gKfx.setAngle(degRoll); |
Yukina | 5:f6e956e8a060 | 65 | gKfy.setAngle(degPitch); |
Yukina | 5:f6e956e8a060 | 66 | gCalibrateY = degPitch; |
Yukina | 5:f6e956e8a060 | 67 | gCalibrateX = degRoll; |
Yukina | 5:f6e956e8a060 | 68 | |
Yukina | 5:f6e956e8a060 | 69 | Phase = STANDBY; |
Yukina | 4:4b3ae90ec778 | 70 | |
Yukina | 5:f6e956e8a060 | 71 | while(1){ |
Yukina | 5:f6e956e8a060 | 72 | switch(Phase){ |
Yukina | 5:f6e956e8a060 | 73 | case STANDBY: |
Yukina | 5:f6e956e8a060 | 74 | /*入力待ち*/ |
Yukina | 5:f6e956e8a060 | 75 | timer_data.start(); |
Yukina | 5:f6e956e8a060 | 76 | tic_data.attach(&_SendData, 1.0/RATE_DATA); |
Yukina | 5:f6e956e8a060 | 77 | Phase = LAUNCH; |
Yukina | 5:f6e956e8a060 | 78 | break; |
Yukina | 5:f6e956e8a060 | 79 | case LAUNCH: |
Yukina | 5:f6e956e8a060 | 80 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 81 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 82 | acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)); |
Yukina | 5:f6e956e8a060 | 83 | } |
Yukina | 5:f6e956e8a060 | 84 | acc_abs = _median(acc_buff,NUM_CNT_MEDIAN); |
Yukina | 5:f6e956e8a060 | 85 | /*加速度判定*/ |
Yukina | 5:f6e956e8a060 | 86 | if(acc_abs>ACC_JUDGE_LAUNCH){ |
Yukina | 5:f6e956e8a060 | 87 | cnt_judge++; |
Yukina | 4:4b3ae90ec778 | 88 | } |
Yukina | 5:f6e956e8a060 | 89 | if(cnt_judge==CNT_JUDGE){ |
Yukina | 5:f6e956e8a060 | 90 | cnt_judge=0; |
Yukina | 5:f6e956e8a060 | 91 | timer_open.start(); |
Yukina | 5:f6e956e8a060 | 92 | Phase = FIRE; |
Yukina | 5:f6e956e8a060 | 93 | } |
Yukina | 5:f6e956e8a060 | 94 | break; |
Yukina | 5:f6e956e8a060 | 95 | case RISE: |
Yukina | 5:f6e956e8a060 | 96 | while(timer_open.read() < TIME_BURNING){ |
Yukina | 5:f6e956e8a060 | 97 | } |
Yukina | 5:f6e956e8a060 | 98 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 99 | alt_buff[cnt_data] = _getAlt(); |
Yukina | 5:f6e956e8a060 | 100 | } |
Yukina | 5:f6e956e8a060 | 101 | alt_md = _median(alt_buff,NUM_CNT_MEDIAN); |
Yukina | 5:f6e956e8a060 | 102 | if(alt_md > ALT_JUDGE_FIRE){ |
Yukina | 5:f6e956e8a060 | 103 | Phase = FIRE; |
Yukina | 5:f6e956e8a060 | 104 | } |
Yukina | 5:f6e956e8a060 | 105 | break; |
Yukina | 5:f6e956e8a060 | 106 | case FIRE: |
Yukina | 6:f17310205c1f | 107 | |
Yukina | 6:f17310205c1f | 108 | //カルマン |
Yukina | 6:f17310205c1f | 109 | /* |
Yukina | 5:f6e956e8a060 | 110 | gPrevMicros = timer_open.read(); |
Yukina | 4:4b3ae90ec778 | 111 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 112 | mpu.getGyro(gyro); |
Yukina | 5:f6e956e8a060 | 113 | degRoll = atan2(acc[1], acc[2]) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 114 | degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 115 | |
Yukina | 5:f6e956e8a060 | 116 | float dpsX = gyro[0] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 117 | float dpsY = gyro[1] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 118 | float dpsZ = gyro[2] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 119 | |
Yukina | 5:f6e956e8a060 | 120 | float curMicros = timer_open.read(); |
Yukina | 5:f6e956e8a060 | 121 | float dt = curMicros - gPrevMicros; |
Yukina | 5:f6e956e8a060 | 122 | gPrevMicros = curMicros; |
Yukina | 4:4b3ae90ec778 | 123 | |
Yukina | 5:f6e956e8a060 | 124 | float degX = gKfx.calcAngle(degRoll, dpsX, dt); |
Yukina | 5:f6e956e8a060 | 125 | float degY = gKfy.calcAngle(degPitch, dpsY, dt); |
Yukina | 5:f6e956e8a060 | 126 | degY -= gCalibrateY; |
Yukina | 5:f6e956e8a060 | 127 | degX -= gCalibrateX; |
Yukina | 5:f6e956e8a060 | 128 | if(degY>ANGLE_JUDGE_FIRE){ |
Yukina | 5:f6e956e8a060 | 129 | Phase = OPEN; |
Yukina | 5:f6e956e8a060 | 130 | } |
Yukina | 6:f17310205c1f | 131 | */ |
Yukina | 6:f17310205c1f | 132 | |
Yukina | 6:f17310205c1f | 133 | //madgwick |
Yukina | 6:f17310205c1f | 134 | float Roll,Pitch,Yaw; |
Yukina | 6:f17310205c1f | 135 | MadgwickFilter.begin(2); |
Yukina | 6:f17310205c1f | 136 | mpu.getAccelero(acc); |
Yukina | 6:f17310205c1f | 137 | mpu.getGyro(gyro); |
Yukina | 6:f17310205c1f | 138 | gyro[0] *= RadToDeg; |
Yukina | 6:f17310205c1f | 139 | gyro[1] *= RadToDeg; |
Yukina | 6:f17310205c1f | 140 | gyro[2] *= RadToDeg; |
Yukina | 6:f17310205c1f | 141 | |
Yukina | 6:f17310205c1f | 142 | MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]); |
Yukina | 6:f17310205c1f | 143 | Roll = MadgwickFilter.getRoll(); |
Yukina | 6:f17310205c1f | 144 | Pitch = MadgwickFilter.getPitch(); |
Yukina | 6:f17310205c1f | 145 | Yaw = MadgwickFilter.getYaw(); |
Yukina | 6:f17310205c1f | 146 | if(Pitch>ANGLE_JUDGE_FIRE){ |
Yukina | 6:f17310205c1f | 147 | Phase = OPEN; |
Yukina | 6:f17310205c1f | 148 | } |
Yukina | 6:f17310205c1f | 149 | |
Yukina | 5:f6e956e8a060 | 150 | break; |
Yukina | 5:f6e956e8a060 | 151 | case OPEN: |
Yukina | 5:f6e956e8a060 | 152 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 153 | alt_buff[cnt_data] = _getAlt(); |
Yukina | 5:f6e956e8a060 | 154 | } |
Yukina | 5:f6e956e8a060 | 155 | alt_md = _median(alt_buff,NUM_CNT_MEDIAN); |
Yukina | 5:f6e956e8a060 | 156 | if(alt_md > alt_max){ |
Yukina | 5:f6e956e8a060 | 157 | alt_max = alt_md; |
Yukina | 5:f6e956e8a060 | 158 | cnt_judge = 0; |
Yukina | 5:f6e956e8a060 | 159 | } |
Yukina | 5:f6e956e8a060 | 160 | else if((alt_max-alt_md) > ALT_JUDGE_OPEN){ |
Yukina | 5:f6e956e8a060 | 161 | cnt_judge++; |
Yukina | 5:f6e956e8a060 | 162 | time_judge = timer_open.read(); |
Yukina | 5:f6e956e8a060 | 163 | } |
Yukina | 4:4b3ae90ec778 | 164 | |
Yukina | 5:f6e956e8a060 | 165 | if((timer_open.read()-time_judge) - TIME_JUDGE_CNT) cnt_judge=0; |
Yukina | 5:f6e956e8a060 | 166 | if(cnt_judge == CNT_JUDGE){ |
Yukina | 5:f6e956e8a060 | 167 | cnt_judge = 0; |
Yukina | 5:f6e956e8a060 | 168 | Phase = RECOVERY; |
Yukina | 5:f6e956e8a060 | 169 | } |
Yukina | 5:f6e956e8a060 | 170 | break; |
Yukina | 5:f6e956e8a060 | 171 | case RECOVERY: |
Yukina | 5:f6e956e8a060 | 172 | tic_data.detach(); |
Yukina | 5:f6e956e8a060 | 173 | //tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); |
Yukina | 5:f6e956e8a060 | 174 | Phase = SEA; |
Yukina | 5:f6e956e8a060 | 175 | break; |
Yukina | 5:f6e956e8a060 | 176 | case SEA: |
Yukina | 5:f6e956e8a060 | 177 | break; |
Yukina | 4:4b3ae90ec778 | 178 | } |
Yukina | 4:4b3ae90ec778 | 179 | } |
Yukina | 4:4b3ae90ec778 | 180 | } |
Yukina | 4:4b3ae90ec778 | 181 | |
Yukina | 4:4b3ae90ec778 | 182 | float _getAlt(){ |
Yukina | 4:4b3ae90ec778 | 183 | float altitude,pressure,temperature; |
Yukina | 4:4b3ae90ec778 | 184 | bmp.ReadData(&temperature,&pressure); |
Yukina | 4:4b3ae90ec778 | 185 | altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; |
Yukina | 4:4b3ae90ec778 | 186 | return altitude; |
Yukina | 4:4b3ae90ec778 | 187 | } |
Yukina | 4:4b3ae90ec778 | 188 | |
Yukina | 5:f6e956e8a060 | 189 | void _SendData(){ |
Yukina | 5:f6e956e8a060 | 190 | float pretime,a[3],g[3],alt; |
Yukina | 5:f6e956e8a060 | 191 | //カルマン |
Yukina | 5:f6e956e8a060 | 192 | /* |
Yukina | 5:f6e956e8a060 | 193 | pretime = timer_data.read(); |
Yukina | 5:f6e956e8a060 | 194 | mpu.getAccelero(a); |
Yukina | 5:f6e956e8a060 | 195 | mpu.getGyro(g); |
Yukina | 5:f6e956e8a060 | 196 | float degroll = atan2(a[1], a[2]) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 197 | float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 198 | |
Yukina | 5:f6e956e8a060 | 199 | float dpsx = g[0] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 200 | float dpsy = g[1] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 201 | float dpsz = g[2] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 202 | |
Yukina | 5:f6e956e8a060 | 203 | float curtime = timer_data.read(); |
Yukina | 5:f6e956e8a060 | 204 | float t = curtime - pretime; |
Yukina | 5:f6e956e8a060 | 205 | pretime = curtime; |
Yukina | 5:f6e956e8a060 | 206 | |
Yukina | 5:f6e956e8a060 | 207 | float degx = gKfx.calcAngle(degroll, dpsx, t); |
Yukina | 5:f6e956e8a060 | 208 | float degy = gKfy.calcAngle(degpitch, dpsx, t); |
Yukina | 5:f6e956e8a060 | 209 | degy -= gCalibrateY; |
Yukina | 5:f6e956e8a060 | 210 | degx -= gCalibrateX; |
Yukina | 5:f6e956e8a060 | 211 | */ |
Yukina | 5:f6e956e8a060 | 212 | /*madgwick*/ |
Yukina | 5:f6e956e8a060 | 213 | float Roll,Pitch,Yaw; |
Yukina | 5:f6e956e8a060 | 214 | MadgwickFilter.begin(2); |
Yukina | 5:f6e956e8a060 | 215 | mpu.getAccelero(a); |
Yukina | 5:f6e956e8a060 | 216 | mpu.getGyro(g); |
Yukina | 5:f6e956e8a060 | 217 | g[0] *= RadToDeg; |
Yukina | 5:f6e956e8a060 | 218 | g[1] *= RadToDeg; |
Yukina | 5:f6e956e8a060 | 219 | g[2] *= RadToDeg; |
Yukina | 5:f6e956e8a060 | 220 | |
Yukina | 5:f6e956e8a060 | 221 | MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]); |
Yukina | 5:f6e956e8a060 | 222 | Roll = MadgwickFilter.getRoll(); |
Yukina | 5:f6e956e8a060 | 223 | Pitch = MadgwickFilter.getPitch(); |
Yukina | 5:f6e956e8a060 | 224 | Yaw = MadgwickFilter.getYaw(); |
Yukina | 5:f6e956e8a060 | 225 | |
Yukina | 5:f6e956e8a060 | 226 | alt = _getAlt(); |
Yukina | 5:f6e956e8a060 | 227 | |
Yukina | 5:f6e956e8a060 | 228 | //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy); |
Yukina | 5:f6e956e8a060 | 229 | pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch); |
Yukina | 5:f6e956e8a060 | 230 | } |
Yukina | 5:f6e956e8a060 | 231 | |
Yukina | 5:f6e956e8a060 | 232 | /* |
Yukina | 5:f6e956e8a060 | 233 | void _SendGPS(){ |
Yukina | 5:f6e956e8a060 | 234 | int cnt_gps; |
Yukina | 5:f6e956e8a060 | 235 | if(gps.readable()){ |
Yukina | 5:f6e956e8a060 | 236 | gps_data[cnt_gps] = gps.getc(); |
Yukina | 5:f6e956e8a060 | 237 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
Yukina | 5:f6e956e8a060 | 238 | cnt_gps = 0; |
Yukina | 5:f6e956e8a060 | 239 | memset(gps_data,'\0',256); |
Yukina | 5:f6e956e8a060 | 240 | }else if(gps_data[cnt_gps] == '\r'){ |
Yukina | 5:f6e956e8a060 | 241 | float world_time, lon_east, lat_north; |
Yukina | 5:f6e956e8a060 | 242 | int rlock, sat_num; |
Yukina | 5:f6e956e8a060 | 243 | char lat,lon; |
Yukina | 5:f6e956e8a060 | 244 | if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ |
Yukina | 5:f6e956e8a060 | 245 | if(rlock==1){ |
Yukina | 5:f6e956e8a060 | 246 | lat_north = _DMS2DEG(lat_north); |
Yukina | 5:f6e956e8a060 | 247 | lon_east = _DMS2DEG(lon_east); |
Yukina | 5:f6e956e8a060 | 248 | twe.printf("%s\r\n",gps_data); |
Yukina | 5:f6e956e8a060 | 249 | twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); |
Yukina | 5:f6e956e8a060 | 250 | }else{ |
Yukina | 5:f6e956e8a060 | 251 | twe.printf("%s\r\n",gps_data); |
Yukina | 5:f6e956e8a060 | 252 | } |
Yukina | 5:f6e956e8a060 | 253 | } |
Yukina | 5:f6e956e8a060 | 254 | }else{ |
Yukina | 5:f6e956e8a060 | 255 | cnt_gps++; |
Yukina | 5:f6e956e8a060 | 256 | } |
Yukina | 4:4b3ae90ec778 | 257 | } |
Yukina | 4:4b3ae90ec778 | 258 | } |
Yukina | 4:4b3ae90ec778 | 259 | |
Yukina | 4:4b3ae90ec778 | 260 | float _DMS2DEG(float raw_data){ |
Yukina | 4:4b3ae90ec778 | 261 | int d=(int)(raw_data/100); |
Yukina | 4:4b3ae90ec778 | 262 | float m=(raw_data-(float)d*100); |
Yukina | 4:4b3ae90ec778 | 263 | return (float)d+m/60; |
Yukina | 4:4b3ae90ec778 | 264 | } |
Yukina | 5:f6e956e8a060 | 265 | */ |
Yukina | 4:4b3ae90ec778 | 266 | |
Yukina | 4:4b3ae90ec778 | 267 | float _median(float data[], int num){ |
Yukina | 4:4b3ae90ec778 | 268 | float *data_cpy, ans; |
Yukina | 4:4b3ae90ec778 | 269 | data_cpy = new float[num]; |
Yukina | 4:4b3ae90ec778 | 270 | memcpy(data_cpy,data,sizeof(float)*num); |
Yukina | 4:4b3ae90ec778 | 271 | |
Yukina | 4:4b3ae90ec778 | 272 | for(int i=0; i<num; i++){ |
Yukina | 4:4b3ae90ec778 | 273 | for(int j=0; j<num-i-1; j++){ |
Yukina | 4:4b3ae90ec778 | 274 | if(data_cpy[j]>data_cpy[j+1]){ |
Yukina | 4:4b3ae90ec778 | 275 | float buff = data_cpy[j+1]; |
Yukina | 4:4b3ae90ec778 | 276 | data_cpy[j+1] = data_cpy[j]; |
Yukina | 4:4b3ae90ec778 | 277 | data_cpy[j] = buff; |
Yukina | 4:4b3ae90ec778 | 278 | } |
Yukina | 4:4b3ae90ec778 | 279 | } |
Yukina | 4:4b3ae90ec778 | 280 | } |
Yukina | 4:4b3ae90ec778 | 281 | |
Yukina | 4:4b3ae90ec778 | 282 | if(num%2!=0) ans = data_cpy[num/2]; |
Yukina | 4:4b3ae90ec778 | 283 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
Yukina | 4:4b3ae90ec778 | 284 | delete[] data_cpy; |
Yukina | 4:4b3ae90ec778 | 285 | return ans; |
Yukina | 5:f6e956e8a060 | 286 | } |