Haruki Sashida / Mbed 2 deprecated 201903_pf_lower_judgment

201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意

Dependencies:   mbed Madgwick MPU6050 Kalman BMP180

main.cpp@7:9953d922499d, 2019-03-04 (annotated)

Committer:
sashida_h
Date:
Mon Mar 04 02:50:16 2019 +0000
Revision:
7:9953d922499d
Parent:
6:f17310205c1f
Child:
8:15a1b22df82f
for longrun

Who changed what in which revision?

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