SSLM1 / Mbed 2 deprecated 6-3_FlightTest

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@1:d9208e6f6952, 2020-11-17 (annotated)

Committer:
rary
Date:
Tue Nov 17 16:16:50 2020 +0000
Revision:
1:d9208e6f6952
Parent:
0:15c3892eee6b 
20201118

Who changed what in which revision?

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