PQ_Hybrid_Electrical_Equipment_Team / Mbed 2 deprecated rocket_logger_sinkan2018_v1

新歓2018の新入生モデロケ体験でデモフライトとして打ち上げた電装モデロケの電装モジュールのプログラム

Dependencies:   GPS_interrupt LPS25H_lib MadgwickFilter mbed mpu9250_i2c

main.cpp@0:6a155e0d095d, 2018-04-24 (annotated)

Committer:
Sigma884
Date:
Tue Apr 24 12:25:58 2018 +0000
Revision:
0:6a155e0d095d
??2018?????????????????????????????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sigma884 0:6a155e0d095d 1 #include "mbed.h"
Sigma884 0:6a155e0d095d 2 #include "mpu9250_i2c.h"
Sigma884 0:6a155e0d095d 3 #include "MadgwickFilter.hpp"
Sigma884 0:6a155e0d095d 4 #include "LPS25H_lib.h"
Sigma884 0:6a155e0d095d 5 #include "GPS_interrupt.h"
Sigma884 0:6a155e0d095d 6 #include "string.h"
Sigma884 0:6a155e0d095d 7
Sigma884 0:6a155e0d095d 8 /**************************************
Sigma884 0:6a155e0d095d 9 コンストラクタ
Sigma884 0:6a155e0d095d 10 **************************************/
Sigma884 0:6a155e0d095d 11 Serial pc(USBTX, USBRX, 115200);
Sigma884 0:6a155e0d095d 12 Serial GPS(p9, p10, 115200);
Sigma884 0:6a155e0d095d 13
Sigma884 0:6a155e0d095d 14 LocalFileSystem local("local");
Sigma884 0:6a155e0d095d 15
Sigma884 0:6a155e0d095d 16 DigitalOut led_lps(LED1);
Sigma884 0:6a155e0d095d 17 DigitalOut led_mpu(LED2);
Sigma884 0:6a155e0d095d 18 DigitalOut led_gps(LED3);
Sigma884 0:6a155e0d095d 19 DigitalOut led_log(LED4);
Sigma884 0:6a155e0d095d 20 AnalogIn vinread(p15);
Sigma884 0:6a155e0d095d 21 PwmOut buzzer(p21);
Sigma884 0:6a155e0d095d 22 I2C m_i2c(p28, p27);
Sigma884 0:6a155e0d095d 23
Sigma884 0:6a155e0d095d 24 mpu9250 nine(m_i2c, AD0_HIGH);
Sigma884 0:6a155e0d095d 25 MadgwickFilter attitude(1.0);
Sigma884 0:6a155e0d095d 26 LPS25H_lib LPS25H(LPS25H_lib::AD0_HIGH, m_i2c);
Sigma884 0:6a155e0d095d 27 GPS_interrupt mGPS(&GPS, 115200);
Sigma884 0:6a155e0d095d 28
Sigma884 0:6a155e0d095d 29 Ticker tick_print;
Sigma884 0:6a155e0d095d 30 Ticker tick_rec;
Sigma884 0:6a155e0d095d 31 Ticker tick_pipi;
Sigma884 0:6a155e0d095d 32 Ticker tick_gps;
Sigma884 0:6a155e0d095d 33 Ticker tick_lps;
Sigma884 0:6a155e0d095d 34 InterruptIn myButton(p18);
Sigma884 0:6a155e0d095d 35 Timer timer_button;
Sigma884 0:6a155e0d095d 36
Sigma884 0:6a155e0d095d 37 /**************************************
Sigma884 0:6a155e0d095d 38 変数の宣言
Sigma884 0:6a155e0d095d 39 **************************************/
Sigma884 0:6a155e0d095d 40 float gyro[3], gyro_rad[3], acc[3], mag[3], euler[3];
Sigma884 0:6a155e0d095d 41 Quaternion quart(1.0, 0.0, 0.0, 0.0);
Sigma884 0:6a155e0d095d 42 float pres, temp, alt, pres_0, temp_0, speed, pres_old;
Sigma884 0:6a155e0d095d 43 float gps_lat, gps_lon, gps_alt, gps_utc[6], gps_knot, gps_deg;
Sigma884 0:6a155e0d095d 44 int gps_sat;
Sigma884 0:6a155e0d095d 45 float vin;
Sigma884 0:6a155e0d095d 46
Sigma884 0:6a155e0d095d 47 bool pipi_now, save, button, top_flight, sec_flight;
Sigma884 0:6a155e0d095d 48 bool flag_test_gps = true; //GPSの設定をするか
Sigma884 0:6a155e0d095d 49
Sigma884 0:6a155e0d095d 50 FILE *fp;
Sigma884 0:6a155e0d095d 51 int time_button;
Sigma884 0:6a155e0d095d 52
Sigma884 0:6a155e0d095d 53 /**************************************
Sigma884 0:6a155e0d095d 54 関数のプロトタイプ宣言
Sigma884 0:6a155e0d095d 55 **************************************/
Sigma884 0:6a155e0d095d 56 void setup();
Sigma884 0:6a155e0d095d 57 void printData();
Sigma884 0:6a155e0d095d 58 void resetPT();
Sigma884 0:6a155e0d095d 59 void startRec();
Sigma884 0:6a155e0d095d 60 void stopRec();
Sigma884 0:6a155e0d095d 61 void recData();
Sigma884 0:6a155e0d095d 62 void pipi();
Sigma884 0:6a155e0d095d 63 void lvup();
Sigma884 0:6a155e0d095d 64 void tone(float freq);
Sigma884 0:6a155e0d095d 65
Sigma884 0:6a155e0d095d 66 void readGPS();
Sigma884 0:6a155e0d095d 67 void readLPS();
Sigma884 0:6a155e0d095d 68 void readMPU();
Sigma884 0:6a155e0d095d 69 float deg2rad(float deg);
Sigma884 0:6a155e0d095d 70 float rad2deg(float rad);
Sigma884 0:6a155e0d095d 71
Sigma884 0:6a155e0d095d 72 void buttonPush();
Sigma884 0:6a155e0d095d 73 void buttonRelease();
Sigma884 0:6a155e0d095d 74
Sigma884 0:6a155e0d095d 75 /**************************************
Sigma884 0:6a155e0d095d 76 メイン関数
Sigma884 0:6a155e0d095d 77 **************************************/
Sigma884 0:6a155e0d095d 78 int main() {
Sigma884 0:6a155e0d095d 79 setup();
Sigma884 0:6a155e0d095d 80 lvup();
Sigma884 0:6a155e0d095d 81
Sigma884 0:6a155e0d095d 82 myButton.mode(PullUp);
Sigma884 0:6a155e0d095d 83 myButton.fall(&buttonPush);
Sigma884 0:6a155e0d095d 84 myButton.rise(&buttonRelease);
Sigma884 0:6a155e0d095d 85
Sigma884 0:6a155e0d095d 86 tick_print.attach(&printData, 1.0f);
Sigma884 0:6a155e0d095d 87 tick_gps.attach(&readGPS, 0.1f);
Sigma884 0:6a155e0d095d 88 tick_lps.attach(&readLPS, 0.05f);
Sigma884 0:6a155e0d095d 89
Sigma884 0:6a155e0d095d 90 while(1) {
Sigma884 0:6a155e0d095d 91 readMPU();
Sigma884 0:6a155e0d095d 92 }
Sigma884 0:6a155e0d095d 93 }
Sigma884 0:6a155e0d095d 94
Sigma884 0:6a155e0d095d 95 /**************************************
Sigma884 0:6a155e0d095d 96 データの表示
Sigma884 0:6a155e0d095d 97 **************************************/
Sigma884 0:6a155e0d095d 98 void printData(){
Sigma884 0:6a155e0d095d 99 for(int i = 0; i < 20; i ++){
Sigma884 0:6a155e0d095d 100 pc.printf("*");
Sigma884 0:6a155e0d095d 101 }
Sigma884 0:6a155e0d095d 102 pc.printf("\r\n");
Sigma884 0:6a155e0d095d 103 pc.printf("TIME -> %2d:%2d:%02.2f\r\n", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]);
Sigma884 0:6a155e0d095d 104 pc.printf("GPS -> %3.7f(N), %3.7f(E), %.2f[m]\r\n", gps_lat, gps_lon, gps_alt);
Sigma884 0:6a155e0d095d 105 pc.printf("SPEED -> %.2f[knot], %.2f[deg]\r\n", gps_knot, gps_deg);
Sigma884 0:6a155e0d095d 106 pc.printf("SAT -> %d\r\n", gps_sat);
Sigma884 0:6a155e0d095d 107 pc.printf("LPS25H -> %.4f[hPa], %.2f[degree], %.2f[m]\r\n", pres, temp, alt);
Sigma884 0:6a155e0d095d 108 pc.printf("SPEED -> %.4f[m/s]\r\n", speed);
Sigma884 0:6a155e0d095d 109 pc.printf("BATTERY -> %.2f[V]\r\n", vin);
Sigma884 0:6a155e0d095d 110 pc.printf("GYRO -> %.4f, %.4f, %.4f\r\n", gyro[0], gyro[1], gyro[2]);
Sigma884 0:6a155e0d095d 111 pc.printf("ACC -> %.4f, %.4f, %.4f\r\n", acc[0], acc[1], acc[2]);
Sigma884 0:6a155e0d095d 112 pc.printf("MAG -> %.2f, %.2f, %.2f\r\n", mag[0], mag[1], mag[2]);
Sigma884 0:6a155e0d095d 113 pc.printf("QUART -> %f, %f, %f, %f\r\n", quart.w, quart.x, quart.y, quart.z);
Sigma884 0:6a155e0d095d 114 pc.printf("EULER -> %f, %f, %f\r\n", euler[0], euler[1], euler[2]);
Sigma884 0:6a155e0d095d 115 pc.printf("TOP -> %d, %d\r\n", top_flight, sec_flight);
Sigma884 0:6a155e0d095d 116 pc.printf("SAVE -> %d\r\n\n\n", save);
Sigma884 0:6a155e0d095d 117 }
Sigma884 0:6a155e0d095d 118
Sigma884 0:6a155e0d095d 119 /**************************************
Sigma884 0:6a155e0d095d 120 高度のリセット
Sigma884 0:6a155e0d095d 121 **************************************/
Sigma884 0:6a155e0d095d 122 void resetPT(){
Sigma884 0:6a155e0d095d 123 pres_0 = pres;
Sigma884 0:6a155e0d095d 124 temp_0 = temp;
Sigma884 0:6a155e0d095d 125 lvup();
Sigma884 0:6a155e0d095d 126 }
Sigma884 0:6a155e0d095d 127
Sigma884 0:6a155e0d095d 128 /**************************************
Sigma884 0:6a155e0d095d 129 記録開始
Sigma884 0:6a155e0d095d 130 **************************************/
Sigma884 0:6a155e0d095d 131 void startRec(){
Sigma884 0:6a155e0d095d 132 fp = fopen("/local/rocketlog.txt", "a");
Sigma884 0:6a155e0d095d 133 pc.printf("START SAVING\r\n");
Sigma884 0:6a155e0d095d 134 led_log = 1;
Sigma884 0:6a155e0d095d 135 wait(1.0f);
Sigma884 0:6a155e0d095d 136 }
Sigma884 0:6a155e0d095d 137
Sigma884 0:6a155e0d095d 138 /**************************************
Sigma884 0:6a155e0d095d 139 記録終了
Sigma884 0:6a155e0d095d 140 **************************************/
Sigma884 0:6a155e0d095d 141 void stopRec(){
Sigma884 0:6a155e0d095d 142 fprintf(fp, "\r\n\n");
Sigma884 0:6a155e0d095d 143 fclose(fp);
Sigma884 0:6a155e0d095d 144 pc.printf("STOP SAVING\r\n");
Sigma884 0:6a155e0d095d 145 led_log = 0;
Sigma884 0:6a155e0d095d 146 wait(1.0f);
Sigma884 0:6a155e0d095d 147 }
Sigma884 0:6a155e0d095d 148
Sigma884 0:6a155e0d095d 149 /**************************************
Sigma884 0:6a155e0d095d 150 記録書き込み
Sigma884 0:6a155e0d095d 151 **************************************/
Sigma884 0:6a155e0d095d 152 void recData(){
Sigma884 0:6a155e0d095d 153 fprintf(fp, "%d/%d/%d,", (int)gps_utc[0], (int)gps_utc[1], (int)gps_utc[2]); //日付
Sigma884 0:6a155e0d095d 154 fprintf(fp, "%2d:%2d:%02.2f,", (int)gps_utc[3], (int)gps_utc[4], gps_utc[5]); //時間
Sigma884 0:6a155e0d095d 155 fprintf(fp, "%3.7f,%3.7f,%.2f,", gps_lat, gps_lon, gps_alt); //GPS座標
Sigma884 0:6a155e0d095d 156 fprintf(fp, "%.2f,%.2f,", gps_knot, gps_deg); //GPS速度
Sigma884 0:6a155e0d095d 157 fprintf(fp, "%d,", gps_sat); //GPS衛星数
Sigma884 0:6a155e0d095d 158 fprintf(fp, "%.4f,%.2f,%.2f,", pres, temp, alt); //気圧センサ
Sigma884 0:6a155e0d095d 159 fprintf(fp, "%.4f,", speed); //Z軸速度
Sigma884 0:6a155e0d095d 160 fprintf(fp, "%.2f,", vin); //電源電圧
Sigma884 0:6a155e0d095d 161 fprintf(fp, "%.4f,%.4f,%.4f,", gyro[0], gyro[1], gyro[2]); //ジャイロ
Sigma884 0:6a155e0d095d 162 fprintf(fp, "%.4f,%.4f,%.4f,", acc[0], acc[1], acc[2]); //加速度
Sigma884 0:6a155e0d095d 163 fprintf(fp, "%.2f,%.2f,%.2f,", mag[0], mag[1], mag[2]); //地磁気
Sigma884 0:6a155e0d095d 164 fprintf(fp, "%f,%f,%f,%f,", quart.w, quart.x, quart.y, quart.z); //クオータニオン
Sigma884 0:6a155e0d095d 165 fprintf(fp, "%f,%f,%f,", euler[0], euler[1], euler[2]); //姿勢角
Sigma884 0:6a155e0d095d 166 fprintf(fp, "%d,%d\r\n", top_flight, sec_flight); //頂点検知
Sigma884 0:6a155e0d095d 167 }
Sigma884 0:6a155e0d095d 168
Sigma884 0:6a155e0d095d 169 /**************************************
Sigma884 0:6a155e0d095d 170 ブザー
Sigma884 0:6a155e0d095d 171 **************************************/
Sigma884 0:6a155e0d095d 172 void pipi(){
Sigma884 0:6a155e0d095d 173 if(pipi_now){
Sigma884 0:6a155e0d095d 174 buzzer = 0.0f;
Sigma884 0:6a155e0d095d 175 pipi_now = !pipi_now;
Sigma884 0:6a155e0d095d 176 }
Sigma884 0:6a155e0d095d 177 else{
Sigma884 0:6a155e0d095d 178 float T = 1.0f / (4.0*440.000f);
Sigma884 0:6a155e0d095d 179 buzzer.period(T);
Sigma884 0:6a155e0d095d 180 buzzer = 0.5f;
Sigma884 0:6a155e0d095d 181 pipi_now = !pipi_now;
Sigma884 0:6a155e0d095d 182 }
Sigma884 0:6a155e0d095d 183 }
Sigma884 0:6a155e0d095d 184
Sigma884 0:6a155e0d095d 185 /**************************************
Sigma884 0:6a155e0d095d 186 レベルアップ
Sigma884 0:6a155e0d095d 187 **************************************/
Sigma884 0:6a155e0d095d 188 void lvup(){
Sigma884 0:6a155e0d095d 189 for(int i = 1; i <= 4; i ++){
Sigma884 0:6a155e0d095d 190 tone(4.0*349.228f);
Sigma884 0:6a155e0d095d 191 wait(0.15f);
Sigma884 0:6a155e0d095d 192 }
Sigma884 0:6a155e0d095d 193 tone(0.0f);
Sigma884 0:6a155e0d095d 194 wait(0.15f);
Sigma884 0:6a155e0d095d 195 tone(4.0*311.127f);
Sigma884 0:6a155e0d095d 196 wait(0.15f);
Sigma884 0:6a155e0d095d 197 tone(0.0f);
Sigma884 0:6a155e0d095d 198 wait(0.15f);
Sigma884 0:6a155e0d095d 199 tone(4.0*391.995f);
Sigma884 0:6a155e0d095d 200 wait(0.15f);
Sigma884 0:6a155e0d095d 201 tone(0.0f);
Sigma884 0:6a155e0d095d 202 wait(0.15f);
Sigma884 0:6a155e0d095d 203 tone(4.0*349.228f);
Sigma884 0:6a155e0d095d 204 wait(1.0f);
Sigma884 0:6a155e0d095d 205 tone(0.0f);
Sigma884 0:6a155e0d095d 206 wait(1.0f);
Sigma884 0:6a155e0d095d 207 }
Sigma884 0:6a155e0d095d 208
Sigma884 0:6a155e0d095d 209 /**************************************
Sigma884 0:6a155e0d095d 210 音を鳴らす
Sigma884 0:6a155e0d095d 211 **************************************/
Sigma884 0:6a155e0d095d 212 void tone(float freq){
Sigma884 0:6a155e0d095d 213 float T = 0;
Sigma884 0:6a155e0d095d 214
Sigma884 0:6a155e0d095d 215 if(freq == 0.0f){
Sigma884 0:6a155e0d095d 216 buzzer = 0.0f;
Sigma884 0:6a155e0d095d 217 }
Sigma884 0:6a155e0d095d 218
Sigma884 0:6a155e0d095d 219 T = 1.0f / freq;
Sigma884 0:6a155e0d095d 220 buzzer.period(T);
Sigma884 0:6a155e0d095d 221 buzzer = 0.5f;
Sigma884 0:6a155e0d095d 222 }
Sigma884 0:6a155e0d095d 223
Sigma884 0:6a155e0d095d 224 /**************************************
Sigma884 0:6a155e0d095d 225 GPS読み取り
Sigma884 0:6a155e0d095d 226 **************************************/
Sigma884 0:6a155e0d095d 227 void readGPS(){
Sigma884 0:6a155e0d095d 228 gps_lat = mGPS.Latitude();
Sigma884 0:6a155e0d095d 229 gps_lon = mGPS.Longitude();
Sigma884 0:6a155e0d095d 230 gps_alt = mGPS.Height();
Sigma884 0:6a155e0d095d 231 mGPS.getUTC(gps_utc);
Sigma884 0:6a155e0d095d 232 gps_utc[3] += 9;
Sigma884 0:6a155e0d095d 233 if(gps_utc[3] >= 24){
Sigma884 0:6a155e0d095d 234 gps_utc[3] -= 24;
Sigma884 0:6a155e0d095d 235 }
Sigma884 0:6a155e0d095d 236 gps_knot = mGPS.Knot();
Sigma884 0:6a155e0d095d 237 gps_deg = mGPS.Degree();
Sigma884 0:6a155e0d095d 238 gps_sat = mGPS.Number();
Sigma884 0:6a155e0d095d 239
Sigma884 0:6a155e0d095d 240 vin = vinread.read() * 8.4;
Sigma884 0:6a155e0d095d 241 }
Sigma884 0:6a155e0d095d 242
Sigma884 0:6a155e0d095d 243 /**************************************
Sigma884 0:6a155e0d095d 244 気圧センサー読み取り・速度計算
Sigma884 0:6a155e0d095d 245 **************************************/
Sigma884 0:6a155e0d095d 246 void readLPS(){
Sigma884 0:6a155e0d095d 247 pres_old = pres;
Sigma884 0:6a155e0d095d 248 pres = LPS25H.getPres();
Sigma884 0:6a155e0d095d 249 temp = LPS25H.getTemp();
Sigma884 0:6a155e0d095d 250 alt = LPS25H.getAlt(pres_0, temp_0);
Sigma884 0:6a155e0d095d 251 speed = (pres - pres_old) / 0.05;
Sigma884 0:6a155e0d095d 252
Sigma884 0:6a155e0d095d 253 int top_c = 0;
Sigma884 0:6a155e0d095d 254 if(alt > 70 && speed < 3.0f && !top_flight){
Sigma884 0:6a155e0d095d 255 top_c ++;
Sigma884 0:6a155e0d095d 256 if(top_c > 10){
Sigma884 0:6a155e0d095d 257 top_c = 0;
Sigma884 0:6a155e0d095d 258 top_flight = true;
Sigma884 0:6a155e0d095d 259 }
Sigma884 0:6a155e0d095d 260 }
Sigma884 0:6a155e0d095d 261 if(alt < 30 && speed < 0.0f && top_flight && !sec_flight){
Sigma884 0:6a155e0d095d 262 top_c ++;
Sigma884 0:6a155e0d095d 263 if(top_c > 10){
Sigma884 0:6a155e0d095d 264 top_c = 0;
Sigma884 0:6a155e0d095d 265 sec_flight = true;
Sigma884 0:6a155e0d095d 266 }
Sigma884 0:6a155e0d095d 267 }
Sigma884 0:6a155e0d095d 268 }
Sigma884 0:6a155e0d095d 269
Sigma884 0:6a155e0d095d 270 /**************************************
Sigma884 0:6a155e0d095d 271 9軸センサー読み取り・姿勢計算
Sigma884 0:6a155e0d095d 272 **************************************/
Sigma884 0:6a155e0d095d 273 void readMPU(){
Sigma884 0:6a155e0d095d 274 nine.getGyro(gyro);
Sigma884 0:6a155e0d095d 275 nine.getAcc(acc);
Sigma884 0:6a155e0d095d 276 nine.getMag(mag);
Sigma884 0:6a155e0d095d 277
Sigma884 0:6a155e0d095d 278 for(int i = 0; i < 3; i ++){
Sigma884 0:6a155e0d095d 279 gyro_rad[i] = deg2rad(gyro[i]);
Sigma884 0:6a155e0d095d 280 }
Sigma884 0:6a155e0d095d 281 attitude.MadgwickAHRSupdate(gyro_rad, acc, mag);
Sigma884 0:6a155e0d095d 282 attitude.getAttitude(&quart);
Sigma884 0:6a155e0d095d 283 attitude.getEulerAngle(euler);
Sigma884 0:6a155e0d095d 284 for(int i = 0; i < 3; i ++){
Sigma884 0:6a155e0d095d 285 euler[i] = rad2deg(euler[i]);
Sigma884 0:6a155e0d095d 286 }
Sigma884 0:6a155e0d095d 287 }
Sigma884 0:6a155e0d095d 288
Sigma884 0:6a155e0d095d 289 /**************************************
Sigma884 0:6a155e0d095d 290 度数法 -> 弧度法
Sigma884 0:6a155e0d095d 291 **************************************/
Sigma884 0:6a155e0d095d 292 float deg2rad(float deg){
Sigma884 0:6a155e0d095d 293 return deg * 0.017453292519943; //* 3.1415926535 / 180;
Sigma884 0:6a155e0d095d 294 }
Sigma884 0:6a155e0d095d 295
Sigma884 0:6a155e0d095d 296 /**************************************
Sigma884 0:6a155e0d095d 297 弧度法 -> 度数法
Sigma884 0:6a155e0d095d 298 **************************************/
Sigma884 0:6a155e0d095d 299 float rad2deg(float rad){
Sigma884 0:6a155e0d095d 300 return rad * 57.29577951308232; //* 180 / 3.145926535;
Sigma884 0:6a155e0d095d 301 }
Sigma884 0:6a155e0d095d 302
Sigma884 0:6a155e0d095d 303 /**************************************
Sigma884 0:6a155e0d095d 304 ボタンが押されたとき
Sigma884 0:6a155e0d095d 305 **************************************/
Sigma884 0:6a155e0d095d 306 void buttonPush(){
Sigma884 0:6a155e0d095d 307 if(!button){
Sigma884 0:6a155e0d095d 308 button = true;
Sigma884 0:6a155e0d095d 309 timer_button.reset();
Sigma884 0:6a155e0d095d 310 timer_button.start();
Sigma884 0:6a155e0d095d 311 }
Sigma884 0:6a155e0d095d 312 }
Sigma884 0:6a155e0d095d 313
Sigma884 0:6a155e0d095d 314 /**************************************
Sigma884 0:6a155e0d095d 315 ボタンが離されたとき
Sigma884 0:6a155e0d095d 316 **************************************/
Sigma884 0:6a155e0d095d 317 void buttonRelease(){
Sigma884 0:6a155e0d095d 318 if(button){
Sigma884 0:6a155e0d095d 319 button = false;
Sigma884 0:6a155e0d095d 320 time_button = timer_button.read();
Sigma884 0:6a155e0d095d 321 timer_button.stop();
Sigma884 0:6a155e0d095d 322 if(time_button >= 2){
Sigma884 0:6a155e0d095d 323 if(!save){
Sigma884 0:6a155e0d095d 324 save = true;
Sigma884 0:6a155e0d095d 325 startRec();
Sigma884 0:6a155e0d095d 326 tick_rec.detach();
Sigma884 0:6a155e0d095d 327 tick_rec.attach(&recData, 0.05f);
Sigma884 0:6a155e0d095d 328 tick_pipi.attach(&pipi, 0.5f);
Sigma884 0:6a155e0d095d 329 }
Sigma884 0:6a155e0d095d 330 else{
Sigma884 0:6a155e0d095d 331 save = false;
Sigma884 0:6a155e0d095d 332 stopRec();
Sigma884 0:6a155e0d095d 333 tick_rec.detach();
Sigma884 0:6a155e0d095d 334 tick_pipi.detach();
Sigma884 0:6a155e0d095d 335 tone(0.0f);
Sigma884 0:6a155e0d095d 336 }
Sigma884 0:6a155e0d095d 337 }
Sigma884 0:6a155e0d095d 338 else{
Sigma884 0:6a155e0d095d 339 resetPT();
Sigma884 0:6a155e0d095d 340 }
Sigma884 0:6a155e0d095d 341 }
Sigma884 0:6a155e0d095d 342 }
Sigma884 0:6a155e0d095d 343
Sigma884 0:6a155e0d095d 344 /**************************************
Sigma884 0:6a155e0d095d 345 セットアップ（最初に1回実行）
Sigma884 0:6a155e0d095d 346 **************************************/
Sigma884 0:6a155e0d095d 347 void setup(){
Sigma884 0:6a155e0d095d 348 wait(0.5f);
Sigma884 0:6a155e0d095d 349 char setup_string[1024];
Sigma884 0:6a155e0d095d 350
Sigma884 0:6a155e0d095d 351 pc.printf("\r\n\nSetting Start\r\n");//
Sigma884 0:6a155e0d095d 352 strcat(setup_string, "\r\n\nSetting Start\r\n");
Sigma884 0:6a155e0d095d 353
Sigma884 0:6a155e0d095d 354 LPS25H.begin(25);
Sigma884 0:6a155e0d095d 355 LPS25H.setFIFO(16);
Sigma884 0:6a155e0d095d 356 if(LPS25H.whoAmI() == 0){
Sigma884 0:6a155e0d095d 357 pc.printf("LPS25H OK!!\r\n");//
Sigma884 0:6a155e0d095d 358 strcat(setup_string, "LPS25H OK!!\r\n");
Sigma884 0:6a155e0d095d 359 led_lps = 1;
Sigma884 0:6a155e0d095d 360 }
Sigma884 0:6a155e0d095d 361 else{
Sigma884 0:6a155e0d095d 362 pc.printf("LPS25H NG...\r\n");//
Sigma884 0:6a155e0d095d 363 strcat(setup_string, "LPS25H NG...\r\n");
Sigma884 0:6a155e0d095d 364 led_lps = 0;
Sigma884 0:6a155e0d095d 365 }
Sigma884 0:6a155e0d095d 366
Sigma884 0:6a155e0d095d 367 nine.setAcc(_16G);
Sigma884 0:6a155e0d095d 368 nine.setGyro(_2000DPS);
Sigma884 0:6a155e0d095d 369 nine.setOffset(-2.17859, -0.11247, 0.924360,
Sigma884 0:6a155e0d095d 370 0.005295, 0.019147, 0.056065,
Sigma884 0:6a155e0d095d 371 49.425, -5.55, 75.15);
Sigma884 0:6a155e0d095d 372 int check_nine = 0;
Sigma884 0:6a155e0d095d 373 if(nine.senserTest()){
Sigma884 0:6a155e0d095d 374 pc.printf("MPU9250 OK!!\r\n");//
Sigma884 0:6a155e0d095d 375 strcat(setup_string, "MPU9250 OK!!\r\n");
Sigma884 0:6a155e0d095d 376 check_nine ++;
Sigma884 0:6a155e0d095d 377 }
Sigma884 0:6a155e0d095d 378 else{
Sigma884 0:6a155e0d095d 379 pc.printf("MPU9250 NG...\r\n");//
Sigma884 0:6a155e0d095d 380 strcat(setup_string, "MPU9250 NG...\r\n");
Sigma884 0:6a155e0d095d 381 }
Sigma884 0:6a155e0d095d 382 if(nine.mag_senserTest()){
Sigma884 0:6a155e0d095d 383 pc.printf("MPU9250 MAG OK!!\r\n");//
Sigma884 0:6a155e0d095d 384 strcat(setup_string, "MPU9250 MAG OK!!\r\n");
Sigma884 0:6a155e0d095d 385 check_nine ++;
Sigma884 0:6a155e0d095d 386 }
Sigma884 0:6a155e0d095d 387 else{
Sigma884 0:6a155e0d095d 388 pc.printf("MPU9250 MAG NG...\r\n");//
Sigma884 0:6a155e0d095d 389 strcat(setup_string, "MPU9250 MAG NG...\r\n");
Sigma884 0:6a155e0d095d 390 }
Sigma884 0:6a155e0d095d 391 if(check_nine == 2){
Sigma884 0:6a155e0d095d 392 led_mpu = 1;
Sigma884 0:6a155e0d095d 393 }
Sigma884 0:6a155e0d095d 394 else{
Sigma884 0:6a155e0d095d 395 led_mpu = 0;
Sigma884 0:6a155e0d095d 396 }
Sigma884 0:6a155e0d095d 397
Sigma884 0:6a155e0d095d 398 if(flag_test_gps){
Sigma884 0:6a155e0d095d 399 pc.printf("GPS setting");//
Sigma884 0:6a155e0d095d 400 strcat(setup_string, "GPS setting");
Sigma884 0:6a155e0d095d 401 led_gps = 0;
Sigma884 0:6a155e0d095d 402
Sigma884 0:6a155e0d095d 403 while(!mGPS.gps_readable){
Sigma884 0:6a155e0d095d 404 pc.printf(".");//
Sigma884 0:6a155e0d095d 405 strcat(setup_string, ".");
Sigma884 0:6a155e0d095d 406 wait(1.0f);
Sigma884 0:6a155e0d095d 407 }
Sigma884 0:6a155e0d095d 408
Sigma884 0:6a155e0d095d 409 pc.printf(" Finished!!\r\n");//
Sigma884 0:6a155e0d095d 410 strcat(setup_string, " Finished!!\r\n");
Sigma884 0:6a155e0d095d 411 led_gps = 1;
Sigma884 0:6a155e0d095d 412 }
Sigma884 0:6a155e0d095d 413
Sigma884 0:6a155e0d095d 414 pc.printf("All setting finished!! -> Start!!\r\n");//
Sigma884 0:6a155e0d095d 415 strcat(setup_string, "All setting finished!! -> Start!!\r\n");
Sigma884 0:6a155e0d095d 416
Sigma884 0:6a155e0d095d 417 led_log = 1;
Sigma884 0:6a155e0d095d 418 fp = fopen("/local/rocketlog.txt", "a");
Sigma884 0:6a155e0d095d 419 fprintf(fp, setup_string);
Sigma884 0:6a155e0d095d 420 fclose(fp);
Sigma884 0:6a155e0d095d 421 setup_string[0] = NULL;
Sigma884 0:6a155e0d095d 422 led_log = 0;
Sigma884 0:6a155e0d095d 423 }