修正済みby皆川
Dependencies: mbed Servo cansat_integrated_2 BMP180
Dependents: cansat_integrated_2
direction.h
- Committer:
- minanao
- Date:
- 2021-10-21
- Revision:
- 0:e7b7def631c2
- Child:
- 1:bb89b58cfa0e
File content as of revision 0:e7b7def631c2:
#include "mbed.h" #include "getGPS.h" #include "MPU9250.h" Serial pc(SERIAL_TX, SERIAL_RX); GPS gps(D1, D0); float calculation_Θ(float x_0,float y_0,float x_1,float y_1,float x,float y){ //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。 float Θ_0 = atan((y_0 - y_1)/(x_0 - x_1)); //目的地の角度 if(y_0 - y_1 > 0 && x_0 - x_1 < 0){ Θ_0 = Θ_0 - 180; } if(y_0 - y_1 < 0 && x_0 - x_1 < 0){ Θ_0 = Θ_0 + 180; } float Omag_x,Omag_y; //地磁気センサのxy平面が描く円の中点 float Θ_1 = atan((y - Omag_y)/(x- Omag_x)); //CanSatの角度 if(y - Omag_y > 0 && x- Omag_x < 0){ Θ_1 = Θ_1 - 180; } if(y - Omag_y < 0 && x - Omag_x < 0){ Θ_1 = Θ_1 + 180; } Θ = Θ_0 - Θ_1; //CanSatから見た目的地の角度 } class direction() { public: Serial pc(SERIAL_TX, SERIAL_RX); GPS gps(D1, D0); float x_0 ,y_0; //目的地 MPU9250 mpu9250(D4, D5); // SDA, SCL Serial pc(USBTX, USBRX, 115200); volatile bool interrupt_flag = false; void detect_shock(void) { interrupt_flag = true; } while(1) { if(gps.getgps()) //現在地取得 pc.printf("(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力 else pc.printf("No data\r\n");//データ取得に失敗した場合 wait(1); } mpu9250.resetMPU9250(); mpu9250.setWakeOnMotion(); while(1) { if (interrupt_flag) { pc.printf("detect\r\n"); wait_ms(1000); interrupt_flag = false; } /* get mag data*/ int16_t mag[3]; mpu9250.readMagData(mag); float mag_f[3]; for (int i = 0; i < 3; ++i) mag_f[i] = float(mag[i]) * mpu9250.getMres(); /* serial */ pc.printf("mx:%3.3f\t", mag_f[0]); pc.printf("my:%3.3f\t", mag_f[1]); pc.printf("mz:%3.3f\t\n", mag_f[2]); pc.printf("\n\n---------------------------\r\n"); wait_ms(100); } }