201903_ISE
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp@9:42b4d337d4cc, 2019-03-09 (annotated)
- Committer:
- sashida_h
- Date:
- Sat Mar 09 12:23:07 2019 +0000
- Revision:
- 9:42b4d337d4cc
- Parent:
- 8:15a1b22df82f
- Child:
- 10:1a626929850e
2019_0309;
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 | /*しきい値など*/ |
sashida_h | 8:15a1b22df82f | 8 | #define ACC_JUDGE_LAUNCH 3.0 //発射判定のしきい値 |
sashida_h | 8:15a1b22df82f | 9 | #define TIME_BURNING 6 //開放判定しない時間(燃焼時間) |
sashida_h | 7:9953d922499d | 10 | #define ALT_JUDGE_FIRE 0 |
sashida_h | 8:15a1b22df82f | 11 | #define ALT_JUDGE_OPEN 1 //落下判定のカウントを1増やす高度差 |
sashida_h | 8:15a1b22df82f | 12 | #define TIME_OPEN 25 //強制的に開放させる時間 |
sashida_h | 8:15a1b22df82f | 13 | #define TIME_SEND 1.0 |
sashida_h | 7:9953d922499d | 14 | #define ANGLE_JUDGE_FIRE_MIN 15 |
sashida_h | 7:9953d922499d | 15 | #define ANGLE_JUDGE_FIRE_MAX 70 |
sashida_h | 8:15a1b22df82f | 16 | #define CNT_JUDGE 10 |
Yukina | 5:f6e956e8a060 | 17 | #define TIME_JUDGE_CNT 1.5 |
Yukina | 5:f6e956e8a060 | 18 | #define NUM_CNT_MEDIAN 10 |
sashida_h | 7:9953d922499d | 19 | #define RATE_GPS 1.0 |
sashida_h | 7:9953d922499d | 20 | #define RATE_DATA 10 |
sashida_h | 7:9953d922499d | 21 | #define TIMER_NOTFIRE 15.0 |
Yukina | 5:f6e956e8a060 | 22 | #define p0 1013.25f |
Yukina | 5:f6e956e8a060 | 23 | #define RadToDeg 57.295779513082320876798154814105 |
Yukina | 5:f6e956e8a060 | 24 | |
sashida_h | 7:9953d922499d | 25 | MPU6050 mpu(PB_7,PB_6); |
sashida_h | 7:9953d922499d | 26 | BMP180 bmp(PB_7,PB_6); |
Yukina | 5:f6e956e8a060 | 27 | KalmanFilter gKfx, gKfy; |
Yukina | 5:f6e956e8a060 | 28 | Madgwick MadgwickFilter; |
sashida_h | 7:9953d922499d | 29 | Serial pc(PA_2, PA_3); |
sashida_h | 7:9953d922499d | 30 | Serial gps(PA_9, PA_10); |
sashida_h | 9:42b4d337d4cc | 31 | DigitalOut myled(PA_15); |
Yukina | 5:f6e956e8a060 | 32 | Timer timer_open; |
Yukina | 5:f6e956e8a060 | 33 | Timer timer_data; |
Yukina | 5:f6e956e8a060 | 34 | Ticker tic_data; |
Yukina | 5:f6e956e8a060 | 35 | Ticker tic_gps; |
mikawataru | 0:0ff20d8e9090 | 36 | |
Yukina | 5:f6e956e8a060 | 37 | /*自作関数*/ |
Yukina | 5:f6e956e8a060 | 38 | float _getAlt(); |
Yukina | 5:f6e956e8a060 | 39 | float _median(float data[],int num); |
Yukina | 5:f6e956e8a060 | 40 | void _SendData(); |
Yukina | 5:f6e956e8a060 | 41 | void _SendGPS(); |
Yukina | 5:f6e956e8a060 | 42 | float _DMS2DEG(float raw_data); |
sashida_h | 9:42b4d337d4cc | 43 | int servo(); |
Yukina | 4:4b3ae90ec778 | 44 | |
Yukina | 5:f6e956e8a060 | 45 | enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; |
Yukina | 4:4b3ae90ec778 | 46 | |
Yukina | 5:f6e956e8a060 | 47 | /*グローバル変数*/ |
sashida_h | 8:15a1b22df82f | 48 | float t = 0; |
sashida_h | 7:9953d922499d | 49 | //地上高度 |
sashida_h | 7:9953d922499d | 50 | float alt_gnd; |
sashida_h | 9:42b4d337d4cc | 51 | float alt_max; |
sashida_h | 7:9953d922499d | 52 | //GPS |
sashida_h | 7:9953d922499d | 53 | int cnt_gps=0; |
sashida_h | 7:9953d922499d | 54 | |
sashida_h | 7:9953d922499d | 55 | int p = 1; |
sashida_h | 9:42b4d337d4cc | 56 | //サーボ |
sashida_h | 9:42b4d337d4cc | 57 | int i = 0; |
sashida_h | 9:42b4d337d4cc | 58 | char c[516]; |
sashida_h | 9:42b4d337d4cc | 59 | char d [8]; |
sashida_h | 9:42b4d337d4cc | 60 | int len; |
sashida_h | 9:42b4d337d4cc | 61 | char f[]="Receive"; |
mikawataru | 0:0ff20d8e9090 | 62 | |
sashida_h | 9:42b4d337d4cc | 63 | void main(){ |
Yukina | 5:f6e956e8a060 | 64 | /*ローカル変数*/ |
Yukina | 5:f6e956e8a060 | 65 | float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs; |
sashida_h | 9:42b4d337d4cc | 66 | float alt_buff[10],alt_md; |
Yukina | 5:f6e956e8a060 | 67 | float time_judge; |
Yukina | 5:f6e956e8a060 | 68 | int cnt_data=0,cnt_judge=0; |
sashida_h | 9:42b4d337d4cc | 69 | char gps_data[256]; |
Yukina | 5:f6e956e8a060 | 70 | /*センサの初期化等*/ |
sashida_h | 7:9953d922499d | 71 | pc.baud(38400); |
sashida_h | 9:42b4d337d4cc | 72 | gps.baud(115200); |
Yukina | 5:f6e956e8a060 | 73 | mpu.setAcceleroRange(3); |
mikawataru | 0:0ff20d8e9090 | 74 | bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); |
Yukina | 5:f6e956e8a060 | 75 | |
Yukina | 5:f6e956e8a060 | 76 | /*初期位置の設定*/ |
Yukina | 5:f6e956e8a060 | 77 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 78 | mpu.getGyro(gyro); |
Yukina | 5:f6e956e8a060 | 79 | |
Yukina | 5:f6e956e8a060 | 80 | Phase = STANDBY; |
sashida_h | 7:9953d922499d | 81 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
sashida_h | 7:9953d922499d | 82 | alt_buff[cnt_data] = _getAlt(); |
sashida_h | 7:9953d922499d | 83 | } |
sashida_h | 7:9953d922499d | 84 | alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN); |
sashida_h | 7:9953d922499d | 85 | wait(2.0); |
sashida_h | 7:9953d922499d | 86 | pc.printf("Hello World!\r\n"); |
sashida_h | 8:15a1b22df82f | 87 | wait(1.0); |
sashida_h | 8:15a1b22df82f | 88 | pc.printf("f:Flight_mode_on\r\n"); |
sashida_h | 9:42b4d337d4cc | 89 | gps.printf("f:Flight_mode_on\r\n"); |
sashida_h | 8:15a1b22df82f | 90 | wait(1.0); |
sashida_h | 9:42b4d337d4cc | 91 | timer_data.start(); |
Yukina | 5:f6e956e8a060 | 92 | while(1){ |
Yukina | 5:f6e956e8a060 | 93 | switch(Phase){ |
Yukina | 5:f6e956e8a060 | 94 | case STANDBY: |
sashida_h | 9:42b4d337d4cc | 95 | //gps.printf("Phase_STANDBY\r\n"); |
Yukina | 5:f6e956e8a060 | 96 | /*入力待ち*/ |
sashida_h | 7:9953d922499d | 97 | //tic_data.attach(&_SendData, 1.0/RATE_DATA); |
sashida_h | 9:42b4d337d4cc | 98 | servo(); |
sashida_h | 9:42b4d337d4cc | 99 | //c[i] = pc.getc(); |
sashida_h | 9:42b4d337d4cc | 100 | //gps.printf("%c",c[i]); |
sashida_h | 9:42b4d337d4cc | 101 | //i++; |
sashida_h | 9:42b4d337d4cc | 102 | //if(c[i] == '\n') i=0; |
sashida_h | 9:42b4d337d4cc | 103 | //wait(1.0); |
Yukina | 5:f6e956e8a060 | 104 | break; |
sashida_h | 8:15a1b22df82f | 105 | |
Yukina | 5:f6e956e8a060 | 106 | case LAUNCH: |
Yukina | 5:f6e956e8a060 | 107 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 108 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 109 | 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 | 110 | } |
Yukina | 5:f6e956e8a060 | 111 | acc_abs = _median(acc_buff,NUM_CNT_MEDIAN); |
sashida_h | 8:15a1b22df82f | 112 | if(timer_data.read() - t > TIME_SEND){ |
sashida_h | 8:15a1b22df82f | 113 | pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge); |
sashida_h | 8:15a1b22df82f | 114 | t = timer_data.read(); |
sashida_h | 7:9953d922499d | 115 | } |
Yukina | 5:f6e956e8a060 | 116 | /*加速度判定*/ |
Yukina | 5:f6e956e8a060 | 117 | if(acc_abs>ACC_JUDGE_LAUNCH){ |
Yukina | 5:f6e956e8a060 | 118 | cnt_judge++; |
Yukina | 4:4b3ae90ec778 | 119 | } |
sashida_h | 8:15a1b22df82f | 120 | if(cnt_judge==CNT_JUDGE){ |
Yukina | 5:f6e956e8a060 | 121 | cnt_judge=0; |
Yukina | 5:f6e956e8a060 | 122 | timer_open.start(); |
sashida_h | 7:9953d922499d | 123 | Phase = RISE; |
Yukina | 5:f6e956e8a060 | 124 | } |
Yukina | 5:f6e956e8a060 | 125 | break; |
sashida_h | 8:15a1b22df82f | 126 | |
Yukina | 5:f6e956e8a060 | 127 | case RISE: |
Yukina | 5:f6e956e8a060 | 128 | while(timer_open.read() < TIME_BURNING){ |
sashida_h | 8:15a1b22df82f | 129 | pc.printf("RISE,time from launch:%f\r\n",timer_open.read()); |
sashida_h | 8:15a1b22df82f | 130 | wait(1.0); |
sashida_h | 7:9953d922499d | 131 | i=0; |
sashida_h | 8:15a1b22df82f | 132 | timer_data.reset(); |
Yukina | 5:f6e956e8a060 | 133 | } |
sashida_h | 8:15a1b22df82f | 134 | Phase = OPEN; |
sashida_h | 8:15a1b22df82f | 135 | t = 0.0; |
Yukina | 5:f6e956e8a060 | 136 | break; |
Yukina | 5:f6e956e8a060 | 137 | case FIRE: |
Yukina | 6:f17310205c1f | 138 | |
Yukina | 6:f17310205c1f | 139 | //カルマン |
Yukina | 6:f17310205c1f | 140 | /* |
Yukina | 5:f6e956e8a060 | 141 | gPrevMicros = timer_open.read(); |
Yukina | 4:4b3ae90ec778 | 142 | mpu.getAccelero(acc); |
Yukina | 5:f6e956e8a060 | 143 | mpu.getGyro(gyro); |
Yukina | 5:f6e956e8a060 | 144 | degRoll = atan2(acc[1], acc[2]) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 145 | degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 146 | |
Yukina | 5:f6e956e8a060 | 147 | float dpsX = gyro[0] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 148 | float dpsY = gyro[1] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 149 | float dpsZ = gyro[2] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 150 | |
Yukina | 5:f6e956e8a060 | 151 | float curMicros = timer_open.read(); |
Yukina | 5:f6e956e8a060 | 152 | float dt = curMicros - gPrevMicros; |
Yukina | 5:f6e956e8a060 | 153 | gPrevMicros = curMicros; |
Yukina | 4:4b3ae90ec778 | 154 | |
Yukina | 5:f6e956e8a060 | 155 | float degX = gKfx.calcAngle(degRoll, dpsX, dt); |
Yukina | 5:f6e956e8a060 | 156 | float degY = gKfy.calcAngle(degPitch, dpsY, dt); |
Yukina | 5:f6e956e8a060 | 157 | degY -= gCalibrateY; |
Yukina | 5:f6e956e8a060 | 158 | degX -= gCalibrateX; |
Yukina | 5:f6e956e8a060 | 159 | if(degY>ANGLE_JUDGE_FIRE){ |
Yukina | 5:f6e956e8a060 | 160 | Phase = OPEN; |
Yukina | 5:f6e956e8a060 | 161 | } |
Yukina | 6:f17310205c1f | 162 | */ |
Yukina | 6:f17310205c1f | 163 | |
Yukina | 6:f17310205c1f | 164 | //madgwick |
Yukina | 6:f17310205c1f | 165 | float Roll,Pitch,Yaw; |
Yukina | 6:f17310205c1f | 166 | MadgwickFilter.begin(2); |
Yukina | 6:f17310205c1f | 167 | mpu.getAccelero(acc); |
Yukina | 6:f17310205c1f | 168 | mpu.getGyro(gyro); |
Yukina | 6:f17310205c1f | 169 | gyro[0] *= RadToDeg; |
Yukina | 6:f17310205c1f | 170 | gyro[1] *= RadToDeg; |
Yukina | 6:f17310205c1f | 171 | gyro[2] *= RadToDeg; |
Yukina | 6:f17310205c1f | 172 | |
Yukina | 6:f17310205c1f | 173 | MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]); |
Yukina | 6:f17310205c1f | 174 | Roll = MadgwickFilter.getRoll(); |
Yukina | 6:f17310205c1f | 175 | Pitch = MadgwickFilter.getPitch(); |
Yukina | 6:f17310205c1f | 176 | Yaw = MadgwickFilter.getYaw(); |
sashida_h | 7:9953d922499d | 177 | i++; |
sashida_h | 7:9953d922499d | 178 | if(i==400){ |
sashida_h | 7:9953d922499d | 179 | //pc.printf("TIME:%f,Pitch:%f\r\n",timer_data.read(),Pitch); |
sashida_h | 7:9953d922499d | 180 | pc.printf("FIRE:Pitch:%f,Time%f\r\n",Pitch,timer_open.read()); |
sashida_h | 7:9953d922499d | 181 | i=0; |
sashida_h | 7:9953d922499d | 182 | } |
sashida_h | 7:9953d922499d | 183 | /*if(Pitch>ANGLE_JUDGE_FIRE_MIN && Pitch < ANGLE_JUDGE_FIRE_MAX){ |
Yukina | 6:f17310205c1f | 184 | Phase = OPEN; |
sashida_h | 7:9953d922499d | 185 | i=0; |
Yukina | 6:f17310205c1f | 186 | } |
sashida_h | 7:9953d922499d | 187 | */ |
Yukina | 6:f17310205c1f | 188 | |
sashida_h | 7:9953d922499d | 189 | //if(timer_open.read()>TIMER_NOTFIRE){ |
sashida_h | 7:9953d922499d | 190 | if(timer_data.read()>60.0){ |
sashida_h | 7:9953d922499d | 191 | Phase = OPEN; |
sashida_h | 7:9953d922499d | 192 | pc.printf("NOT_FIRE!!\r\n"); |
sashida_h | 7:9953d922499d | 193 | } |
Yukina | 5:f6e956e8a060 | 194 | break; |
Yukina | 5:f6e956e8a060 | 195 | case OPEN: |
Yukina | 5:f6e956e8a060 | 196 | for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ |
Yukina | 5:f6e956e8a060 | 197 | alt_buff[cnt_data] = _getAlt(); |
Yukina | 5:f6e956e8a060 | 198 | } |
Yukina | 5:f6e956e8a060 | 199 | alt_md = _median(alt_buff,NUM_CNT_MEDIAN); |
sashida_h | 7:9953d922499d | 200 | alt_md = alt_md - alt_gnd; |
sashida_h | 8:15a1b22df82f | 201 | if(timer_open.read() - t > TIME_SEND){ |
sashida_h | 8:15a1b22df82f | 202 | pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge); |
sashida_h | 8:15a1b22df82f | 203 | t = timer_open.read(); |
sashida_h | 7:9953d922499d | 204 | } |
Yukina | 5:f6e956e8a060 | 205 | if(alt_md > alt_max){ |
Yukina | 5:f6e956e8a060 | 206 | alt_max = alt_md; |
Yukina | 5:f6e956e8a060 | 207 | cnt_judge = 0; |
Yukina | 5:f6e956e8a060 | 208 | } |
Yukina | 5:f6e956e8a060 | 209 | else if((alt_max-alt_md) > ALT_JUDGE_OPEN){ |
Yukina | 5:f6e956e8a060 | 210 | cnt_judge++; |
sashida_h | 8:15a1b22df82f | 211 | //time_judge = timer_open.read(); |
Yukina | 5:f6e956e8a060 | 212 | } |
Yukina | 4:4b3ae90ec778 | 213 | |
sashida_h | 7:9953d922499d | 214 | //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0; |
sashida_h | 8:15a1b22df82f | 215 | if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){ |
Yukina | 5:f6e956e8a060 | 216 | Phase = RECOVERY; |
sashida_h | 9:42b4d337d4cc | 217 | tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); |
Yukina | 5:f6e956e8a060 | 218 | } |
Yukina | 5:f6e956e8a060 | 219 | break; |
Yukina | 5:f6e956e8a060 | 220 | case RECOVERY: |
sashida_h | 9:42b4d337d4cc | 221 | /*while(1){ |
sashida_h | 8:15a1b22df82f | 222 | if(gps.readable()){ |
sashida_h | 8:15a1b22df82f | 223 | gps_data[cnt_gps] = gps.getc(); |
sashida_h | 8:15a1b22df82f | 224 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
sashida_h | 8:15a1b22df82f | 225 | cnt_gps = 0; |
sashida_h | 8:15a1b22df82f | 226 | memset(gps_data,'\0',256); |
sashida_h | 8:15a1b22df82f | 227 | }else if(gps_data[cnt_gps] == '\r'){ |
sashida_h | 8:15a1b22df82f | 228 | float world_time, lon_east, lat_north; |
sashida_h | 8:15a1b22df82f | 229 | int rlock, sat_num; |
sashida_h | 8:15a1b22df82f | 230 | char lat,lon; |
sashida_h | 8:15a1b22df82f | 231 | if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ |
sashida_h | 8:15a1b22df82f | 232 | if(rlock==1){ |
sashida_h | 8:15a1b22df82f | 233 | lat_north = _DMS2DEG(lat_north); |
sashida_h | 8:15a1b22df82f | 234 | lon_east = _DMS2DEG(lon_east); |
sashida_h | 8:15a1b22df82f | 235 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 8:15a1b22df82f | 236 | //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); |
sashida_h | 8:15a1b22df82f | 237 | int japan_time = int(world_time) - 9; |
sashida_h | 8:15a1b22df82f | 238 | pc.printf("Lat:%f,Lon:%f,time:%d,max_alt:%f\r\n",lat_north,lon_east,world_time,alt_max); |
sashida_h | 8:15a1b22df82f | 239 | break; |
sashida_h | 8:15a1b22df82f | 240 | }else{ |
sashida_h | 8:15a1b22df82f | 241 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 8:15a1b22df82f | 242 | pc.printf("NoGPSSignal,max_alt:%f\r\n",alt_max); |
sashida_h | 8:15a1b22df82f | 243 | break; |
sashida_h | 8:15a1b22df82f | 244 | } |
sashida_h | 8:15a1b22df82f | 245 | }else{ |
sashida_h | 9:42b4d337d4cc | 246 | //pc.printf("No_Satellite_signal\r\n"); |
sashida_h | 8:15a1b22df82f | 247 | } |
sashida_h | 8:15a1b22df82f | 248 | }else{ |
sashida_h | 8:15a1b22df82f | 249 | cnt_gps++; |
sashida_h | 8:15a1b22df82f | 250 | } |
sashida_h | 8:15a1b22df82f | 251 | } |
sashida_h | 8:15a1b22df82f | 252 | } |
sashida_h | 8:15a1b22df82f | 253 | |
sashida_h | 9:42b4d337d4cc | 254 | //Phase = SEA;*/ |
Yukina | 5:f6e956e8a060 | 255 | break; |
Yukina | 5:f6e956e8a060 | 256 | case SEA: |
sashida_h | 7:9953d922499d | 257 | |
Yukina | 5:f6e956e8a060 | 258 | break; |
Yukina | 4:4b3ae90ec778 | 259 | } |
Yukina | 4:4b3ae90ec778 | 260 | } |
Yukina | 4:4b3ae90ec778 | 261 | } |
Yukina | 4:4b3ae90ec778 | 262 | |
Yukina | 4:4b3ae90ec778 | 263 | float _getAlt(){ |
Yukina | 4:4b3ae90ec778 | 264 | float altitude,pressure,temperature; |
Yukina | 4:4b3ae90ec778 | 265 | bmp.ReadData(&temperature,&pressure); |
Yukina | 4:4b3ae90ec778 | 266 | altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; |
Yukina | 4:4b3ae90ec778 | 267 | return altitude; |
Yukina | 4:4b3ae90ec778 | 268 | } |
Yukina | 4:4b3ae90ec778 | 269 | |
Yukina | 5:f6e956e8a060 | 270 | void _SendData(){ |
Yukina | 5:f6e956e8a060 | 271 | float pretime,a[3],g[3],alt; |
Yukina | 5:f6e956e8a060 | 272 | //カルマン |
Yukina | 5:f6e956e8a060 | 273 | /* |
Yukina | 5:f6e956e8a060 | 274 | pretime = timer_data.read(); |
Yukina | 5:f6e956e8a060 | 275 | mpu.getAccelero(a); |
Yukina | 5:f6e956e8a060 | 276 | mpu.getGyro(g); |
Yukina | 5:f6e956e8a060 | 277 | float degroll = atan2(a[1], a[2]) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 278 | float degpitch = atan(-a[0] / sqrt(a[1] * a[1] + a[2] * a[2])) * RadToDeg; |
Yukina | 5:f6e956e8a060 | 279 | |
Yukina | 5:f6e956e8a060 | 280 | float dpsx = g[0] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 281 | float dpsy = g[1] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 282 | float dpsz = g[2] * RadToDeg; |
Yukina | 5:f6e956e8a060 | 283 | |
Yukina | 5:f6e956e8a060 | 284 | float curtime = timer_data.read(); |
Yukina | 5:f6e956e8a060 | 285 | float t = curtime - pretime; |
Yukina | 5:f6e956e8a060 | 286 | pretime = curtime; |
Yukina | 5:f6e956e8a060 | 287 | |
Yukina | 5:f6e956e8a060 | 288 | float degx = gKfx.calcAngle(degroll, dpsx, t); |
Yukina | 5:f6e956e8a060 | 289 | float degy = gKfy.calcAngle(degpitch, dpsx, t); |
Yukina | 5:f6e956e8a060 | 290 | degy -= gCalibrateY; |
Yukina | 5:f6e956e8a060 | 291 | degx -= gCalibrateX; |
Yukina | 5:f6e956e8a060 | 292 | */ |
Yukina | 5:f6e956e8a060 | 293 | /*madgwick*/ |
Yukina | 5:f6e956e8a060 | 294 | float Roll,Pitch,Yaw; |
Yukina | 5:f6e956e8a060 | 295 | MadgwickFilter.begin(2); |
Yukina | 5:f6e956e8a060 | 296 | mpu.getAccelero(a); |
Yukina | 5:f6e956e8a060 | 297 | mpu.getGyro(g); |
Yukina | 5:f6e956e8a060 | 298 | g[0] *= RadToDeg; |
Yukina | 5:f6e956e8a060 | 299 | g[1] *= RadToDeg; |
Yukina | 5:f6e956e8a060 | 300 | g[2] *= RadToDeg; |
Yukina | 5:f6e956e8a060 | 301 | |
Yukina | 5:f6e956e8a060 | 302 | MadgwickFilter.updateIMU(g[0],g[1],g[2],a[0],a[1],a[2]); |
Yukina | 5:f6e956e8a060 | 303 | Roll = MadgwickFilter.getRoll(); |
Yukina | 5:f6e956e8a060 | 304 | Pitch = MadgwickFilter.getPitch(); |
Yukina | 5:f6e956e8a060 | 305 | Yaw = MadgwickFilter.getYaw(); |
Yukina | 5:f6e956e8a060 | 306 | |
Yukina | 5:f6e956e8a060 | 307 | alt = _getAlt(); |
sashida_h | 7:9953d922499d | 308 | alt = alt - alt_gnd; |
Yukina | 5:f6e956e8a060 | 309 | //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy); |
Yukina | 5:f6e956e8a060 | 310 | pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch); |
Yukina | 5:f6e956e8a060 | 311 | } |
Yukina | 5:f6e956e8a060 | 312 | |
sashida_h | 9:42b4d337d4cc | 313 | float _DMS2DEG(float raw_data){ |
sashida_h | 9:42b4d337d4cc | 314 | int d=(int)(raw_data/100); |
sashida_h | 9:42b4d337d4cc | 315 | float m=(raw_data-(float)d*100); |
sashida_h | 9:42b4d337d4cc | 316 | return (float)d+m/60; |
sashida_h | 9:42b4d337d4cc | 317 | } |
sashida_h | 9:42b4d337d4cc | 318 | |
sashida_h | 9:42b4d337d4cc | 319 | |
sashida_h | 9:42b4d337d4cc | 320 | float _median(float data[], int num){ |
sashida_h | 9:42b4d337d4cc | 321 | float *data_cpy, ans; |
sashida_h | 9:42b4d337d4cc | 322 | data_cpy = new float[num]; |
sashida_h | 9:42b4d337d4cc | 323 | memcpy(data_cpy,data,sizeof(float)*num); |
sashida_h | 9:42b4d337d4cc | 324 | |
sashida_h | 9:42b4d337d4cc | 325 | for(i=0; i<num; i++){ |
sashida_h | 9:42b4d337d4cc | 326 | for(int j=0; j<num-i-1; j++){ |
sashida_h | 9:42b4d337d4cc | 327 | if(data_cpy[j]>data_cpy[j+1]){ |
sashida_h | 9:42b4d337d4cc | 328 | float buff = data_cpy[j+1]; |
sashida_h | 9:42b4d337d4cc | 329 | data_cpy[j+1] = data_cpy[j]; |
sashida_h | 9:42b4d337d4cc | 330 | data_cpy[j] = buff; |
sashida_h | 9:42b4d337d4cc | 331 | } |
sashida_h | 9:42b4d337d4cc | 332 | } |
sashida_h | 9:42b4d337d4cc | 333 | } |
sashida_h | 9:42b4d337d4cc | 334 | |
sashida_h | 9:42b4d337d4cc | 335 | if(num%2!=0) ans = data_cpy[num/2]; |
sashida_h | 9:42b4d337d4cc | 336 | else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; |
sashida_h | 9:42b4d337d4cc | 337 | delete[] data_cpy; |
sashida_h | 9:42b4d337d4cc | 338 | return ans; |
sashida_h | 9:42b4d337d4cc | 339 | } |
sashida_h | 9:42b4d337d4cc | 340 | |
sashida_h | 9:42b4d337d4cc | 341 | int servo(void){ |
sashida_h | 9:42b4d337d4cc | 342 | do{ |
sashida_h | 9:42b4d337d4cc | 343 | c[i] = pc.getc(); |
sashida_h | 9:42b4d337d4cc | 344 | gps.printf("%c",c[i]); |
sashida_h | 9:42b4d337d4cc | 345 | i++; |
sashida_h | 9:42b4d337d4cc | 346 | }while(c[i-1] != '\n'); |
sashida_h | 9:42b4d337d4cc | 347 | gps.printf("path\r\n"); |
sashida_h | 9:42b4d337d4cc | 348 | //gps.printf("%s",c); |
sashida_h | 9:42b4d337d4cc | 349 | myled = 1; |
sashida_h | 9:42b4d337d4cc | 350 | //gps.printf("%s",c); |
sashida_h | 9:42b4d337d4cc | 351 | len = strlen(f); |
sashida_h | 9:42b4d337d4cc | 352 | i=0; |
sashida_h | 9:42b4d337d4cc | 353 | while(i != 4){ |
sashida_h | 9:42b4d337d4cc | 354 | if (c[i] != f[i]) break; |
sashida_h | 9:42b4d337d4cc | 355 | i++; |
sashida_h | 9:42b4d337d4cc | 356 | } |
sashida_h | 9:42b4d337d4cc | 357 | if(i > 3){ |
sashida_h | 9:42b4d337d4cc | 358 | i=0; |
sashida_h | 9:42b4d337d4cc | 359 | do{ |
sashida_h | 9:42b4d337d4cc | 360 | d[i]=c[13+i]; |
sashida_h | 9:42b4d337d4cc | 361 | i++; |
sashida_h | 9:42b4d337d4cc | 362 | }while(d[i-1] != ')'); |
sashida_h | 9:42b4d337d4cc | 363 | d[i-1] = '\0'; |
sashida_h | 9:42b4d337d4cc | 364 | gps.printf("d:%s",d); |
sashida_h | 9:42b4d337d4cc | 365 | i = 0; |
sashida_h | 9:42b4d337d4cc | 366 | if(d[0] == 'o') myled = 1; |
sashida_h | 9:42b4d337d4cc | 367 | if(d[0] == 'l') myled = 0; |
sashida_h | 9:42b4d337d4cc | 368 | if(d[0] == 'f'){ |
sashida_h | 9:42b4d337d4cc | 369 | //pc.printf("flight_mode\r\n"); |
sashida_h | 9:42b4d337d4cc | 370 | Phase = LAUNCH; |
sashida_h | 9:42b4d337d4cc | 371 | } |
sashida_h | 9:42b4d337d4cc | 372 | } |
sashida_h | 9:42b4d337d4cc | 373 | |
sashida_h | 9:42b4d337d4cc | 374 | i=0; |
sashida_h | 9:42b4d337d4cc | 375 | memset(c,'\0',64); |
sashida_h | 9:42b4d337d4cc | 376 | memset(d,'\0',8); |
sashida_h | 9:42b4d337d4cc | 377 | //pc.printf("2\r\n"); |
sashida_h | 9:42b4d337d4cc | 378 | return 0; |
sashida_h | 9:42b4d337d4cc | 379 | } |
sashida_h | 7:9953d922499d | 380 | |
Yukina | 5:f6e956e8a060 | 381 | void _SendGPS(){ |
sashida_h | 7:9953d922499d | 382 | char gps_data[256]; |
sashida_h | 7:9953d922499d | 383 | while(1){ |
sashida_h | 7:9953d922499d | 384 | if(gps.readable()){ |
sashida_h | 7:9953d922499d | 385 | gps_data[cnt_gps] = gps.getc(); |
sashida_h | 7:9953d922499d | 386 | if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ |
sashida_h | 7:9953d922499d | 387 | cnt_gps = 0; |
sashida_h | 7:9953d922499d | 388 | memset(gps_data,'\0',256); |
sashida_h | 7:9953d922499d | 389 | }else if(gps_data[cnt_gps] == '\r'){ |
sashida_h | 7:9953d922499d | 390 | float world_time, lon_east, lat_north; |
sashida_h | 7:9953d922499d | 391 | int rlock, sat_num; |
sashida_h | 7:9953d922499d | 392 | char lat,lon; |
sashida_h | 7:9953d922499d | 393 | if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){ |
sashida_h | 7:9953d922499d | 394 | if(rlock==1){ |
sashida_h | 7:9953d922499d | 395 | lat_north = _DMS2DEG(lat_north); |
sashida_h | 7:9953d922499d | 396 | lon_east = _DMS2DEG(lon_east); |
sashida_h | 7:9953d922499d | 397 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 7:9953d922499d | 398 | //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); |
sashida_h | 7:9953d922499d | 399 | int japan_time = int(world_time) - 9; |
sashida_h | 9:42b4d337d4cc | 400 | pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max); |
sashida_h | 7:9953d922499d | 401 | break; |
sashida_h | 7:9953d922499d | 402 | }else{ |
sashida_h | 7:9953d922499d | 403 | //pc.printf("%s\r\n",gps_data); |
sashida_h | 7:9953d922499d | 404 | pc.printf("NoGPSSignal\r\n"); |
sashida_h | 7:9953d922499d | 405 | break; |
sashida_h | 7:9953d922499d | 406 | } |
Yukina | 5:f6e956e8a060 | 407 | }else{ |
sashida_h | 7:9953d922499d | 408 | //ffpc.printf("No_Satellite_signal\r\n"); |
Yukina | 5:f6e956e8a060 | 409 | } |
sashida_h | 7:9953d922499d | 410 | }else{ |
sashida_h | 7:9953d922499d | 411 | cnt_gps++; |
Yukina | 5:f6e956e8a060 | 412 | } |
Yukina | 5:f6e956e8a060 | 413 | } |
Yukina | 4:4b3ae90ec778 | 414 | } |
Yukina | 4:4b3ae90ec778 | 415 | |
sashida_h | 7:9953d922499d | 416 | } |