新歓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?
User | Revision | Line number | New 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 | } |