SSLM1
/
5-4_recoverymode
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: 2_MPU9250 mbed 2_GPS_GMS6-CR6 2_LPS33HW 2_EEPROM 2_P7100
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "math.h" 00003 #include "EEPROM.h" 00004 #include "LPS33HW.h" 00005 #include "P7100.h" 00006 #include "MPU9250.h" 00007 #include "GMS6_CR6.h" 00008 00009 00010 Timer t; //時間計測 00011 Serial pc(USBTX, USBRX); 00012 LPS33HW lps(p9, p10); 00013 EEPROM rom(p9, p10); 00014 MPU9250 mpu(p9, p10); 00015 P7100 p7100(p5, p6, p7, p8); 00016 Serial twe(p13, p14); 00017 GMS6_CR6 gps(p28, p27); 00018 00019 DigitalIn Flight_Pin(p30); 00020 DigitalOut ThrustValve(p21); 00021 DigitalOut DepressValve(p22); 00022 00023 void get_save_low(); //自作関数プロトタイプ宣言 00024 float get_save_high(); //自作関数プロトタイプ宣言 00025 00026 char s[64]; //データ取得配列 00027 float accel[3], gyro[3]; //MPU9250data 00028 float AX, AY, AZ, GX, GY, GZ; //MPU9250data 00029 float T; //時刻データ 00030 float P; //気圧データ取得 00031 float V; //電圧取得 00032 float TP = 0.25 * V - 0.125; //タンク圧力換算 00033 00034 int pointer_high = 0; //EEPROM保存アドレス(high) 00035 int pointer_low = 0; //EEPROM保存アドレス(low) 00036 00037 00038 00039 //main 00040 int main() 00041 { 00042 pc.baud(460800); //ボーレート変更 00043 twe.baud(230400); //tweボーレート変更 00044 00045 float lat_north; //GPS緯度 00046 float lon_east; //GPS経度 00047 float GPS[2]; //GPSデータ格納配列 00048 00049 double groundP = 0; //地上の気圧データ 00050 double heightP = 0; //高度TBDmの気圧 00051 double judgeP[5]; //判定用気圧データ 00052 double pastP = 0; //一つ前の気圧データ 00053 double dP[5]; //気圧データの差異 00054 int judge = 0; //判定結果 00055 int i = 0; //差異配列の番号 00056 memset(judgeP, 0, 5); 00057 memset(dP, 1, 5); 00058 00059 int n_dvalve = 0; //脱圧バルブの開閉回数 00060 00061 t.start(); //timer Start 00062 00063 //sensor Setup 00064 lps.start(1); //気圧センサスタート 00065 lps.start(1); //気圧センサスタート 00066 wait(0.1); 00067 mpu.start(); //MPU9250 start&setup 00068 mpu.accelsetup(3); 00069 mpu.gyrosetup(3); 00070 mpu.AKsetup(1); 00071 wait(0.2); 00072 00073 pc.printf("Power ON!\r\n"); 00074 twe.printf("Power ON!\r\n^"); 00075 00076 //地上の気圧取得&計算 00077 for(int a = 0; a < 1000; a++) { 00078 groundP = groundP + lps.data_read(); 00079 wait_ms(50); 00080 } 00081 groundP = groundP/1000; //地上の気圧データ 00082 heightP = 0.998862 * groundP; //高度10mの気圧を計算 00083 00084 00085 00086 //Wait mode 00087 pc.printf("Wait mode Start!\r\n"); 00088 twe.printf("Wait mode Start!\r\n^"); 00089 while(Flight_Pin == 0) { 00090 get_save_low(); //data get&save 00091 wait_ms(1000); 00092 } 00093 pc.printf("Wait mode Finish!\r\n"); 00094 twe.printf("Wait mode Start!\r\n^"); 00095 00096 00097 00098 //Flight mode 00099 pc.printf("Flight mode Start!\r\n"); 00100 twe.printf("Flight mode Start!\r\n^"); 00101 while(1) { 00102 for(int k = 0; k < 200; k++) { 00103 judgeP[i] = 0; //初期化 00104 for(int n = 0; n < 2; n++) { //2回に一回送信 00105 judgeP[i] = judgeP[i] + get_save_high(); //data get&save 00106 } 00107 twe.printf("^"); //送信トリガー 00108 00109 judgeP[i] = judgeP[i]/2; //判定用気圧を計算 00110 dP[i] = judgeP[i] - pastP; //差異を取得 00111 pastP = judgeP[i]; //判定用気圧値を過去データに置き換える 00112 i++; 00113 if(i == 5) { 00114 i = 0; 00115 } 00116 00117 if(dP[0] > 0.05 && dP[1] > 0.05 && dP[2] > 0.05 && dP[3] > 0.05 && dP[4] > 0.05) { //空中判定 00118 if(judgeP[0] > heightP && judgeP[1] > heightP && judgeP[2] > heightP && judgeP[3] > heightP && judgeP[4] > heightP) { //高度判定 00119 judge = 1; 00120 break; 00121 } 00122 } 00123 else if(dP[0] < 0.001 && dP[1] < 0.001 && dP[2] < 0.001 && dP[3] < 0.001 && dP[4] < 0.001) { 00124 judge = 2; 00125 break; 00126 } 00127 } 00128 00129 if(judge == 1 || judge == 2) { 00130 break; //判定を通過したらループを抜ける 00131 } 00132 00133 gps.read(GPS); //測位データ取得 00134 lat_north = GPS[0]; 00135 lon_east = GPS[1]; 00136 00137 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信 00138 twe.printf("%f,%f\r\n^", lat_north, lon_east); //送信トリガー 00139 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用 00140 } 00141 memset(GPS,'\0', 2); //送信トリガー 00142 } 00143 pc.printf("Flight mode Finish!\r\n"); 00144 twe.printf("Flight mode Finish!\r\n^"); 00145 00146 00147 00148 //thruster mode 00149 if(judge == 1) { 00150 pc.printf("Thruster mode Start!\r\n"); 00151 twe.printf("Thruster mode Start!\r\n^"); 00152 ThrustValve = 1; //スラスタ電磁弁開 00153 for(int k = 0; k < 250; k++) 00154 { 00155 for(int n = 0; n < 2; n++) //2回に一回送信 00156 { 00157 get_save_high(); //data get&save 00158 } 00159 twe.printf("^"); //送信トリガー 00160 } 00161 ThrustValve = 0; //スラスタ電磁弁閉 00162 pc.printf("Thruster mode Finish!\r\n"); 00163 twe.printf("Thruster mode Finish!\r\n^"); 00164 } 00165 00166 00167 00168 //depress mode 00169 pc.printf("Depress mode Start!\r\n"); 00170 twe.printf("Depress mode Start!\r\n^"); 00171 memset(judgeP, 0, 5); 00172 memset(dP, 1, 5); 00173 pastP = 0; 00174 00175 while(1) { 00176 DepressValve = 1; 00177 00178 for(int k = 0; k < 100; k++) { 00179 judgeP[i] = 0; //初期化 00180 00181 for(int n = 0; n < 2; n++) { //2回に一回送信 00182 judgeP[i] = judgeP[i] + get_save_high(); //data get&save 00183 } 00184 twe.printf("^"); //送信トリガー 00185 00186 judgeP[i] = judgeP[i]/2; //判定用気圧を計算 00187 dP[i] = judgeP[i] - pastP; //差異を取得 00188 pastP = judgeP[i]; //判定用気圧値を過去データに置き換える 00189 00190 if(dP[0] < 0.001 && dP[1] < 0.001 && dP[2] < 0.001 && dP[3] < 0.001 && dP[4] < 0.001 && n_dvalve > 2 || n_dvalve == 10) //着地判定 00191 { 00192 judge = 3; 00193 break; 00194 } 00195 i++; 00196 if(i == 5) {i = 0;} 00197 } 00198 00199 DepressValve = 0; 00200 n_dvalve++; 00201 00202 if(judge == 3) { 00203 break; //地上判定を通過したらループを抜ける 00204 } 00205 00206 gps.read_low(GPS); //測位データ取得 00207 lat_north = GPS[0]; 00208 lon_east = GPS[1]; 00209 00210 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信 00211 twe.printf("%f,%f\r\n^", lat_north, lon_east); //送信トリガー 00212 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用 00213 } 00214 memset(GPS,'\0', 2); //送信トリガー 00215 } 00216 pc.printf("Depress mode Finish!\r\n"); 00217 twe.printf("Depress mode Finish!\r\n^"); 00218 00219 00220 00221 //recovery mode 00222 pc.printf("Recovery mode Start!\r\n"); 00223 twe.printf("Recovery mode Start!\r\n^"); 00224 while(1) 00225 { 00226 T = t.read(); //時刻データ取得 00227 V = p7100.v_read(); //電圧取得 00228 TP = 0.25 * V - 0.125; //タンク圧力換算 00229 00230 sprintf(s, "%8.3f %6.3f\r\n", T, TP); //floatからchar*へ変換 00231 00232 rom.write_low(pointer_low, s, 64); // write tha data 00233 pointer_low = pointer_low + 64; //アドレスずらし 00234 00235 pc.printf("%s\r\n", s); //check用 00236 twe.printf("%s", s); //無線機送信 00237 memset(s, '\0', 64 ); //初期化 00238 00239 gps.read_low(GPS); //測位データ取得 00240 lat_north = GPS[0]; 00241 lon_east = GPS[1]; 00242 00243 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信 00244 twe.printf("%f,%f\r\n", lat_north, lon_east); //送信 00245 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用 00246 } 00247 memset(GPS,'\0', 2); 00248 twe.printf("%^"); //無線機トリガー 00249 wait_ms(1000); 00250 } 00251 pc.printf("Recovery mode Finish!\r\n"); 00252 twe.printf("Recovery mode Finish!\r\n^"); 00253 } 00254 00255 00256 00257 void get_save_low() 00258 { 00259 T = t.read(); //時刻データ取得 00260 P = lps.data_read(); //気圧データ取得 00261 V = p7100.v_read(); //電圧取得 00262 TP = 0.25 * V - 0.125; //タンク圧力換算 00263 00264 mpu.accel_read(3, accel); 00265 mpu.gyro_read(3, gyro); 00266 AX = accel[0]; 00267 AY = accel[1]; 00268 AZ = accel[2]; 00269 GX = gyro[0]; 00270 GY = gyro[1]; 00271 GZ = gyro[2]; 00272 00273 sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換 00274 00275 rom.write_low(pointer_low, s, 64); // write tha data 00276 pointer_low = pointer_low + 64; //アドレスずらし 00277 00278 pc.printf("%s\r\n", s); //check用 00279 00280 memset(s, '\0', 64 ); //初期化 00281 } 00282 00283 float get_save_high() 00284 { 00285 T = t.read(); //時刻データ取得 00286 P = lps.data_read(); //気圧データ取得 00287 V = p7100.v_read(); //電圧取得 00288 TP = 0.25 * V - 0.125; //タンク圧力換算 00289 00290 mpu.accel_read(3, accel); 00291 mpu.gyro_read(3, gyro); 00292 AX = accel[0]; 00293 AY = accel[1]; 00294 AZ = accel[2]; 00295 GX = gyro[0]; 00296 GY = gyro[1]; 00297 GZ = gyro[2]; 00298 00299 sprintf(s, "%8.3f %8.3f %6.3f %5.3f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換 00300 00301 rom.write_high(pointer_high, s, 64); // write tha data 00302 00303 twe.printf("%s", s); //無線機送信 00304 00305 pointer_high = pointer_high + 64; //アドレスずらし 00306 00307 pc.printf("%s\r\n", s); //check用 00308 00309 memset(s, '\0', 64 ); //初期化 00310 00311 return P; 00312 }
Generated on Fri Jul 15 2022 05:10:19 by
1.7.2