SSLM1 / Mbed 2 deprecated 5-6_checkmode

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

main.cpp@0:9fb816db0c8b, 2020-07-27 (annotated)

Committer:
rary
Date:
Mon Jul 27 12:39:33 2020 +0000
Revision:
0:9fb816db0c8b
Child:
1:80140aac4bec
add checkmode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rary 0:9fb816db0c8b 1 #include "mbed.h"
rary 0:9fb816db0c8b 2 #include "math.h"
rary 0:9fb816db0c8b 3 #include "EEPROM.h"
rary 0:9fb816db0c8b 4 #include "LPS33HW.h"
rary 0:9fb816db0c8b 5 #include "P7100.h"
rary 0:9fb816db0c8b 6 #include "MPU9250.h"
rary 0:9fb816db0c8b 7 #include "GMS6_CR6.h"
rary 0:9fb816db0c8b 8
rary 0:9fb816db0c8b 9
rary 0:9fb816db0c8b 10 Timer t; //時間計測
rary 0:9fb816db0c8b 11 Serial pc(USBTX, USBRX);
rary 0:9fb816db0c8b 12 LPS33HW lps(p9, p10);
rary 0:9fb816db0c8b 13 EEPROM rom(p9, p10);
rary 0:9fb816db0c8b 14 MPU9250 mpu(p9, p10);
rary 0:9fb816db0c8b 15 P7100 p7100(p5, p6, p7, p8);
rary 0:9fb816db0c8b 16 Serial twe(p28, p27); //EFM:p28, p27
rary 0:9fb816db0c8b 17 GMS6_CR6 gps(p13, p14); //EFM:p13, p14
rary 0:9fb816db0c8b 18
rary 0:9fb816db0c8b 19 DigitalIn Flight_Pin(p30);
rary 0:9fb816db0c8b 20 DigitalOut ThrustValve(p21);
rary 0:9fb816db0c8b 21 DigitalOut DepressValve(p22);
rary 0:9fb816db0c8b 22 DigitalOut LED_1(p23);
rary 0:9fb816db0c8b 23 DigitalOut LED_2(p24);
rary 0:9fb816db0c8b 24 DigitalOut LED_3(p25);
rary 0:9fb816db0c8b 25 DigitalOut LED_4(p26);
rary 0:9fb816db0c8b 26
rary 0:9fb816db0c8b 27 void get_save_low(); //自作関数プロトタイプ宣言
rary 0:9fb816db0c8b 28 float get_save_high(); //自作関数プロトタイプ宣言
rary 0:9fb816db0c8b 29 void recovery(); //自作関数プロトタイプ宣言
rary 0:9fb816db0c8b 30 void check();
rary 0:9fb816db0c8b 31
rary 0:9fb816db0c8b 32 char s[64]; //データ取得配列
rary 0:9fb816db0c8b 33 float accel[3], gyro[3]; //MPU9250data
rary 0:9fb816db0c8b 34 float AX, AY, AZ, GX, GY, GZ; //MPU9250data
rary 0:9fb816db0c8b 35 float T; //時刻データ
rary 0:9fb816db0c8b 36 float P; //気圧データ取得
rary 0:9fb816db0c8b 37 float V; //電圧取得
rary 0:9fb816db0c8b 38 float TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:9fb816db0c8b 39
rary 0:9fb816db0c8b 40 int pointer_high = 0; //EEPROM保存アドレス(high)
rary 0:9fb816db0c8b 41 int pointer_low = 0; //EEPROM保存アドレス(low)
rary 0:9fb816db0c8b 42
rary 0:9fb816db0c8b 43 float lat_north; //GPS緯度
rary 0:9fb816db0c8b 44 float lon_east; //GPS経度
rary 0:9fb816db0c8b 45 float GPS[2]; //GPSデータ格納配列
rary 0:9fb816db0c8b 46
rary 0:9fb816db0c8b 47 //main
rary 0:9fb816db0c8b 48 int main()
rary 0:9fb816db0c8b 49 {
rary 0:9fb816db0c8b 50 pc.baud(460800); //ボーレート変更
rary 0:9fb816db0c8b 51 twe.baud(230400); //tweボーレート変更
rary 0:9fb816db0c8b 52
rary 0:9fb816db0c8b 53 ThrustValve = 0; //DigitalOut初期化
rary 0:9fb816db0c8b 54 DepressValve = 0;
rary 0:9fb816db0c8b 55 LED_1 = 0;
rary 0:9fb816db0c8b 56 LED_2 = 0;
rary 0:9fb816db0c8b 57 LED_3 = 0;
rary 0:9fb816db0c8b 58 LED_4 = 0;
rary 0:9fb816db0c8b 59
rary 0:9fb816db0c8b 60 //判定用変数
rary 0:9fb816db0c8b 61 double groundP = 0; //地上の気圧データ
rary 0:9fb816db0c8b 62 double heightP = 0; //高度TBDmの気圧
rary 0:9fb816db0c8b 63 double judgeP[5]; //判定用気圧データ
rary 0:9fb816db0c8b 64 double pastP = 0; //一つ前の気圧データ
rary 0:9fb816db0c8b 65 double dP[5]; //気圧データの差異
rary 0:9fb816db0c8b 66 int judge = 0; //判定結果
rary 0:9fb816db0c8b 67 int i = 0; //差異配列の番号
rary 0:9fb816db0c8b 68 memset(judgeP, 0, 5);
rary 0:9fb816db0c8b 69 memset(dP, 1, 5);
rary 0:9fb816db0c8b 70
rary 0:9fb816db0c8b 71 int n_dvalve = 0; //脱圧バルブの開閉回数
rary 0:9fb816db0c8b 72
rary 0:9fb816db0c8b 73 t.start(); //timer Start
rary 0:9fb816db0c8b 74
rary 0:9fb816db0c8b 75 //sensor Setup
rary 0:9fb816db0c8b 76 lps.start(1); //気圧センサスタート
rary 0:9fb816db0c8b 77 lps.start(1); //気圧センサスタート
rary 0:9fb816db0c8b 78 wait(0.1);
rary 0:9fb816db0c8b 79 mpu.start(); //MPU9250 start&setup
rary 0:9fb816db0c8b 80 mpu.accelsetup(3);
rary 0:9fb816db0c8b 81 mpu.gyrosetup(3);
rary 0:9fb816db0c8b 82 mpu.AKsetup(1);
rary 0:9fb816db0c8b 83 wait(0.2);
rary 0:9fb816db0c8b 84
rary 0:9fb816db0c8b 85 pc.printf("Power ON!\r\n");
rary 0:9fb816db0c8b 86 twe.printf("Power ON!\r\n");
rary 0:9fb816db0c8b 87 wait(2);
rary 0:9fb816db0c8b 88
rary 0:9fb816db0c8b 89 //Check mode
rary 0:9fb816db0c8b 90 pc.printf("Check mode Start!\r\n");
rary 0:9fb816db0c8b 91 twe.printf("Check mode Start!\r\n");
rary 0:9fb816db0c8b 92 //valve check
rary 0:9fb816db0c8b 93 ThrustValve = 1;wait(1);ThrustValve = 0;
rary 0:9fb816db0c8b 94 wait(1);
rary 0:9fb816db0c8b 95 ThrustValve = 1;wait(1);ThrustValve = 0;
rary 0:9fb816db0c8b 96 wait(2);
rary 0:9fb816db0c8b 97 DepressValve = 1;wait(1);DepressValve = 0;
rary 0:9fb816db0c8b 98 wait(1);
rary 0:9fb816db0c8b 99 DepressValve = 1;wait(1);DepressValve = 0;
rary 0:9fb816db0c8b 100 wait(1);
rary 0:9fb816db0c8b 101
rary 0:9fb816db0c8b 102 for(int c = 0; c < 60 ; c++) {
rary 0:9fb816db0c8b 103 LED_1 = 1;
rary 0:9fb816db0c8b 104 LED_2 = 1;
rary 0:9fb816db0c8b 105 LED_3 = 1;
rary 0:9fb816db0c8b 106 LED_4 = 1;
rary 0:9fb816db0c8b 107 check();
rary 0:9fb816db0c8b 108 LED_1 = 0;
rary 0:9fb816db0c8b 109 LED_2 = 0;
rary 0:9fb816db0c8b 110 LED_3 = 0;
rary 0:9fb816db0c8b 111 LED_4 = 0;
rary 0:9fb816db0c8b 112 wait(1);
rary 0:9fb816db0c8b 113 }
rary 0:9fb816db0c8b 114
rary 0:9fb816db0c8b 115
rary 0:9fb816db0c8b 116
rary 0:9fb816db0c8b 117 //地上の気圧取得&計算
rary 0:9fb816db0c8b 118 pc.printf("Calibration Start!\r\n");
rary 0:9fb816db0c8b 119 twe.printf("Calibration Start!\r\n");
rary 0:9fb816db0c8b 120 for(int a = 0; a < 1000; a++) {
rary 0:9fb816db0c8b 121 groundP = groundP + lps.data_read();
rary 0:9fb816db0c8b 122 wait_ms(50);
rary 0:9fb816db0c8b 123 }
rary 0:9fb816db0c8b 124 groundP = groundP/1000; //地上の気圧データ
rary 0:9fb816db0c8b 125 heightP = 0.998862 * groundP; //高度10mの気圧を計算
rary 0:9fb816db0c8b 126
rary 0:9fb816db0c8b 127
rary 0:9fb816db0c8b 128
rary 0:9fb816db0c8b 129 //Wait mode
rary 0:9fb816db0c8b 130 pc.printf("Wait mode Start!\r\n");
rary 0:9fb816db0c8b 131 twe.printf("Wait mode Start!\r\n");
rary 0:9fb816db0c8b 132 LED_1 = 1;
rary 0:9fb816db0c8b 133 while(Flight_Pin == 0) {
rary 0:9fb816db0c8b 134 get_save_low(); //data get&save
rary 0:9fb816db0c8b 135 wait_ms(950);
rary 0:9fb816db0c8b 136 }
rary 0:9fb816db0c8b 137 pc.printf("Wait mode Finish!\r\n");
rary 0:9fb816db0c8b 138 twe.printf("Wait mode Finish!\r\n");
rary 0:9fb816db0c8b 139 LED_1 = 0;
rary 0:9fb816db0c8b 140
rary 0:9fb816db0c8b 141
rary 0:9fb816db0c8b 142
rary 0:9fb816db0c8b 143 //Flight mode
rary 0:9fb816db0c8b 144 pc.printf("Flight mode Start!\r\n");
rary 0:9fb816db0c8b 145 twe.printf("Flight mode Start!\r\n");
rary 0:9fb816db0c8b 146 LED_2 = 1;
rary 0:9fb816db0c8b 147 while(1) {
rary 0:9fb816db0c8b 148 judgeP[i] = 0; //初期化
rary 0:9fb816db0c8b 149
rary 0:9fb816db0c8b 150 for(int n = 0; n < 2; n++) { //2回に一回送信
rary 0:9fb816db0c8b 151 judgeP[i] = judgeP[i] + get_save_high(); //data get&save
rary 0:9fb816db0c8b 152 }
rary 0:9fb816db0c8b 153 twe.printf("\r\n"); //送信トリガー
rary 0:9fb816db0c8b 154
rary 0:9fb816db0c8b 155 //判定
rary 0:9fb816db0c8b 156 judgeP[i] = judgeP[i]/2; //判定用気圧を計算
rary 0:9fb816db0c8b 157 dP[i] = judgeP[i] - pastP; //差異を取得
rary 0:9fb816db0c8b 158 pastP = judgeP[i]; //判定用気圧値を過去データに置き換える
rary 0:9fb816db0c8b 159 i++;
rary 0:9fb816db0c8b 160 if(i == 5) {
rary 0:9fb816db0c8b 161 i = 0;
rary 0:9fb816db0c8b 162 }
rary 0:9fb816db0c8b 163
rary 0:9fb816db0c8b 164 if(dP[0] > 0.03 && dP[1] > 0.03 && dP[2] > 0.03 && dP[3] > 0.03 && dP[4] > 0.03) { //空中判定
rary 0:9fb816db0c8b 165 if(judgeP[0] > heightP && judgeP[1] > heightP && judgeP[2] > heightP && judgeP[3] > heightP && judgeP[4] > heightP) { //高度判定
rary 0:9fb816db0c8b 166 judge = 1;
rary 0:9fb816db0c8b 167 break;
rary 0:9fb816db0c8b 168 }
rary 0:9fb816db0c8b 169 } else if(dP[0] <= 0 && dP[1] <= 0 && dP[2] <= 0 && dP[3] <= 0 && dP[4] <= 0) {
rary 0:9fb816db0c8b 170 judge = 1;
rary 0:9fb816db0c8b 171 break;
rary 0:9fb816db0c8b 172 }
rary 0:9fb816db0c8b 173 }
rary 0:9fb816db0c8b 174 pc.printf("Flight mode Finish!\r\n");
rary 0:9fb816db0c8b 175 twe.printf("Flight mode Finish!\r\n");
rary 0:9fb816db0c8b 176 LED_2 = 0;
rary 0:9fb816db0c8b 177
rary 0:9fb816db0c8b 178
rary 0:9fb816db0c8b 179
rary 0:9fb816db0c8b 180 //thruster mode
rary 0:9fb816db0c8b 181 if(judge == 1) {
rary 0:9fb816db0c8b 182 pc.printf("Thruster mode Start!\r\n");
rary 0:9fb816db0c8b 183 twe.printf("Thruster mode Start!\r\n");
rary 0:9fb816db0c8b 184 LED_3 = 1;
rary 0:9fb816db0c8b 185 ThrustValve = 1; //スラスタ電磁弁開
rary 0:9fb816db0c8b 186 for(int k = 0; k < 250; k++) {
rary 0:9fb816db0c8b 187 for(int n = 0; n < 2; n++) { //2回に一回送信
rary 0:9fb816db0c8b 188 get_save_high(); //data get&save
rary 0:9fb816db0c8b 189 }
rary 0:9fb816db0c8b 190 twe.printf("\r\n"); //送信トリガー
rary 0:9fb816db0c8b 191 }
rary 0:9fb816db0c8b 192 ThrustValve = 0; //スラスタ電磁弁閉
rary 0:9fb816db0c8b 193 pc.printf("Thruster mode Finish!\r\n");
rary 0:9fb816db0c8b 194 twe.printf("Thruster mode Finish!\r\n");
rary 0:9fb816db0c8b 195 }
rary 0:9fb816db0c8b 196
rary 0:9fb816db0c8b 197
rary 0:9fb816db0c8b 198
rary 0:9fb816db0c8b 199 //depress mode
rary 0:9fb816db0c8b 200 pc.printf("Depress mode Start!\r\n");
rary 0:9fb816db0c8b 201 twe.printf("Depress mode Start!\r\n");
rary 0:9fb816db0c8b 202 LED_4 = 1;
rary 0:9fb816db0c8b 203 memset(judgeP, 0, 5); //判定初期化
rary 0:9fb816db0c8b 204 memset(dP, 1, 5);
rary 0:9fb816db0c8b 205 pastP = 0;
rary 0:9fb816db0c8b 206
rary 0:9fb816db0c8b 207 while(1) {
rary 0:9fb816db0c8b 208 DepressValve = 1; //脱圧電磁弁開
rary 0:9fb816db0c8b 209
rary 0:9fb816db0c8b 210 judgeP[i] = 0; //初期化
rary 0:9fb816db0c8b 211
rary 0:9fb816db0c8b 212 for(int k = 0; k < 150; k++) {
rary 0:9fb816db0c8b 213 for(int n = 0; n < 2; n++) { //2回に一回送信
rary 0:9fb816db0c8b 214 judgeP[i] = judgeP[i] + get_save_high(); //data get&save
rary 0:9fb816db0c8b 215 }
rary 0:9fb816db0c8b 216 twe.printf("\r\n"); //送信トリガー
rary 0:9fb816db0c8b 217
rary 0:9fb816db0c8b 218 //判定
rary 0:9fb816db0c8b 219 judgeP[i] = judgeP[i]/2; //判定用気圧を計算
rary 0:9fb816db0c8b 220 dP[i] = judgeP[i] - pastP; //差異を取得
rary 0:9fb816db0c8b 221 pastP = judgeP[i]; //判定用気圧値を過去データに置き換える
rary 0:9fb816db0c8b 222
rary 0:9fb816db0c8b 223 if(dP[0] <= 0 && dP[1] <= 0 && dP[2] <= 0 && dP[3] <= 0 && dP[4] <= 0 && n_dvalve > 2 || n_dvalve == 4) { //着地判定
rary 0:9fb816db0c8b 224 judge = 3;
rary 0:9fb816db0c8b 225 break;
rary 0:9fb816db0c8b 226 }
rary 0:9fb816db0c8b 227 i++;
rary 0:9fb816db0c8b 228 if(i == 5) {
rary 0:9fb816db0c8b 229 i = 0;
rary 0:9fb816db0c8b 230 }
rary 0:9fb816db0c8b 231 }
rary 0:9fb816db0c8b 232
rary 0:9fb816db0c8b 233 DepressValve = 0; //脱圧電磁弁閉
rary 0:9fb816db0c8b 234 wait_ms(100);
rary 0:9fb816db0c8b 235 n_dvalve++;
rary 0:9fb816db0c8b 236
rary 0:9fb816db0c8b 237 if(judge == 3) {
rary 0:9fb816db0c8b 238 break; //地上判定を通過したらループを抜ける
rary 0:9fb816db0c8b 239 }
rary 0:9fb816db0c8b 240 }
rary 0:9fb816db0c8b 241 pc.printf("Depress mode Finish!\r\n");
rary 0:9fb816db0c8b 242 twe.printf("Depress mode Finish!\r\n");
rary 0:9fb816db0c8b 243 LED_4 = 0;
rary 0:9fb816db0c8b 244
rary 0:9fb816db0c8b 245
rary 0:9fb816db0c8b 246
rary 0:9fb816db0c8b 247 //recovery mode
rary 0:9fb816db0c8b 248 pc.printf("Recovery mode Start!\r\n");
rary 0:9fb816db0c8b 249 twe.printf("Recovery mode Start!\r\n");
rary 0:9fb816db0c8b 250 LED_1 = 1;
rary 0:9fb816db0c8b 251 LED_2 = 1;
rary 0:9fb816db0c8b 252 LED_4 = 1;
rary 0:9fb816db0c8b 253 while(1) {
rary 0:9fb816db0c8b 254 recovery();
rary 0:9fb816db0c8b 255 }
rary 0:9fb816db0c8b 256 pc.printf("Recovery mode Finish!\r\n");
rary 0:9fb816db0c8b 257 twe.printf("Recovery mode Finish!\r\n");
rary 0:9fb816db0c8b 258 }
rary 0:9fb816db0c8b 259
rary 0:9fb816db0c8b 260
rary 0:9fb816db0c8b 261 //自作関数
rary 0:9fb816db0c8b 262 void get_save_low()
rary 0:9fb816db0c8b 263 {
rary 0:9fb816db0c8b 264 T = t.read(); //時刻データ取得
rary 0:9fb816db0c8b 265 P = lps.data_read(); //気圧データ取得
rary 0:9fb816db0c8b 266 V = p7100.v_read(); //電圧取得
rary 0:9fb816db0c8b 267 TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:9fb816db0c8b 268
rary 0:9fb816db0c8b 269 mpu.accel_read(3, accel);
rary 0:9fb816db0c8b 270 mpu.gyro_read(3, gyro);
rary 0:9fb816db0c8b 271 AX = accel[0];
rary 0:9fb816db0c8b 272 AY = accel[1];
rary 0:9fb816db0c8b 273 AZ = accel[2];
rary 0:9fb816db0c8b 274 GX = gyro[0];
rary 0:9fb816db0c8b 275 GY = gyro[1];
rary 0:9fb816db0c8b 276 GZ = gyro[2];
rary 0:9fb816db0c8b 277
rary 0:9fb816db0c8b 278 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*へ変換
rary 0:9fb816db0c8b 279
rary 0:9fb816db0c8b 280 rom.write_low(pointer_low, s, 64); // write tha data
rary 0:9fb816db0c8b 281 pointer_low = pointer_low + 64; //アドレスずらし
rary 0:9fb816db0c8b 282
rary 0:9fb816db0c8b 283 pc.printf("%s\r\n", s); //check用
rary 0:9fb816db0c8b 284
rary 0:9fb816db0c8b 285 memset(s, '\0', 64 ); //初期化
rary 0:9fb816db0c8b 286 }
rary 0:9fb816db0c8b 287
rary 0:9fb816db0c8b 288 float get_save_high()
rary 0:9fb816db0c8b 289 {
rary 0:9fb816db0c8b 290 T = t.read(); //時刻データ取得
rary 0:9fb816db0c8b 291 P = lps.data_read(); //気圧データ取得
rary 0:9fb816db0c8b 292 V = p7100.v_read(); //電圧取得
rary 0:9fb816db0c8b 293 TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:9fb816db0c8b 294
rary 0:9fb816db0c8b 295 mpu.accel_read(3, accel);
rary 0:9fb816db0c8b 296 mpu.gyro_read(3, gyro);
rary 0:9fb816db0c8b 297 AX = accel[0];
rary 0:9fb816db0c8b 298 AY = accel[1];
rary 0:9fb816db0c8b 299 AZ = accel[2];
rary 0:9fb816db0c8b 300 GX = gyro[0];
rary 0:9fb816db0c8b 301 GY = gyro[1];
rary 0:9fb816db0c8b 302 GZ = gyro[2];
rary 0:9fb816db0c8b 303
rary 0:9fb816db0c8b 304 sprintf(s, "%8.3f %8.3f %6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\r\n", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
rary 0:9fb816db0c8b 305
rary 0:9fb816db0c8b 306 rom.write_high(pointer_high, s, 64); // write tha data
rary 0:9fb816db0c8b 307
rary 0:9fb816db0c8b 308 twe.printf("%8.3f %8.3f %6.3f %5.2f,", T, P, TP, AZ); //無線機送信
rary 0:9fb816db0c8b 309
rary 0:9fb816db0c8b 310 pointer_high = pointer_high + 64; //アドレスずらし
rary 0:9fb816db0c8b 311
rary 0:9fb816db0c8b 312 pc.printf("%s\r\n", s); //check用
rary 0:9fb816db0c8b 313
rary 0:9fb816db0c8b 314 memset(s, '\0', 64 ); //初期化
rary 0:9fb816db0c8b 315
rary 0:9fb816db0c8b 316 return P;
rary 0:9fb816db0c8b 317 }
rary 0:9fb816db0c8b 318
rary 0:9fb816db0c8b 319 void recovery()
rary 0:9fb816db0c8b 320 {
rary 0:9fb816db0c8b 321 T = t.read(); //時刻データ取得
rary 0:9fb816db0c8b 322 V = p7100.v_read(); //電圧取得
rary 0:9fb816db0c8b 323 TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:9fb816db0c8b 324
rary 0:9fb816db0c8b 325 sprintf(s, "%8.3f %6.3f\r\n", T, TP); //floatからchar*へ変換
rary 0:9fb816db0c8b 326
rary 0:9fb816db0c8b 327 rom.write_low(pointer_low, s, 64); // write tha data
rary 0:9fb816db0c8b 328 pointer_low = pointer_low + 64; //アドレスずらし
rary 0:9fb816db0c8b 329
rary 0:9fb816db0c8b 330 pc.printf("%s\r\n", s); //check用
rary 0:9fb816db0c8b 331 twe.printf("%8.3f %6.3f, ", T, TP); //無線機送信
rary 0:9fb816db0c8b 332 memset(s, '\0', 64 ); //初期化
rary 0:9fb816db0c8b 333
rary 0:9fb816db0c8b 334 gps.read_solid(GPS); //測位データ取得
rary 0:9fb816db0c8b 335 lat_north = GPS[0];
rary 0:9fb816db0c8b 336 lon_east = GPS[1];
rary 0:9fb816db0c8b 337
rary 0:9fb816db0c8b 338 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信
rary 0:9fb816db0c8b 339 twe.printf("%f,%f", lat_north, lon_east); //送信
rary 0:9fb816db0c8b 340 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
rary 0:9fb816db0c8b 341 }
rary 0:9fb816db0c8b 342 twe.printf("\r\n"); //無線機トリガー
rary 0:9fb816db0c8b 343 memset(GPS,'\0', 2);
rary 0:9fb816db0c8b 344 }
rary 0:9fb816db0c8b 345
rary 0:9fb816db0c8b 346 void check()
rary 0:9fb816db0c8b 347 {
rary 0:9fb816db0c8b 348 T = t.read(); //時刻データ取得
rary 0:9fb816db0c8b 349 P = lps.data_read(); //気圧データ取得
rary 0:9fb816db0c8b 350 V = p7100.v_read(); //電圧取得
rary 0:9fb816db0c8b 351 TP = 0.25 * V - 0.125; //タンク圧力換算
rary 0:9fb816db0c8b 352
rary 0:9fb816db0c8b 353 mpu.accel_read(3, accel);
rary 0:9fb816db0c8b 354 mpu.gyro_read(3, gyro);
rary 0:9fb816db0c8b 355 AX = accel[0];
rary 0:9fb816db0c8b 356 AY = accel[1];
rary 0:9fb816db0c8b 357 AZ = accel[2];
rary 0:9fb816db0c8b 358 GX = gyro[0];
rary 0:9fb816db0c8b 359 GY = gyro[1];
rary 0:9fb816db0c8b 360 GZ = gyro[2];
rary 0:9fb816db0c8b 361
rary 0:9fb816db0c8b 362 sprintf(s, "%8.3f %8.3f %6.3f %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", T, P, TP, AX, AY, AZ, GX, GY, GZ); //floatからchar*へ変換
rary 0:9fb816db0c8b 363 twe.printf("%s\r\n", s); //送信
rary 0:9fb816db0c8b 364 pc.printf("%s\r\n\r\n", s); //check用
rary 0:9fb816db0c8b 365 memset(s, '\0', 64 ); //初期化
rary 0:9fb816db0c8b 366
rary 0:9fb816db0c8b 367 gps.read_solid(GPS); //測位データ取得
rary 0:9fb816db0c8b 368 lat_north = GPS[0];
rary 0:9fb816db0c8b 369 lon_east = GPS[1];
rary 0:9fb816db0c8b 370
rary 0:9fb816db0c8b 371 if(lat_north > 20 && lat_north < 90 && lon_east > 90 && lon_east < 180) { //緯度が正常な値のときのみ送信
rary 0:9fb816db0c8b 372 twe.printf("%f,%f\r\n", lat_north, lon_east); //送信
rary 0:9fb816db0c8b 373 pc.printf("%f,%f\r\n", lat_north, lon_east); //check用
rary 0:9fb816db0c8b 374 }
rary 0:9fb816db0c8b 375 memset(GPS,'\0', 2);
rary 0:9fb816db0c8b 376 }