pyonta cansat / Mbed 2 deprecated FM_integration

k

Dependencies:   Hm MPL MPU60580 MU2Class SDFileSystem mbed

main.cpp@0:6ddf1386e71d, 2017-09-11 (annotated)

Committer:
pyonta2017
Date:
Mon Sep 11 10:05:39 2017 +0000
Revision:
0:6ddf1386e71d
Child:
1:8a25883c423c
koreyade

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyonta2017 0:6ddf1386e71d 1 #include "mbed.h"
pyonta2017 0:6ddf1386e71d 2 #include "xprintf.h"
pyonta2017 0:6ddf1386e71d 3 #include "MPL3115A2.h"
pyonta2017 0:6ddf1386e71d 4 #include "HMC5883L.h"
pyonta2017 0:6ddf1386e71d 5 #include "MPU6050.h"
pyonta2017 0:6ddf1386e71d 6 #include "SDFileSystem.h"
pyonta2017 0:6ddf1386e71d 7 #include "EthernetPowerControl.h"
pyonta2017 0:6ddf1386e71d 8 #define M_PI 3.141592
pyonta2017 0:6ddf1386e71d 9
pyonta2017 0:6ddf1386e71d 10 //hmc calibration mode
pyonta2017 0:6ddf1386e71d 11 #define STOP 0 //initial
pyonta2017 0:6ddf1386e71d 12 #define CAL 1 //calibration
pyonta2017 0:6ddf1386e71d 13 #define RUN 2 //run
pyonta2017 0:6ddf1386e71d 14
pyonta2017 0:6ddf1386e71d 15 //Sequence phase
pyonta2017 0:6ddf1386e71d 16 #define Preparing 1
pyonta2017 0:6ddf1386e71d 17 #define Rising 2
pyonta2017 0:6ddf1386e71d 18 #define Descending 3
pyonta2017 0:6ddf1386e71d 19 #define Landing_Fusing 4
pyonta2017 0:6ddf1386e71d 20 #define Moving 5
pyonta2017 0:6ddf1386e71d 21
pyonta2017 0:6ddf1386e71d 22 //Moving step
pyonta2017 0:6ddf1386e71d 23 #define get_goaldirection 0
pyonta2017 0:6ddf1386e71d 24 #define attitude_determination 1
pyonta2017 0:6ddf1386e71d 25 #define calculate_direction 2
pyonta2017 0:6ddf1386e71d 26 #define direction_control 3
pyonta2017 0:6ddf1386e71d 27 #define jump 4
pyonta2017 0:6ddf1386e71d 28
pyonta2017 0:6ddf1386e71d 29 DigitalOut myled1(LED1);
pyonta2017 0:6ddf1386e71d 30 DigitalOut myled2(LED2);
pyonta2017 0:6ddf1386e71d 31 DigitalOut myled3(LED3);
pyonta2017 0:6ddf1386e71d 32 DigitalOut myled4(LED4);
pyonta2017 0:6ddf1386e71d 33
pyonta2017 0:6ddf1386e71d 34 //ピンアサイン確認
pyonta2017 0:6ddf1386e71d 35 Serial gps(p13, p14);
pyonta2017 0:6ddf1386e71d 36 Serial Mu2(p28, p27);
pyonta2017 0:6ddf1386e71d 37 I2C i2c(p9, p10); // sda, scl
pyonta2017 0:6ddf1386e71d 38 Serial mp(USBTX, USBRX);
pyonta2017 0:6ddf1386e71d 39 SDFileSystem sd(p5, p6, p7, p8, "sd");
pyonta2017 0:6ddf1386e71d 40 LocalFileSystem local("local");
pyonta2017 0:6ddf1386e71d 41 DigitalOut controlmotor1(p22),controlmotor2(p23);//方向制御モーター
pyonta2017 0:6ddf1386e71d 42 PwmOut pin21(p21);
pyonta2017 0:6ddf1386e71d 43 DigitalOut jumpmotor(p24);
pyonta2017 0:6ddf1386e71d 44
pyonta2017 0:6ddf1386e71d 45 //溶断
pyonta2017 0:6ddf1386e71d 46 DigitalOut para(p18);
pyonta2017 0:6ddf1386e71d 47 DigitalOut marker(p19);
pyonta2017 0:6ddf1386e71d 48 DigitalOut jumparm(p20);
pyonta2017 0:6ddf1386e71d 49 //
pyonta2017 0:6ddf1386e71d 50 DigitalOut Flightpin(p26);
pyonta2017 0:6ddf1386e71d 51
pyonta2017 0:6ddf1386e71d 52 Timer timer;
pyonta2017 0:6ddf1386e71d 53 int val;
pyonta2017 0:6ddf1386e71d 54 int val_total;
pyonta2017 0:6ddf1386e71d 55 unsigned char timer_set;
pyonta2017 0:6ddf1386e71d 56 unsigned char phase;
pyonta2017 0:6ddf1386e71d 57 unsigned char step;
pyonta2017 0:6ddf1386e71d 58
pyonta2017 0:6ddf1386e71d 59 //気圧///////
pyonta2017 0:6ddf1386e71d 60 MPL3115A2 APsensor(&i2c, &mp);
pyonta2017 0:6ddf1386e71d 61 int alt;
pyonta2017 0:6ddf1386e71d 62 int ini_alt;
pyonta2017 0:6ddf1386e71d 63 int min_alt;
pyonta2017 0:6ddf1386e71d 64 int min_alt5;
pyonta2017 0:6ddf1386e71d 65 int max_alt;
pyonta2017 0:6ddf1386e71d 66 int rela_max;
pyonta2017 0:6ddf1386e71d 67 int rela_ini;
pyonta2017 0:6ddf1386e71d 68 int flag1;
pyonta2017 0:6ddf1386e71d 69 int flag2;
pyonta2017 0:6ddf1386e71d 70 int flag3;
pyonta2017 0:6ddf1386e71d 71 unsigned char alttimes;
pyonta2017 0:6ddf1386e71d 72 void getAltitude(){
pyonta2017 0:6ddf1386e71d 73 //wait_ms(300);
pyonta2017 0:6ddf1386e71d 74 Altitude a;
pyonta2017 0:6ddf1386e71d 75 Temperature t;
pyonta2017 0:6ddf1386e71d 76 Pressure p;
pyonta2017 0:6ddf1386e71d 77 APsensor.readAltitude(&a);
pyonta2017 0:6ddf1386e71d 78 APsensor.readTemperature(&t);
pyonta2017 0:6ddf1386e71d 79 APsensor.setModeStandby();
pyonta2017 0:6ddf1386e71d 80 APsensor.setModeBarometer();
pyonta2017 0:6ddf1386e71d 81 APsensor.setModeActive();
pyonta2017 0:6ddf1386e71d 82 APsensor.readPressure(&p);
pyonta2017 0:6ddf1386e71d 83 alt = a;
pyonta2017 0:6ddf1386e71d 84 //気球試験用
pyonta2017 0:6ddf1386e71d 85 if(phase == Preparing){
pyonta2017 0:6ddf1386e71d 86 rela_ini = alt - ini_alt;
pyonta2017 0:6ddf1386e71d 87 if(rela_ini > 50)flag1 = flag1+1;
pyonta2017 0:6ddf1386e71d 88 }
pyonta2017 0:6ddf1386e71d 89
pyonta2017 0:6ddf1386e71d 90 if(phase == Rising){
pyonta2017 0:6ddf1386e71d 91 if(max_alt < alt){
pyonta2017 0:6ddf1386e71d 92 max_alt = alt;
pyonta2017 0:6ddf1386e71d 93 }
pyonta2017 0:6ddf1386e71d 94 else{
pyonta2017 0:6ddf1386e71d 95 rela_max = max_alt -alt;
pyonta2017 0:6ddf1386e71d 96 if(rela_max > 50)flag2 = flag2+1;
pyonta2017 0:6ddf1386e71d 97 }
pyonta2017 0:6ddf1386e71d 98 }
pyonta2017 0:6ddf1386e71d 99 //着地判定
pyonta2017 0:6ddf1386e71d 100 if(phase == Descending){
pyonta2017 0:6ddf1386e71d 101 if (min_alt > alt){
pyonta2017 0:6ddf1386e71d 102 min_alt = alt;
pyonta2017 0:6ddf1386e71d 103 min_alt5 = min_alt + 5;
pyonta2017 0:6ddf1386e71d 104 flag3 -= 1;
pyonta2017 0:6ddf1386e71d 105 }
pyonta2017 0:6ddf1386e71d 106 else {
pyonta2017 0:6ddf1386e71d 107 if( alt < min_alt5){
pyonta2017 0:6ddf1386e71d 108 flag3 +=2;
pyonta2017 0:6ddf1386e71d 109 }
pyonta2017 0:6ddf1386e71d 110 }
pyonta2017 0:6ddf1386e71d 111 }
pyonta2017 0:6ddf1386e71d 112 mp.printf("alt:%d,max:%d,ini:%d,phase:%d\r\n",alt,max_alt,ini_alt,phase);
pyonta2017 0:6ddf1386e71d 113 mp.printf("rela_ini:%d,rela_max:%d\r\n",rela_ini,rela_max);
pyonta2017 0:6ddf1386e71d 114 mp.printf("flag1:%d,flag2:%d,flag3:%d\r\n",flag1,flag2,flag3);
pyonta2017 0:6ddf1386e71d 115 //mp.printf("OFF_H: 0x%X, OFF_T: 0x%X, OFF_P: 0x%X\r\n", APsensor.offsetAltitude(), APsensor.offsetTemperature(), APsensor.offsetPressure());
pyonta2017 0:6ddf1386e71d 116 APsensor.setModeStandby();
pyonta2017 0:6ddf1386e71d 117 APsensor.setModeAltimeter();
pyonta2017 0:6ddf1386e71d 118 APsensor.setModeActive();
pyonta2017 0:6ddf1386e71d 119 }
pyonta2017 0:6ddf1386e71d 120 /////
pyonta2017 0:6ddf1386e71d 121
pyonta2017 0:6ddf1386e71d 122 //GPS
pyonta2017 0:6ddf1386e71d 123 int i,rlock,mode;
pyonta2017 0:6ddf1386e71d 124 char gps_data[256];
pyonta2017 0:6ddf1386e71d 125 char ns,ew;
pyonta2017 0:6ddf1386e71d 126 float w_time,hokui,tokei;
pyonta2017 0:6ddf1386e71d 127 float g_hokui,g_tokei;
pyonta2017 0:6ddf1386e71d 128 float d_hokui,m_hokui,d_tokei,m_tokei;
pyonta2017 0:6ddf1386e71d 129 unsigned char c;
pyonta2017 0:6ddf1386e71d 130 //目標経度，緯度
pyonta2017 0:6ddf1386e71d 131 float goal_tokei,goal_hokui;
pyonta2017 0:6ddf1386e71d 132 float goal_direction;
pyonta2017 0:6ddf1386e71d 133 float direction_y;
pyonta2017 0:6ddf1386e71d 134 float direction_x;
pyonta2017 0:6ddf1386e71d 135 float angular_difference;
pyonta2017 0:6ddf1386e71d 136
pyonta2017 0:6ddf1386e71d 137 void getGPS(){
pyonta2017 0:6ddf1386e71d 138 //目的地 ブラックロック修正
pyonta2017 0:6ddf1386e71d 139 goal_tokei = 118.9833;
pyonta2017 0:6ddf1386e71d 140 goal_hokui = 40.92493;
pyonta2017 0:6ddf1386e71d 141 c = gps.getc();
pyonta2017 0:6ddf1386e71d 142 if( c=='$' || i == 256){
pyonta2017 0:6ddf1386e71d 143 mode = 0;
pyonta2017 0:6ddf1386e71d 144 i = 0;
pyonta2017 0:6ddf1386e71d 145 for(int j=0; j<256; j++){
pyonta2017 0:6ddf1386e71d 146 gps_data[j]=NULL;
pyonta2017 0:6ddf1386e71d 147 }
pyonta2017 0:6ddf1386e71d 148 }
pyonta2017 0:6ddf1386e71d 149 if(mode==0){
pyonta2017 0:6ddf1386e71d 150 if((gps_data[i]=c) != '\r'){
pyonta2017 0:6ddf1386e71d 151 i++;
pyonta2017 0:6ddf1386e71d 152 }else{
pyonta2017 0:6ddf1386e71d 153 gps_data[i]='\0';
pyonta2017 0:6ddf1386e71d 154
pyonta2017 0:6ddf1386e71d 155 if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
pyonta2017 0:6ddf1386e71d 156 if(rlock==1){
pyonta2017 0:6ddf1386e71d 157 //mp.printf("Status:Lock(%d)\n\r",rlock);
pyonta2017 0:6ddf1386e71d 158 //logitude
pyonta2017 0:6ddf1386e71d 159 d_tokei= int(tokei/100);
pyonta2017 0:6ddf1386e71d 160 m_tokei= (tokei-d_tokei*100)/60;
pyonta2017 0:6ddf1386e71d 161 g_tokei= d_tokei+m_tokei;
pyonta2017 0:6ddf1386e71d 162 //Latitude
pyonta2017 0:6ddf1386e71d 163 d_hokui=int(hokui/100);
pyonta2017 0:6ddf1386e71d 164 m_hokui=(hokui-d_hokui*100)/60;
pyonta2017 0:6ddf1386e71d 165 g_hokui=d_hokui+m_hokui;
pyonta2017 0:6ddf1386e71d 166 mp.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui);
pyonta2017 0:6ddf1386e71d 167 Mu2.printf("@DT14%f,%f\r\n",g_tokei, g_hokui);
pyonta2017 0:6ddf1386e71d 168 float longitudinalDifference =-(goal_tokei - g_tokei);
pyonta2017 0:6ddf1386e71d 169 float latitudinalDifference = goal_hokui - g_hokui;
pyonta2017 0:6ddf1386e71d 170
pyonta2017 0:6ddf1386e71d 171 //球面三角法
pyonta2017 0:6ddf1386e71d 172 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
pyonta2017 0:6ddf1386e71d 173 direction_x = cos(g_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(g_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
pyonta2017 0:6ddf1386e71d 174 goal_direction = atan2f(direction_y,direction_x);
pyonta2017 0:6ddf1386e71d 175 if(goal_direction < 0)goal_direction += 2*M_PI;
pyonta2017 0:6ddf1386e71d 176
pyonta2017 0:6ddf1386e71d 177 //磁北に対する角度に調節 ネバダ東に14度
pyonta2017 0:6ddf1386e71d 178 goal_direction -= 0.244346;
pyonta2017 0:6ddf1386e71d 179 if (goal_direction < 0) goal_direction += 2*M_PI;
pyonta2017 0:6ddf1386e71d 180 mp.printf("goal:%f\n\r",goal_direction*180/M_PI);
pyonta2017 0:6ddf1386e71d 181 //mp.printf("longitudinalDifference:%f",longitudinalDifference);
pyonta2017 0:6ddf1386e71d 182 //mp.printf("latitudinalDifference:%f",latitudinalDifference);
pyonta2017 0:6ddf1386e71d 183 if(phase == Descending)getAltitude();
pyonta2017 0:6ddf1386e71d 184 if(phase == Moving)step = attitude_determination;
pyonta2017 0:6ddf1386e71d 185 }
pyonta2017 0:6ddf1386e71d 186 else{
pyonta2017 0:6ddf1386e71d 187 mp.printf("\n\rStatus:unLock(%d)\n\r",rlock);
pyonta2017 0:6ddf1386e71d 188 mp.printf("%s",gps_data);
pyonta2017 0:6ddf1386e71d 189 Mu2.printf("@DT02No\r\n");
pyonta2017 0:6ddf1386e71d 190 if(phase == Descending)getAltitude();
pyonta2017 0:6ddf1386e71d 191 //if(phase == Moving)test_get_direction();
pyonta2017 0:6ddf1386e71d 192 }
pyonta2017 0:6ddf1386e71d 193 sprintf(gps_data, "");
pyonta2017 0:6ddf1386e71d 194 }
pyonta2017 0:6ddf1386e71d 195 }
pyonta2017 0:6ddf1386e71d 196 }
pyonta2017 0:6ddf1386e71d 197 }
pyonta2017 0:6ddf1386e71d 198
pyonta2017 0:6ddf1386e71d 199 void MU2initialize(){
pyonta2017 0:6ddf1386e71d 200 Mu2.baud(19200);
pyonta2017 0:6ddf1386e71d 201 Mu2.printf("@EI34\r\n");
pyonta2017 0:6ddf1386e71d 202 wait(1);
pyonta2017 0:6ddf1386e71d 203 Mu2.printf("@DI25\r\n");
pyonta2017 0:6ddf1386e71d 204 wait(1);
pyonta2017 0:6ddf1386e71d 205 Mu2.printf("@CH0F\r\n");
pyonta2017 0:6ddf1386e71d 206 wait(1);
pyonta2017 0:6ddf1386e71d 207 Mu2.printf("@GI34\r\n");
pyonta2017 0:6ddf1386e71d 208 wait(1);
pyonta2017 0:6ddf1386e71d 209 }
pyonta2017 0:6ddf1386e71d 210
pyonta2017 0:6ddf1386e71d 211 /////////////////////
pyonta2017 0:6ddf1386e71d 212 //地磁気
pyonta2017 0:6ddf1386e71d 213 HMC5883L compass(p9, p10);
pyonta2017 0:6ddf1386e71d 214 Ticker interrupt;
pyonta2017 0:6ddf1386e71d 215 double heading0 = 0.0;
pyonta2017 0:6ddf1386e71d 216 double heading1 = 0.0;
pyonta2017 0:6ddf1386e71d 217 double heading2 = 0.0;
pyonta2017 0:6ddf1386e71d 218 double heading3 = 0.0;
pyonta2017 0:6ddf1386e71d 219 double headingLPF = 0.0;
pyonta2017 0:6ddf1386e71d 220 double initHeading;
pyonta2017 0:6ddf1386e71d 221 double tgtHeading;
pyonta2017 0:6ddf1386e71d 222 double preHeading = 0.0;
pyonta2017 0:6ddf1386e71d 223 unsigned char hmc_mode;
pyonta2017 0:6ddf1386e71d 224
pyonta2017 0:6ddf1386e71d 225 int maxX, minX, maxY, minY;
pyonta2017 0:6ddf1386e71d 226 int ofsX = 0; //calibration x
pyonta2017 0:6ddf1386e71d 227 int ofsY = 0; //calibration y
pyonta2017 0:6ddf1386e71d 228 int counter = 0;
pyonta2017 0:6ddf1386e71d 229 int ofsXset = 1122;
pyonta2017 0:6ddf1386e71d 230 int ofsYset = -466;
pyonta2017 0:6ddf1386e71d 231 int ofsZset = -522;
pyonta2017 0:6ddf1386e71d 232
pyonta2017 0:6ddf1386e71d 233 //地磁気キャリブレーション用
pyonta2017 0:6ddf1386e71d 234 void intrpt() {
pyonta2017 0:6ddf1386e71d 235 int16_t raw[3];
pyonta2017 0:6ddf1386e71d 236 compass.getXYZ(raw);
pyonta2017 0:6ddf1386e71d 237 //double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
pyonta2017 0:6ddf1386e71d 238 double heading = atan2(static_cast<double>(raw[2]-ofsYset), static_cast<double>(raw[0]-ofsXset));
pyonta2017 0:6ddf1386e71d 239 if(heading < 0)heading += 2*M_PI;
pyonta2017 0:6ddf1386e71d 240 if(heading > 2*M_PI)heading -= 2*M_PI;
pyonta2017 0:6ddf1386e71d 241 heading3 = heading2;
pyonta2017 0:6ddf1386e71d 242 heading2 = heading1;
pyonta2017 0:6ddf1386e71d 243 heading1 = heading0;
pyonta2017 0:6ddf1386e71d 244 heading0 = heading;
pyonta2017 0:6ddf1386e71d 245 headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
pyonta2017 0:6ddf1386e71d 246
pyonta2017 0:6ddf1386e71d 247 switch(hmc_mode) {
pyonta2017 0:6ddf1386e71d 248 case STOP:
pyonta2017 0:6ddf1386e71d 249 if(counter == 100) {
pyonta2017 0:6ddf1386e71d 250 initHeading = headingLPF;
pyonta2017 0:6ddf1386e71d 251 hmc_mode = CAL;
pyonta2017 0:6ddf1386e71d 252 maxX = raw[0];
pyonta2017 0:6ddf1386e71d 253 minX = raw[0];
pyonta2017 0:6ddf1386e71d 254 maxY = raw[2];
pyonta2017 0:6ddf1386e71d 255 minY = raw[2];
pyonta2017 0:6ddf1386e71d 256 counter = 0;
pyonta2017 0:6ddf1386e71d 257 //mp.printf("STOP end\n\r");
pyonta2017 0:6ddf1386e71d 258 }
pyonta2017 0:6ddf1386e71d 259 break;
pyonta2017 0:6ddf1386e71d 260 case CAL:
pyonta2017 0:6ddf1386e71d 261 if(raw[0] > maxX)maxX = raw[0];
pyonta2017 0:6ddf1386e71d 262 if(raw[0] < minX)minX = raw[0];
pyonta2017 0:6ddf1386e71d 263 if(raw[2] > maxY)maxY = raw[2];
pyonta2017 0:6ddf1386e71d 264 if(raw[2] < minY)minY = raw[2];
pyonta2017 0:6ddf1386e71d 265 if((counter > 100)
pyonta2017 0:6ddf1386e71d 266 && (headingLPF > (initHeading-0.01))
pyonta2017 0:6ddf1386e71d 267 && (headingLPF < (initHeading+0.01))) {
pyonta2017 0:6ddf1386e71d 268 ofsX = (maxX + minX)/2;
pyonta2017 0:6ddf1386e71d 269 ofsY = (maxY + minY)/2;
pyonta2017 0:6ddf1386e71d 270 //hmc_mode = RUN;
pyonta2017 0:6ddf1386e71d 271 counter = 0;
pyonta2017 0:6ddf1386e71d 272 //mp.printf("ofs=%d,%d\n\r",ofsX,ofsY);
pyonta2017 0:6ddf1386e71d 273 }
pyonta2017 0:6ddf1386e71d 274 break;
pyonta2017 0:6ddf1386e71d 275 case RUN:
pyonta2017 0:6ddf1386e71d 276 mp.printf("heading=%f,x=%d,y=%d\n\r",headingLPF*180/M_PI,raw[0]-ofsXset,raw[2]-ofsYset);
pyonta2017 0:6ddf1386e71d 277 //mp.printf("%d,%d,%d\r\n",raw[0],raw[2],raw[1]);
pyonta2017 0:6ddf1386e71d 278 //step = calculate_direction;
pyonta2017 0:6ddf1386e71d 279 break;
pyonta2017 0:6ddf1386e71d 280
pyonta2017 0:6ddf1386e71d 281 }
pyonta2017 0:6ddf1386e71d 282 counter++;
pyonta2017 0:6ddf1386e71d 283 }
pyonta2017 0:6ddf1386e71d 284
pyonta2017 0:6ddf1386e71d 285 //加速度
pyonta2017 0:6ddf1386e71d 286 MPU6050 mpu;
pyonta2017 0:6ddf1386e71d 287 int16_t ax, ay, az;
pyonta2017 0:6ddf1386e71d 288 int16_t gx, gy, gz;
pyonta2017 0:6ddf1386e71d 289 int32_t axa, aya, aza;
pyonta2017 0:6ddf1386e71d 290 int32_t gxa, gya, gza;
pyonta2017 0:6ddf1386e71d 291
pyonta2017 0:6ddf1386e71d 292 //オフセット値
pyonta2017 0:6ddf1386e71d 293 double axoffset = 586.711;
pyonta2017 0:6ddf1386e71d 294 double ayoffset = -174.847;
pyonta2017 0:6ddf1386e71d 295 double azoffset = -2195.34375;
pyonta2017 0:6ddf1386e71d 296
pyonta2017 0:6ddf1386e71d 297 //センサの出力あたりの加速度[m/s^2]
pyonta2017 0:6ddf1386e71d 298 double axrate = 0.000601615;
pyonta2017 0:6ddf1386e71d 299 double ayrate = 0.000598334;
pyonta2017 0:6ddf1386e71d 300 double azrate = 0.0005845;
pyonta2017 0:6ddf1386e71d 301
pyonta2017 0:6ddf1386e71d 302 //3軸キャリブレーション
pyonta2017 0:6ddf1386e71d 303 double phi;
pyonta2017 0:6ddf1386e71d 304 double theta;
pyonta2017 0:6ddf1386e71d 305 double heading3axis;
pyonta2017 0:6ddf1386e71d 306
pyonta2017 0:6ddf1386e71d 307 double bfy;
pyonta2017 0:6ddf1386e71d 308 double bfx;
pyonta2017 0:6ddf1386e71d 309
pyonta2017 0:6ddf1386e71d 310 //3軸からheading算出
pyonta2017 0:6ddf1386e71d 311 void getheading3axis() {
pyonta2017 0:6ddf1386e71d 312 //加速度算出
pyonta2017 0:6ddf1386e71d 313 mpu.getAcceleration(&ax, &ay, &az);
pyonta2017 0:6ddf1386e71d 314 double axcal = -axrate * (static_cast<double>(ax) - axoffset);
pyonta2017 0:6ddf1386e71d 315 double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
pyonta2017 0:6ddf1386e71d 316 double azcal = -azrate * (static_cast<double>(az) - azoffset);
pyonta2017 0:6ddf1386e71d 317
pyonta2017 0:6ddf1386e71d 318 phi = atan2(aycal,azcal);
pyonta2017 0:6ddf1386e71d 319 //phi += M_PI;
pyonta2017 0:6ddf1386e71d 320 theta = atan2(-axcal,aycal*sin(phi)+azcal*cos(phi));
pyonta2017 0:6ddf1386e71d 321 //theta += M_PI;
pyonta2017 0:6ddf1386e71d 322
pyonta2017 0:6ddf1386e71d 323 //地磁気算出
pyonta2017 0:6ddf1386e71d 324 int16_t raw[3];
pyonta2017 0:6ddf1386e71d 325 compass.getXYZ(raw);
pyonta2017 0:6ddf1386e71d 326 bfy = -(static_cast<double>(raw[1]-ofsZset))*sin(phi) + (static_cast<double>(raw[2]-ofsYset))*cos(phi);
pyonta2017 0:6ddf1386e71d 327 bfx = (static_cast<double>(raw[0]-ofsXset))*cos(theta)
pyonta2017 0:6ddf1386e71d 328 + (static_cast<double>(raw[2]-ofsYset))*sin(theta)*sin(phi)
pyonta2017 0:6ddf1386e71d 329 + (static_cast<double>(raw[1]-ofsZset))*sin(theta)*cos(phi);
pyonta2017 0:6ddf1386e71d 330 double heading = atan2(-bfy, bfx);
pyonta2017 0:6ddf1386e71d 331 if(heading < 0)heading += 2*M_PI;
pyonta2017 0:6ddf1386e71d 332 if(heading > 2*M_PI)heading -= 2*M_PI;
pyonta2017 0:6ddf1386e71d 333 heading3 = heading2;
pyonta2017 0:6ddf1386e71d 334 heading2 = heading1;
pyonta2017 0:6ddf1386e71d 335 heading1 = heading0;
pyonta2017 0:6ddf1386e71d 336 heading0 = heading;
pyonta2017 0:6ddf1386e71d 337 headingLPF = (heading0 + heading1 + heading2 + heading3)/4;//low pass filter
pyonta2017 0:6ddf1386e71d 338 //headingLPF += 0.349066;
pyonta2017 0:6ddf1386e71d 339 if(headingLPF > 2*M_PI) headingLPF -= 2*M_PI;
pyonta2017 0:6ddf1386e71d 340 //mp.printf("ax:%f,ay:%f,az:%f\r\n",axcal,aycal,azcal);
pyonta2017 0:6ddf1386e71d 341 mp.printf("heading:%f,phi:%f,theta:%f\r\n",headingLPF*180/M_PI,phi*180/M_PI,theta*180/M_PI);
pyonta2017 0:6ddf1386e71d 342 }
pyonta2017 0:6ddf1386e71d 343
pyonta2017 0:6ddf1386e71d 344 //ADXL375
pyonta2017 0:6ddf1386e71d 345 const int addr = 0xA6;
pyonta2017 0:6ddf1386e71d 346 char adxl[6];
pyonta2017 0:6ddf1386e71d 347 short int axout = 0;
pyonta2017 0:6ddf1386e71d 348 short int ayout = 0;
pyonta2017 0:6ddf1386e71d 349 short int azout = 0;
pyonta2017 0:6ddf1386e71d 350
pyonta2017 0:6ddf1386e71d 351 void adxl_init(){
pyonta2017 0:6ddf1386e71d 352 char fifo[2] = {0x38,0x80};
pyonta2017 0:6ddf1386e71d 353 i2c.write(addr,fifo,2); //FIFO_Mode -> Stream
pyonta2017 0:6ddf1386e71d 354 char power[2] = {0x2D,0x08};
pyonta2017 0:6ddf1386e71d 355 i2c.write(addr,power,2); //measurebit -> 1
pyonta2017 0:6ddf1386e71d 356 }
pyonta2017 0:6ddf1386e71d 357
pyonta2017 0:6ddf1386e71d 358 void adxl_get(){
pyonta2017 0:6ddf1386e71d 359 adxl[0] = 0x32;
pyonta2017 0:6ddf1386e71d 360 i2c.write(addr,adxl,1);
pyonta2017 0:6ddf1386e71d 361 i2c.read(0xA7,adxl,6);
pyonta2017 0:6ddf1386e71d 362 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:6ddf1386e71d 363 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:6ddf1386e71d 364 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:6ddf1386e71d 365 }
pyonta2017 0:6ddf1386e71d 366
pyonta2017 0:6ddf1386e71d 367
pyonta2017 0:6ddf1386e71d 368 //モーター駆動方向制御
pyonta2017 0:6ddf1386e71d 369 void motor_drive(){
pyonta2017 0:6ddf1386e71d 370 controlmotor1.write(1);
pyonta2017 0:6ddf1386e71d 371 controlmotor2.write(0);
pyonta2017 0:6ddf1386e71d 372 pin21.write(0.8);
pyonta2017 0:6ddf1386e71d 373 //if(pin1)mp.printf("Yes!\r\n");
pyonta2017 0:6ddf1386e71d 374 }
pyonta2017 0:6ddf1386e71d 375
pyonta2017 0:6ddf1386e71d 376 //溶断プログラム
pyonta2017 0:6ddf1386e71d 377 void burning(){
pyonta2017 0:6ddf1386e71d 378 wait(5);
pyonta2017 0:6ddf1386e71d 379 myled1 = 1;
pyonta2017 0:6ddf1386e71d 380 para = 1;
pyonta2017 0:6ddf1386e71d 381 wait(1);
pyonta2017 0:6ddf1386e71d 382 myled1 = 0;
pyonta2017 0:6ddf1386e71d 383 para = 0;
pyonta2017 0:6ddf1386e71d 384 wait(5);
pyonta2017 0:6ddf1386e71d 385 myled2 = 1;
pyonta2017 0:6ddf1386e71d 386 marker = 1;
pyonta2017 0:6ddf1386e71d 387 wait(1);
pyonta2017 0:6ddf1386e71d 388 myled2 = 0;
pyonta2017 0:6ddf1386e71d 389 marker = 0;
pyonta2017 0:6ddf1386e71d 390 wait(5);
pyonta2017 0:6ddf1386e71d 391 myled3 = 1;
pyonta2017 0:6ddf1386e71d 392 jumparm = 1;
pyonta2017 0:6ddf1386e71d 393 wait(1);
pyonta2017 0:6ddf1386e71d 394 myled3 = 0;
pyonta2017 0:6ddf1386e71d 395 jumparm = 0;
pyonta2017 0:6ddf1386e71d 396 }
pyonta2017 0:6ddf1386e71d 397 //跳躍モーター駆動まいまいかわいい
pyonta2017 0:6ddf1386e71d 398 void jumping(){
pyonta2017 0:6ddf1386e71d 399 timer.start();
pyonta2017 0:6ddf1386e71d 400 //モーター駆動時間調整
pyonta2017 0:6ddf1386e71d 401 while(val < 4){
pyonta2017 0:6ddf1386e71d 402 val = timer.read();
pyonta2017 0:6ddf1386e71d 403 jumpmotor = 1;
pyonta2017 0:6ddf1386e71d 404 myled4 != myled4;
pyonta2017 0:6ddf1386e71d 405 }
pyonta2017 0:6ddf1386e71d 406 timer.reset();
pyonta2017 0:6ddf1386e71d 407 timer.stop();
pyonta2017 0:6ddf1386e71d 408 jumpmotor = 0;
pyonta2017 0:6ddf1386e71d 409 val = 0;
pyonta2017 0:6ddf1386e71d 410 }
pyonta2017 0:6ddf1386e71d 411
pyonta2017 0:6ddf1386e71d 412 ///test
pyonta2017 0:6ddf1386e71d 413 //GPSなし方向制御，跳躍テスト
pyonta2017 0:6ddf1386e71d 414 float test_tokei,test_hokui;
pyonta2017 0:6ddf1386e71d 415 void test_get_direction(){
pyonta2017 0:6ddf1386e71d 416 //目的地 大岡山駅
pyonta2017 0:6ddf1386e71d 417 goal_tokei = 139.686234;
pyonta2017 0:6ddf1386e71d 418 goal_hokui = 35.614330;
pyonta2017 0:6ddf1386e71d 419
pyonta2017 0:6ddf1386e71d 420 //テスト場所石川台一号館
pyonta2017 0:6ddf1386e71d 421 test_tokei = 139.681529;
pyonta2017 0:6ddf1386e71d 422 test_hokui = 35.605263;
pyonta2017 0:6ddf1386e71d 423
pyonta2017 0:6ddf1386e71d 424 float longitudinalDifference = goal_tokei - test_tokei;
pyonta2017 0:6ddf1386e71d 425 float latitudinalDifference = goal_hokui - test_hokui;
pyonta2017 0:6ddf1386e71d 426 //球面三角法
pyonta2017 0:6ddf1386e71d 427 direction_y = cos(goal_hokui*M_PI/180)*sin(longitudinalDifference*M_PI/180);
pyonta2017 0:6ddf1386e71d 428 direction_x = cos(test_hokui*M_PI/180)*sin(goal_hokui*M_PI/180) - sin(test_hokui*M_PI/180)*cos(goal_hokui*M_PI/180)*cos(longitudinalDifference*M_PI/180);
pyonta2017 0:6ddf1386e71d 429 goal_direction = atan2f(direction_y,direction_x);
pyonta2017 0:6ddf1386e71d 430 if(goal_direction < 0)goal_direction += 2*M_PI;
pyonta2017 0:6ddf1386e71d 431
pyonta2017 0:6ddf1386e71d 432 //磁北に対する角度に調節 東京7度
pyonta2017 0:6ddf1386e71d 433 goal_direction += 0.122173;
pyonta2017 0:6ddf1386e71d 434 //goal_direction2 += 0.122173;
pyonta2017 0:6ddf1386e71d 435 if (goal_direction < 0) goal_direction += 2*M_PI;
pyonta2017 0:6ddf1386e71d 436 }
pyonta2017 0:6ddf1386e71d 437
pyonta2017 0:6ddf1386e71d 438 /////////////////////////main////////////////////////////////////
pyonta2017 0:6ddf1386e71d 439 int main(){
pyonta2017 0:6ddf1386e71d 440 PHY_PowerDown();//省電力
pyonta2017 0:6ddf1386e71d 441 wait(2);
pyonta2017 0:6ddf1386e71d 442 //センサ初期化
pyonta2017 0:6ddf1386e71d 443 Mu2.baud(19200);
pyonta2017 0:6ddf1386e71d 444 Mu2.printf("@EI34\r\n");
pyonta2017 0:6ddf1386e71d 445 wait(1);
pyonta2017 0:6ddf1386e71d 446 Mu2.printf("@DI25\r\n");
pyonta2017 0:6ddf1386e71d 447 wait(1);
pyonta2017 0:6ddf1386e71d 448 Mu2.printf("@CH0F\r\n");
pyonta2017 0:6ddf1386e71d 449 wait(1);
pyonta2017 0:6ddf1386e71d 450 Mu2.printf("@GI34\r\n");
pyonta2017 0:6ddf1386e71d 451 wait(1);
pyonta2017 0:6ddf1386e71d 452 APsensor.init();
pyonta2017 0:6ddf1386e71d 453 wait(0.2);
pyonta2017 0:6ddf1386e71d 454 hmc_mode = RUN;
pyonta2017 0:6ddf1386e71d 455 mpu.initialize();
pyonta2017 0:6ddf1386e71d 456 wait(0.2);
pyonta2017 0:6ddf1386e71d 457 compass.init();
pyonta2017 0:6ddf1386e71d 458 wait(0.2);
pyonta2017 0:6ddf1386e71d 459 adxl_init();
pyonta2017 0:6ddf1386e71d 460 wait(0.2);
pyonta2017 0:6ddf1386e71d 461 APsensor.setOffsetAltitude(100);
pyonta2017 0:6ddf1386e71d 462 APsensor.setOffsetTemperature(10);
pyonta2017 0:6ddf1386e71d 463 APsensor.setOffsetPressure(-32);
pyonta2017 0:6ddf1386e71d 464 mkdir("/sd/mydir", 0777);
pyonta2017 0:6ddf1386e71d 465 min_alt = 4000.0;
pyonta2017 0:6ddf1386e71d 466 Flightpin = 1;
pyonta2017 0:6ddf1386e71d 467 //高度初期値
pyonta2017 0:6ddf1386e71d 468 for(int h = 0; h < 10; h++){
pyonta2017 0:6ddf1386e71d 469 getAltitude();
pyonta2017 0:6ddf1386e71d 470 ini_alt = ini_alt + alt;
pyonta2017 0:6ddf1386e71d 471 wait(0.5);
pyonta2017 0:6ddf1386e71d 472 }
pyonta2017 0:6ddf1386e71d 473 ini_alt = ini_alt/10;
pyonta2017 0:6ddf1386e71d 474 mp.printf("ini_alt:%d\r\n",ini_alt);
pyonta2017 0:6ddf1386e71d 475 int jump_count;
pyonta2017 0:6ddf1386e71d 476 phase = Preparing;
pyonta2017 0:6ddf1386e71d 477 wait(900);//開始15分待機
pyonta2017 0:6ddf1386e71d 478 //phase = Moving;
pyonta2017 0:6ddf1386e71d 479 /////////////////////////////単機能////////
pyonta2017 0:6ddf1386e71d 480
pyonta2017 0:6ddf1386e71d 481 //wait(5);
pyonta2017 0:6ddf1386e71d 482
pyonta2017 0:6ddf1386e71d 483 //while(1){
pyonta2017 0:6ddf1386e71d 484 //myled1 = 1;
pyonta2017 0:6ddf1386e71d 485 //Flightpin = 1;
pyonta2017 0:6ddf1386e71d 486 //GPS_MU2
pyonta2017 0:6ddf1386e71d 487 //getGPS();
pyonta2017 0:6ddf1386e71d 488 //方向制御用モーター
pyonta2017 0:6ddf1386e71d 489 /*
pyonta2017 0:6ddf1386e71d 490 controlmotor1.write(1);
pyonta2017 0:6ddf1386e71d 491 controlmotor2.write(0);
pyonta2017 0:6ddf1386e71d 492 pin21.write(0.8);
pyonta2017 0:6ddf1386e71d 493 wait(3);
pyonta2017 0:6ddf1386e71d 494 controlmotor1.write(0);
pyonta2017 0:6ddf1386e71d 495 controlmotor2.write(0);
pyonta2017 0:6ddf1386e71d 496 pin21.write(0.0);
pyonta2017 0:6ddf1386e71d 497 */
pyonta2017 0:6ddf1386e71d 498 //跳躍用モーター
pyonta2017 0:6ddf1386e71d 499 //jumpmotor = 1;
pyonta2017 0:6ddf1386e71d 500 //wait(5);
pyonta2017 0:6ddf1386e71d 501 //jumpmotor = 0;
pyonta2017 0:6ddf1386e71d 502 //溶断
pyonta2017 0:6ddf1386e71d 503 //burning();
pyonta2017 0:6ddf1386e71d 504 //気圧
pyonta2017 0:6ddf1386e71d 505 //getAltitude();
pyonta2017 0:6ddf1386e71d 506 //mpu
pyonta2017 0:6ddf1386e71d 507 /*
pyonta2017 0:6ddf1386e71d 508 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
pyonta2017 0:6ddf1386e71d 509 double axcal = -axrate * (static_cast<double>(ax) - axoffset);
pyonta2017 0:6ddf1386e71d 510 double aycal = -ayrate * (static_cast<double>(ay) - ayoffset);
pyonta2017 0:6ddf1386e71d 511 double azcal = -azrate * (static_cast<double>(az) - azoffset);
pyonta2017 0:6ddf1386e71d 512 mp.printf("x:%f,y:%f,z:%f",axcal,aycal,azcal);
pyonta2017 0:6ddf1386e71d 513 */
pyonta2017 0:6ddf1386e71d 514 //hmc
pyonta2017 0:6ddf1386e71d 515 /*
pyonta2017 0:6ddf1386e71d 516 hmc_mode = RUN;
pyonta2017 0:6ddf1386e71d 517 intrpt();
pyonta2017 0:6ddf1386e71d 518 wait(0.5);
pyonta2017 0:6ddf1386e71d 519 */
pyonta2017 0:6ddf1386e71d 520 //hmc_mpu
pyonta2017 0:6ddf1386e71d 521 //getheading3axis() ;
pyonta2017 0:6ddf1386e71d 522 //wait(0.3);
pyonta2017 0:6ddf1386e71d 523 //adxl_SDカード 跳躍高さ測定
pyonta2017 0:6ddf1386e71d 524 /*
pyonta2017 0:6ddf1386e71d 525 mkdir("/sd/mydir", 0777);
pyonta2017 0:6ddf1386e71d 526 wait(1);
pyonta2017 0:6ddf1386e71d 527 FILE *fp = fopen("/sd/mydir/height1.txt", "a");
pyonta2017 0:6ddf1386e71d 528 wait(1);
pyonta2017 0:6ddf1386e71d 529 if(fp == NULL) {
pyonta2017 0:6ddf1386e71d 530 error("Could not open file for write\n");
pyonta2017 0:6ddf1386e71d 531 }
pyonta2017 0:6ddf1386e71d 532 timer.reset();
pyonta2017 0:6ddf1386e71d 533 timer.start();
pyonta2017 0:6ddf1386e71d 534 val = 0;
pyonta2017 0:6ddf1386e71d 535 while(val < 5000){
pyonta2017 0:6ddf1386e71d 536 jumpmotor = 1;
pyonta2017 0:6ddf1386e71d 537 val = timer.read_ms();
pyonta2017 0:6ddf1386e71d 538 adxl[0] = 0x32;
pyonta2017 0:6ddf1386e71d 539 i2c.write(addr,adxl,1);
pyonta2017 0:6ddf1386e71d 540 i2c.read(0xA7,adxl,6);
pyonta2017 0:6ddf1386e71d 541 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:6ddf1386e71d 542 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:6ddf1386e71d 543 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:6ddf1386e71d 544 fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:6ddf1386e71d 545 wait_ms(2);
pyonta2017 0:6ddf1386e71d 546 myled1 !=myled1;
pyonta2017 0:6ddf1386e71d 547 }
pyonta2017 0:6ddf1386e71d 548 myled1 = 0;
pyonta2017 0:6ddf1386e71d 549 jumpmotor = 0;
pyonta2017 0:6ddf1386e71d 550 wait(1);
pyonta2017 0:6ddf1386e71d 551 fclose(fp);
pyonta2017 0:6ddf1386e71d 552 */
pyonta2017 0:6ddf1386e71d 553 //
pyonta2017 0:6ddf1386e71d 554
pyonta2017 0:6ddf1386e71d 555 //}
pyonta2017 0:6ddf1386e71d 556 /////////////////////////
pyonta2017 0:6ddf1386e71d 557
pyonta2017 0:6ddf1386e71d 558
pyonta2017 0:6ddf1386e71d 559 /*
pyonta2017 0:6ddf1386e71d 560 //////////////////////////安全試験用/////////
pyonta2017 0:6ddf1386e71d 561 //振動試験
pyonta2017 0:6ddf1386e71d 562 myled1 = 0;
pyonta2017 0:6ddf1386e71d 563 //wait(5);//test
pyonta2017 0:6ddf1386e71d 564 wait(900);//end to end
pyonta2017 0:6ddf1386e71d 565 //静荷重
pyonta2017 0:6ddf1386e71d 566 FILE *sl = fopen("/local/static2.txt", "a");
pyonta2017 0:6ddf1386e71d 567 wait(10);
pyonta2017 0:6ddf1386e71d 568 myled1 = 1;
pyonta2017 0:6ddf1386e71d 569 wait(5);
pyonta2017 0:6ddf1386e71d 570 timer.reset();
pyonta2017 0:6ddf1386e71d 571 timer.start();
pyonta2017 0:6ddf1386e71d 572 //while(val < 30000){ //test
pyonta2017 0:6ddf1386e71d 573 while(val < 600000){ //end to end
pyonta2017 0:6ddf1386e71d 574 val = timer.read_ms();
pyonta2017 0:6ddf1386e71d 575 adxl[0] = 0x32;
pyonta2017 0:6ddf1386e71d 576 i2c.write(addr,adxl,1);
pyonta2017 0:6ddf1386e71d 577 i2c.read(0xA7,adxl,6);
pyonta2017 0:6ddf1386e71d 578 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:6ddf1386e71d 579 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:6ddf1386e71d 580 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:6ddf1386e71d 581 //pc.printf("time:%d,X:%d,Y:%d,Z:%d\r\n",val,axout,ayout,azout);
pyonta2017 0:6ddf1386e71d 582 fprintf(sl, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:6ddf1386e71d 583 wait_ms(200);
pyonta2017 0:6ddf1386e71d 584 //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
pyonta2017 0:6ddf1386e71d 585 }
pyonta2017 0:6ddf1386e71d 586 fclose(sl);
pyonta2017 0:6ddf1386e71d 587 timer.reset();
pyonta2017 0:6ddf1386e71d 588 timer.stop();
pyonta2017 0:6ddf1386e71d 589 val = 0;
pyonta2017 0:6ddf1386e71d 590 myled1 = 0;
pyonta2017 0:6ddf1386e71d 591 //wait(5); //test
pyonta2017 0:6ddf1386e71d 592 wait(960); //end to end
pyonta2017 0:6ddf1386e71d 593
pyonta2017 0:6ddf1386e71d 594 //開傘衝撃
pyonta2017 0:6ddf1386e71d 595 FILE *im = fopen("/local/impact2.txt", "a");
pyonta2017 0:6ddf1386e71d 596 wait(10);
pyonta2017 0:6ddf1386e71d 597 myled2 = 1;
pyonta2017 0:6ddf1386e71d 598 timer.reset();
pyonta2017 0:6ddf1386e71d 599 timer.start();
pyonta2017 0:6ddf1386e71d 600 //while(val < 10000){ //test
pyonta2017 0:6ddf1386e71d 601 while(val < 30000){ //end to end
pyonta2017 0:6ddf1386e71d 602 val = timer.read_ms();
pyonta2017 0:6ddf1386e71d 603 adxl[0] = 0x32;
pyonta2017 0:6ddf1386e71d 604 i2c.write(addr,adxl,1);
pyonta2017 0:6ddf1386e71d 605 i2c.read(0xA7,adxl,6);
pyonta2017 0:6ddf1386e71d 606 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:6ddf1386e71d 607 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:6ddf1386e71d 608 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:6ddf1386e71d 609 fprintf(im, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:6ddf1386e71d 610 wait_ms(3);
pyonta2017 0:6ddf1386e71d 611 //wait必要、ＳＤがbusyの状態で送っちゃってるんじゃない？
pyonta2017 0:6ddf1386e71d 612 }
pyonta2017 0:6ddf1386e71d 613 fclose(im);
pyonta2017 0:6ddf1386e71d 614 timer.reset();
pyonta2017 0:6ddf1386e71d 615 timer.stop();
pyonta2017 0:6ddf1386e71d 616 val = 0;
pyonta2017 0:6ddf1386e71d 617 myled2 = 0;
pyonta2017 0:6ddf1386e71d 618 //wait(30); //test
pyonta2017 0:6ddf1386e71d 619 wait(600); //end to end
pyonta2017 0:6ddf1386e71d 620 ///////////////////////////
pyonta2017 0:6ddf1386e71d 621 */
pyonta2017 0:6ddf1386e71d 622 //着地検知用timer
pyonta2017 0:6ddf1386e71d 623 timer.reset();
pyonta2017 0:6ddf1386e71d 624 timer.start();
pyonta2017 0:6ddf1386e71d 625 val = 0;
pyonta2017 0:6ddf1386e71d 626 while(1){
pyonta2017 0:6ddf1386e71d 627 switch(phase){
pyonta2017 0:6ddf1386e71d 628 case Preparing:
pyonta2017 0:6ddf1386e71d 629 myled1 = 1;
pyonta2017 0:6ddf1386e71d 630 getAltitude();
pyonta2017 0:6ddf1386e71d 631 wait(0.5);
pyonta2017 0:6ddf1386e71d 632 val = timer.read();
pyonta2017 0:6ddf1386e71d 633 if(flag1 > 20) phase = Rising;
pyonta2017 0:6ddf1386e71d 634 if( val > 600) phase = Rising; //計25分
pyonta2017 0:6ddf1386e71d 635 break;
pyonta2017 0:6ddf1386e71d 636 case Rising:
pyonta2017 0:6ddf1386e71d 637 myled1 = 0;
pyonta2017 0:6ddf1386e71d 638 myled2 = 1;
pyonta2017 0:6ddf1386e71d 639 getAltitude();
pyonta2017 0:6ddf1386e71d 640 wait(0.5);
pyonta2017 0:6ddf1386e71d 641 val = timer.read();
pyonta2017 0:6ddf1386e71d 642 if(flag2 > 10) phase = Descending;
pyonta2017 0:6ddf1386e71d 643 if(val > 900) phase = Descending;
pyonta2017 0:6ddf1386e71d 644 break;
pyonta2017 0:6ddf1386e71d 645 case Descending:
pyonta2017 0:6ddf1386e71d 646 val = timer.read();
pyonta2017 0:6ddf1386e71d 647 myled2 = 0;
pyonta2017 0:6ddf1386e71d 648 myled3 = 1;
pyonta2017 0:6ddf1386e71d 649 getGPS();
pyonta2017 0:6ddf1386e71d 650
pyonta2017 0:6ddf1386e71d 651 if ( val >1500){
pyonta2017 0:6ddf1386e71d 652 timer.reset();
pyonta2017 0:6ddf1386e71d 653 val = 0;
pyonta2017 0:6ddf1386e71d 654 timer_set = 1;
pyonta2017 0:6ddf1386e71d 655 }
pyonta2017 0:6ddf1386e71d 656 if (timer_set == 1){
pyonta2017 0:6ddf1386e71d 657 val_total = 1500 + val;
pyonta2017 0:6ddf1386e71d 658 }
pyonta2017 0:6ddf1386e71d 659 if ( flag3 >= 300){
pyonta2017 0:6ddf1386e71d 660 if( val_total > 1800){
pyonta2017 0:6ddf1386e71d 661 phase = Landing_Fusing;
pyonta2017 0:6ddf1386e71d 662 }
pyonta2017 0:6ddf1386e71d 663 }
pyonta2017 0:6ddf1386e71d 664 if( val_total > 2400) phase = Landing_Fusing;//電源onから55分
pyonta2017 0:6ddf1386e71d 665 break;
pyonta2017 0:6ddf1386e71d 666 case Landing_Fusing:
pyonta2017 0:6ddf1386e71d 667 timer.reset();
pyonta2017 0:6ddf1386e71d 668 timer.stop();
pyonta2017 0:6ddf1386e71d 669 val = 0;
pyonta2017 0:6ddf1386e71d 670 myled3 = 0;
pyonta2017 0:6ddf1386e71d 671 myled4 = 1;
pyonta2017 0:6ddf1386e71d 672 wait(30);
pyonta2017 0:6ddf1386e71d 673 burning();
pyonta2017 0:6ddf1386e71d 674 wait(5);
pyonta2017 0:6ddf1386e71d 675 Mu2.printf("@DT04FIRE\r\n");
pyonta2017 0:6ddf1386e71d 676 for ( int gp; gp <= 10; gp++){
pyonta2017 0:6ddf1386e71d 677 getGPS();
pyonta2017 0:6ddf1386e71d 678 }
pyonta2017 0:6ddf1386e71d 679 wait(5);
pyonta2017 0:6ddf1386e71d 680 jumpmotor = 1;
pyonta2017 0:6ddf1386e71d 681 wait(15);
pyonta2017 0:6ddf1386e71d 682 jumpmotor = 0;
pyonta2017 0:6ddf1386e71d 683 phase = Moving;
pyonta2017 0:6ddf1386e71d 684 break;
pyonta2017 0:6ddf1386e71d 685 case Moving:
pyonta2017 0:6ddf1386e71d 686 myled2 = 0;
pyonta2017 0:6ddf1386e71d 687 myled1 = 0;
pyonta2017 0:6ddf1386e71d 688 myled3 = 0;
pyonta2017 0:6ddf1386e71d 689 myled4 = 0;
pyonta2017 0:6ddf1386e71d 690 wait(1);
pyonta2017 0:6ddf1386e71d 691 switch(step){
pyonta2017 0:6ddf1386e71d 692 case get_goaldirection:
pyonta2017 0:6ddf1386e71d 693 myled1 = 1;
pyonta2017 0:6ddf1386e71d 694 getGPS();
pyonta2017 0:6ddf1386e71d 695 //test_get_direction();
pyonta2017 0:6ddf1386e71d 696 step = attitude_determination;
pyonta2017 0:6ddf1386e71d 697 wait(1);
pyonta2017 0:6ddf1386e71d 698 break;
pyonta2017 0:6ddf1386e71d 699 case attitude_determination:
pyonta2017 0:6ddf1386e71d 700 myled1 = 0;
pyonta2017 0:6ddf1386e71d 701 myled2 = 1;
pyonta2017 0:6ddf1386e71d 702 timer.reset();
pyonta2017 0:6ddf1386e71d 703 timer.start();
pyonta2017 0:6ddf1386e71d 704 val = 0;
pyonta2017 0:6ddf1386e71d 705 while(val < 3){
pyonta2017 0:6ddf1386e71d 706 val = timer.read();
pyonta2017 0:6ddf1386e71d 707 getheading3axis();
pyonta2017 0:6ddf1386e71d 708 wait(0.5);
pyonta2017 0:6ddf1386e71d 709 }
pyonta2017 0:6ddf1386e71d 710 getheading3axis();
pyonta2017 0:6ddf1386e71d 711 step = calculate_direction;
pyonta2017 0:6ddf1386e71d 712 wait(1);
pyonta2017 0:6ddf1386e71d 713 break;
pyonta2017 0:6ddf1386e71d 714 case calculate_direction:
pyonta2017 0:6ddf1386e71d 715 myled2 = 0;
pyonta2017 0:6ddf1386e71d 716 myled3 = 1;
pyonta2017 0:6ddf1386e71d 717 if(headingLPF < M_PI){
pyonta2017 0:6ddf1386e71d 718 if(goal_direction < headingLPF+M_PI){
pyonta2017 0:6ddf1386e71d 719 angular_difference = headingLPF - goal_direction;
pyonta2017 0:6ddf1386e71d 720 }
pyonta2017 0:6ddf1386e71d 721 else{
pyonta2017 0:6ddf1386e71d 722 angular_difference = 2*M_PI + (headingLPF - goal_direction);
pyonta2017 0:6ddf1386e71d 723 }
pyonta2017 0:6ddf1386e71d 724 }
pyonta2017 0:6ddf1386e71d 725 else{
pyonta2017 0:6ddf1386e71d 726 if(goal_direction < headingLPF - M_PI){
pyonta2017 0:6ddf1386e71d 727 angular_difference = headingLPF - goal_direction - 2*M_PI;
pyonta2017 0:6ddf1386e71d 728 }
pyonta2017 0:6ddf1386e71d 729 else{
pyonta2017 0:6ddf1386e71d 730 angular_difference = headingLPF - goal_direction;
pyonta2017 0:6ddf1386e71d 731 }
pyonta2017 0:6ddf1386e71d 732 }
pyonta2017 0:6ddf1386e71d 733 mp.printf("heading:%f,goal_direction%f\n\r",headingLPF*180/M_PI,goal_direction*180/M_PI);
pyonta2017 0:6ddf1386e71d 734 //mp.printf("rangular_difference = %f\n\r" , angular_difference*180/M_PI);
pyonta2017 0:6ddf1386e71d 735 //40度以下でジャンプ
pyonta2017 0:6ddf1386e71d 736 if ((angular_difference < 0.698132) && (angular_difference > -0.698132)){
pyonta2017 0:6ddf1386e71d 737 step = jump;
pyonta2017 0:6ddf1386e71d 738 }
pyonta2017 0:6ddf1386e71d 739 else {
pyonta2017 0:6ddf1386e71d 740 step = direction_control;
pyonta2017 0:6ddf1386e71d 741 }
pyonta2017 0:6ddf1386e71d 742 wait(1);
pyonta2017 0:6ddf1386e71d 743 break;
pyonta2017 0:6ddf1386e71d 744
pyonta2017 0:6ddf1386e71d 745 case direction_control:
pyonta2017 0:6ddf1386e71d 746 myled1 = 0;
pyonta2017 0:6ddf1386e71d 747 myled3 = 0;
pyonta2017 0:6ddf1386e71d 748 myled4 = 1;
pyonta2017 0:6ddf1386e71d 749 //int drive_time;
pyonta2017 0:6ddf1386e71d 750 timer.reset();
pyonta2017 0:6ddf1386e71d 751 timer.stop();
pyonta2017 0:6ddf1386e71d 752 timer.start();
pyonta2017 0:6ddf1386e71d 753 val = 0;
pyonta2017 0:6ddf1386e71d 754 while(val < 2){
pyonta2017 0:6ddf1386e71d 755 val = timer.read();
pyonta2017 0:6ddf1386e71d 756 motor_drive();
pyonta2017 0:6ddf1386e71d 757 }
pyonta2017 0:6ddf1386e71d 758 controlmotor1.write(0);
pyonta2017 0:6ddf1386e71d 759 controlmotor2.write(0);
pyonta2017 0:6ddf1386e71d 760 pin21.write(0.0);
pyonta2017 0:6ddf1386e71d 761 timer.reset();
pyonta2017 0:6ddf1386e71d 762 timer.stop();
pyonta2017 0:6ddf1386e71d 763 val = 0;
pyonta2017 0:6ddf1386e71d 764 //step = jump;
pyonta2017 0:6ddf1386e71d 765 if(jump_count == 3){
pyonta2017 0:6ddf1386e71d 766 step = jump;
pyonta2017 0:6ddf1386e71d 767 }
pyonta2017 0:6ddf1386e71d 768 else{
pyonta2017 0:6ddf1386e71d 769 step = attitude_determination;
pyonta2017 0:6ddf1386e71d 770 }
pyonta2017 0:6ddf1386e71d 771 jump_count = jump_count+1;
pyonta2017 0:6ddf1386e71d 772 break;
pyonta2017 0:6ddf1386e71d 773
pyonta2017 0:6ddf1386e71d 774 case jump:
pyonta2017 0:6ddf1386e71d 775 //ログファイル
pyonta2017 0:6ddf1386e71d 776 myled1 = 1;
pyonta2017 0:6ddf1386e71d 777 myled2 = 1;
pyonta2017 0:6ddf1386e71d 778 wait(2);
pyonta2017 0:6ddf1386e71d 779 FILE *fp = fopen("/sd/mydir/height1.txt", "a");
pyonta2017 0:6ddf1386e71d 780 wait(1);
pyonta2017 0:6ddf1386e71d 781 timer.reset();
pyonta2017 0:6ddf1386e71d 782 timer.start();
pyonta2017 0:6ddf1386e71d 783 val = 0;
pyonta2017 0:6ddf1386e71d 784 jumpmotor = 1;
pyonta2017 0:6ddf1386e71d 785 Mu2.printf("@DT04JUMP\r\n");
pyonta2017 0:6ddf1386e71d 786 while(val < 4000){
pyonta2017 0:6ddf1386e71d 787 val = timer.read_ms();
pyonta2017 0:6ddf1386e71d 788 adxl[0] = 0x32;
pyonta2017 0:6ddf1386e71d 789 i2c.write(addr,adxl,1);
pyonta2017 0:6ddf1386e71d 790 i2c.read(0xA7,adxl,6);
pyonta2017 0:6ddf1386e71d 791 axout = (((int16_t )adxl[1]) << 8) | adxl[0];
pyonta2017 0:6ddf1386e71d 792 ayout = (((int16_t )adxl[3]) << 8) | adxl[2];
pyonta2017 0:6ddf1386e71d 793 azout = (((int16_t )adxl[5]) << 8) | adxl[4];
pyonta2017 0:6ddf1386e71d 794 fprintf(fp, "%d,%d,%d,%d\r\n",val,axout,ayout,azout);
pyonta2017 0:6ddf1386e71d 795 wait_ms(2);
pyonta2017 0:6ddf1386e71d 796 }
pyonta2017 0:6ddf1386e71d 797 jumpmotor = 0;
pyonta2017 0:6ddf1386e71d 798 wait(1);
pyonta2017 0:6ddf1386e71d 799 fclose(fp);
pyonta2017 0:6ddf1386e71d 800 myled3 = 1;
pyonta2017 0:6ddf1386e71d 801 jump_count = 0;
pyonta2017 0:6ddf1386e71d 802 wait(3);
pyonta2017 0:6ddf1386e71d 803 step = get_goaldirection;
pyonta2017 0:6ddf1386e71d 804 //step = direction_control;
pyonta2017 0:6ddf1386e71d 805 break;
pyonta2017 0:6ddf1386e71d 806 }
pyonta2017 0:6ddf1386e71d 807 }
pyonta2017 0:6ddf1386e71d 808 }
pyonta2017 0:6ddf1386e71d 809 }