imu/gnss logger ver1.1
Dependencies: mbed MPU9250_SPI MG354PDH0 SDFileSystem
main.cpp@3:d564c0a27ba7, 2021-08-29 (annotated)
- Committer:
- Joeatsumi
- Date:
- Sun Aug 29 02:07:08 2021 +0000
- Revision:
- 3:d564c0a27ba7
- Parent:
- 2:9216162a9e17
- Child:
- 4:d49256697f27
We added pressure, Humidity, tempreture sensor to INS_GNSS logger.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Joeatsumi | 0:91d5f51f73e2 | 1 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 2 | //GNSS logger with ublox-NEO7M |
Joeatsumi | 0:91d5f51f73e2 | 3 | //MPU board: mbed LPC1768 |
Joeatsumi | 0:91d5f51f73e2 | 4 | //GNSS module: ublox-NEO7M |
Joeatsumi | 2:9216162a9e17 | 5 | //2021/04/28 A.Toda |
Joeatsumi | 0:91d5f51f73e2 | 6 | //======================================================== |
Joeatsumi | 0:91d5f51f73e2 | 7 | #include "mbed.h" |
Joeatsumi | 1:8757d12d193b | 8 | #include "SDFileSystem.h" |
Joeatsumi | 2:9216162a9e17 | 9 | #include "MG354PDH0.h" |
Joeatsumi | 0:91d5f51f73e2 | 10 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 11 | //Port Setting |
Joeatsumi | 0:91d5f51f73e2 | 12 | Serial pc(USBTX, USBRX); // tx, rx |
Joeatsumi | 3:d564c0a27ba7 | 13 | Serial epson_imu(p9, p10); // IMU通信用シリアルポート |
Joeatsumi | 0:91d5f51f73e2 | 14 | SPI spi(p11, p12, p13); // mosi, miso, sclk |
Joeatsumi | 2:9216162a9e17 | 15 | |
Joeatsumi | 3:d564c0a27ba7 | 16 | DigitalOut myled_1(LED1); |
Joeatsumi | 3:d564c0a27ba7 | 17 | DigitalOut myled_2(LED2); |
Joeatsumi | 3:d564c0a27ba7 | 18 | DigitalOut myled_3(LED3); |
Joeatsumi | 3:d564c0a27ba7 | 19 | DigitalOut myled_4(LED4); |
Joeatsumi | 3:d564c0a27ba7 | 20 | |
Joeatsumi | 3:d564c0a27ba7 | 21 | DigitalOut CS(p14); // NEO-7MのCSピン |
Joeatsumi | 3:d564c0a27ba7 | 22 | DigitalOut BME_280_CS(p26); // BME280のCSピン |
Joeatsumi | 3:d564c0a27ba7 | 23 | |
Joeatsumi | 3:d564c0a27ba7 | 24 | DigitalOut log_low(p23); |
Joeatsumi | 3:d564c0a27ba7 | 25 | DigitalOut log_high(p22); |
Joeatsumi | 3:d564c0a27ba7 | 26 | DigitalIn log_switch(p15); |
Joeatsumi | 3:d564c0a27ba7 | 27 | |
Joeatsumi | 3:d564c0a27ba7 | 28 | AnalogIn thermopile_input_1(p20); |
Joeatsumi | 3:d564c0a27ba7 | 29 | AnalogIn thermopile_input_2(p19); |
Joeatsumi | 1:8757d12d193b | 30 | |
Joeatsumi | 2:9216162a9e17 | 31 | MG354PDH0 imu(&epson_imu); // IMU |
Joeatsumi | 1:8757d12d193b | 32 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs |
Joeatsumi | 1:8757d12d193b | 33 | |
Joeatsumi | 1:8757d12d193b | 34 | //ファイルポインタ |
Joeatsumi | 1:8757d12d193b | 35 | FILE *fp; |
Joeatsumi | 1:8757d12d193b | 36 | FILE *im; |
Joeatsumi | 3:d564c0a27ba7 | 37 | FILE *th; |
Joeatsumi | 0:91d5f51f73e2 | 38 | |
Joeatsumi | 0:91d5f51f73e2 | 39 | //========================================================= |
Joeatsumi | 2:9216162a9e17 | 40 | //IMUの変数 |
Joeatsumi | 2:9216162a9e17 | 41 | float gyro_val[3];//角速度の値 |
Joeatsumi | 2:9216162a9e17 | 42 | float acc_val[3];//加速度の値 |
Joeatsumi | 2:9216162a9e17 | 43 | |
Joeatsumi | 2:9216162a9e17 | 44 | //========================================================= |
Joeatsumi | 3:d564c0a27ba7 | 45 | //サーモパイルセンサの変数 |
Joeatsumi | 3:d564c0a27ba7 | 46 | float thermopile_voltage_1; |
Joeatsumi | 3:d564c0a27ba7 | 47 | float thermopile_voltage_2; |
Joeatsumi | 3:d564c0a27ba7 | 48 | |
Joeatsumi | 3:d564c0a27ba7 | 49 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 50 | //受信したメッセージから抽出したい情報 |
Joeatsumi | 0:91d5f51f73e2 | 51 | float latitude,longitude,height_float; //緯度、経度、高度 |
Joeatsumi | 0:91d5f51f73e2 | 52 | int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である |
Joeatsumi | 0:91d5f51f73e2 | 53 | float velN_float,velE_float,velD_float; // NED座標系に置ける速度 |
Joeatsumi | 0:91d5f51f73e2 | 54 | |
Joeatsumi | 0:91d5f51f73e2 | 55 | //========================================================= |
Joeatsumi | 3:d564c0a27ba7 | 56 | //BME280の変数 |
Joeatsumi | 3:d564c0a27ba7 | 57 | const unsigned char BME280_SPI_MASK = 0x7F; |
Joeatsumi | 3:d564c0a27ba7 | 58 | uint16_t dig_T1; |
Joeatsumi | 3:d564c0a27ba7 | 59 | int16_t dig_T2, dig_T3; |
Joeatsumi | 3:d564c0a27ba7 | 60 | uint16_t dig_P1; |
Joeatsumi | 3:d564c0a27ba7 | 61 | int16_t dig_P2, dig_P3, dig_P4, dig_P5, dig_P6, dig_P7, dig_P8, dig_P9; |
Joeatsumi | 3:d564c0a27ba7 | 62 | uint16_t dig_H1, dig_H3; |
Joeatsumi | 3:d564c0a27ba7 | 63 | int16_t dig_H2, dig_H4, dig_H5, dig_H6; |
Joeatsumi | 3:d564c0a27ba7 | 64 | int32_t t_fine; |
Joeatsumi | 3:d564c0a27ba7 | 65 | |
Joeatsumi | 3:d564c0a27ba7 | 66 | float bem280_tempreture; |
Joeatsumi | 3:d564c0a27ba7 | 67 | float bem280_humidity; |
Joeatsumi | 3:d564c0a27ba7 | 68 | float bem280_pressure; |
Joeatsumi | 3:d564c0a27ba7 | 69 | |
Joeatsumi | 3:d564c0a27ba7 | 70 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 71 | //UBXデータを処理したかどうかのフラグ |
Joeatsumi | 0:91d5f51f73e2 | 72 | int flag_posllh,flag_velned; |
Joeatsumi | 0:91d5f51f73e2 | 73 | |
Joeatsumi | 0:91d5f51f73e2 | 74 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 75 | //処理時間計測の為のタイマー |
Joeatsumi | 0:91d5f51f73e2 | 76 | Timer processing_timer; |
Joeatsumi | 0:91d5f51f73e2 | 77 | |
Joeatsumi | 0:91d5f51f73e2 | 78 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 79 | //処理時間 |
Joeatsumi | 2:9216162a9e17 | 80 | int processed_time,processed_time_before,processed_time_after; |
Joeatsumi | 2:9216162a9e17 | 81 | float measurement_time_g; |
Joeatsumi | 0:91d5f51f73e2 | 82 | |
Joeatsumi | 0:91d5f51f73e2 | 83 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 84 | //Ticker |
Joeatsumi | 0:91d5f51f73e2 | 85 | Ticker timer1; // |
Joeatsumi | 0:91d5f51f73e2 | 86 | Ticker timer2; // |
Joeatsumi | 0:91d5f51f73e2 | 87 | |
Joeatsumi | 0:91d5f51f73e2 | 88 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 89 | //Logging variables |
Joeatsumi | 1:8757d12d193b | 90 | float imu_mesurement_freq = 200.0; //Hz |
Joeatsumi | 1:8757d12d193b | 91 | float gnss_mesurement_freq = 5.0; //theta_update_freq; |
Joeatsumi | 0:91d5f51f73e2 | 92 | |
Joeatsumi | 0:91d5f51f73e2 | 93 | float imu_interval = 1.0f/imu_mesurement_freq; //sec |
Joeatsumi | 0:91d5f51f73e2 | 94 | float gnss_interval = 1.0f/gnss_mesurement_freq; //sec |
Joeatsumi | 0:91d5f51f73e2 | 95 | |
Joeatsumi | 1:8757d12d193b | 96 | int logging_status; |
Joeatsumi | 0:91d5f51f73e2 | 97 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 98 | //Header char |
Joeatsumi | 0:91d5f51f73e2 | 99 | const unsigned char UBX_HEADER[] = { 0xB5, 0x62 }; |
Joeatsumi | 0:91d5f51f73e2 | 100 | const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 }; |
Joeatsumi | 0:91d5f51f73e2 | 101 | const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 }; |
Joeatsumi | 0:91d5f51f73e2 | 102 | |
Joeatsumi | 0:91d5f51f73e2 | 103 | const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 }; |
Joeatsumi | 0:91d5f51f73e2 | 104 | |
Joeatsumi | 0:91d5f51f73e2 | 105 | enum _ubxMsgType { |
Joeatsumi | 0:91d5f51f73e2 | 106 | MT_NONE, |
Joeatsumi | 0:91d5f51f73e2 | 107 | MT_NAV_POSLLH, |
Joeatsumi | 0:91d5f51f73e2 | 108 | MT_NAV_STATUS, |
Joeatsumi | 0:91d5f51f73e2 | 109 | MT_NAV_VELNED |
Joeatsumi | 0:91d5f51f73e2 | 110 | }; |
Joeatsumi | 0:91d5f51f73e2 | 111 | |
Joeatsumi | 0:91d5f51f73e2 | 112 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 113 | //メッセージの構造体 |
Joeatsumi | 0:91d5f51f73e2 | 114 | struct NAV_POSLLH { |
Joeatsumi | 0:91d5f51f73e2 | 115 | unsigned char cls; |
Joeatsumi | 0:91d5f51f73e2 | 116 | unsigned char id; |
Joeatsumi | 0:91d5f51f73e2 | 117 | unsigned short len; |
Joeatsumi | 0:91d5f51f73e2 | 118 | unsigned long iTOW; |
Joeatsumi | 0:91d5f51f73e2 | 119 | long lon; |
Joeatsumi | 0:91d5f51f73e2 | 120 | long lat; |
Joeatsumi | 0:91d5f51f73e2 | 121 | long height; |
Joeatsumi | 0:91d5f51f73e2 | 122 | long hMSL; |
Joeatsumi | 0:91d5f51f73e2 | 123 | unsigned long hAcc; |
Joeatsumi | 0:91d5f51f73e2 | 124 | unsigned long vAcc; |
Joeatsumi | 0:91d5f51f73e2 | 125 | }; |
Joeatsumi | 0:91d5f51f73e2 | 126 | |
Joeatsumi | 0:91d5f51f73e2 | 127 | struct NAV_STATUS { |
Joeatsumi | 0:91d5f51f73e2 | 128 | unsigned char cls; |
Joeatsumi | 0:91d5f51f73e2 | 129 | unsigned char id; |
Joeatsumi | 0:91d5f51f73e2 | 130 | unsigned short len; |
Joeatsumi | 0:91d5f51f73e2 | 131 | unsigned long iTOW; |
Joeatsumi | 0:91d5f51f73e2 | 132 | unsigned char gpsFix; |
Joeatsumi | 0:91d5f51f73e2 | 133 | char flags; |
Joeatsumi | 0:91d5f51f73e2 | 134 | char fixStat; |
Joeatsumi | 0:91d5f51f73e2 | 135 | char flags2; |
Joeatsumi | 0:91d5f51f73e2 | 136 | unsigned long ttff; |
Joeatsumi | 0:91d5f51f73e2 | 137 | unsigned long msss; |
Joeatsumi | 0:91d5f51f73e2 | 138 | }; |
Joeatsumi | 0:91d5f51f73e2 | 139 | |
Joeatsumi | 0:91d5f51f73e2 | 140 | struct NAV_VELNED { |
Joeatsumi | 0:91d5f51f73e2 | 141 | unsigned char cls; |
Joeatsumi | 0:91d5f51f73e2 | 142 | unsigned char id; |
Joeatsumi | 0:91d5f51f73e2 | 143 | unsigned short len; |
Joeatsumi | 0:91d5f51f73e2 | 144 | unsigned long iTOW; |
Joeatsumi | 0:91d5f51f73e2 | 145 | signed long velN; |
Joeatsumi | 0:91d5f51f73e2 | 146 | signed long velE; |
Joeatsumi | 0:91d5f51f73e2 | 147 | signed long velD; |
Joeatsumi | 0:91d5f51f73e2 | 148 | unsigned long speed; |
Joeatsumi | 0:91d5f51f73e2 | 149 | unsigned long gSpeed; |
Joeatsumi | 0:91d5f51f73e2 | 150 | signed long heading; |
Joeatsumi | 0:91d5f51f73e2 | 151 | unsigned long sAcc; |
Joeatsumi | 0:91d5f51f73e2 | 152 | unsigned long cAcc; |
Joeatsumi | 0:91d5f51f73e2 | 153 | |
Joeatsumi | 0:91d5f51f73e2 | 154 | }; |
Joeatsumi | 0:91d5f51f73e2 | 155 | |
Joeatsumi | 0:91d5f51f73e2 | 156 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 157 | //受信したメッセージを格納する為の共用体 |
Joeatsumi | 0:91d5f51f73e2 | 158 | union UBXMessage { |
Joeatsumi | 0:91d5f51f73e2 | 159 | NAV_VELNED navVelned;//payload size is 36bytes |
Joeatsumi | 0:91d5f51f73e2 | 160 | NAV_POSLLH navPosllh;//payload size is 28bytes |
Joeatsumi | 0:91d5f51f73e2 | 161 | NAV_STATUS navStatus;//payload size is 16bytes |
Joeatsumi | 0:91d5f51f73e2 | 162 | }; |
Joeatsumi | 0:91d5f51f73e2 | 163 | |
Joeatsumi | 0:91d5f51f73e2 | 164 | UBXMessage ubxMessage; |
Joeatsumi | 0:91d5f51f73e2 | 165 | |
Joeatsumi | 0:91d5f51f73e2 | 166 | // The last two bytes of the message is a checksum value, used to confirm that the received payload is valid. |
Joeatsumi | 0:91d5f51f73e2 | 167 | // The procedure used to calculate this is given as pseudo-code in the uBlox manual. |
Joeatsumi | 0:91d5f51f73e2 | 168 | void calcChecksum(unsigned char* CK, int msgSize) { |
Joeatsumi | 0:91d5f51f73e2 | 169 | memset(CK, 0, 2); |
Joeatsumi | 0:91d5f51f73e2 | 170 | for (int i = 0; i < msgSize; i++) { |
Joeatsumi | 0:91d5f51f73e2 | 171 | CK[0] += ((unsigned char*)(&ubxMessage))[i]; |
Joeatsumi | 0:91d5f51f73e2 | 172 | CK[1] += CK[0]; |
Joeatsumi | 0:91d5f51f73e2 | 173 | } |
Joeatsumi | 0:91d5f51f73e2 | 174 | } |
Joeatsumi | 0:91d5f51f73e2 | 175 | |
Joeatsumi | 0:91d5f51f73e2 | 176 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 177 | // Compares the first two bytes of the ubxMessage struct with a specific message header. |
Joeatsumi | 0:91d5f51f73e2 | 178 | // Returns true if the two bytes match. |
Joeatsumi | 0:91d5f51f73e2 | 179 | bool compareMsgHeader(const unsigned char* msgHeader) { |
Joeatsumi | 0:91d5f51f73e2 | 180 | unsigned char* ptr = (unsigned char*)(&ubxMessage); |
Joeatsumi | 0:91d5f51f73e2 | 181 | return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1]; |
Joeatsumi | 0:91d5f51f73e2 | 182 | } |
Joeatsumi | 0:91d5f51f73e2 | 183 | |
Joeatsumi | 0:91d5f51f73e2 | 184 | //========================================================= |
Joeatsumi | 0:91d5f51f73e2 | 185 | // Reads in bytes from the GPS module and checks to see if a valid message has been constructed. |
Joeatsumi | 0:91d5f51f73e2 | 186 | // Returns the type of the message found if successful, or MT_NONE if no message was found. |
Joeatsumi | 0:91d5f51f73e2 | 187 | // After a successful return the contents of the ubxMessage union will be valid, for the |
Joeatsumi | 0:91d5f51f73e2 | 188 | // message type that was found. Note that further calls to this function can invalidate the |
Joeatsumi | 0:91d5f51f73e2 | 189 | // message content, so you must use the obtained values before calling this function again. |
Joeatsumi | 0:91d5f51f73e2 | 190 | void processGPS() { |
Joeatsumi | 0:91d5f51f73e2 | 191 | |
Joeatsumi | 0:91d5f51f73e2 | 192 | static int fpos = 0; |
Joeatsumi | 0:91d5f51f73e2 | 193 | static unsigned char checksum[2]; |
Joeatsumi | 0:91d5f51f73e2 | 194 | |
Joeatsumi | 0:91d5f51f73e2 | 195 | static unsigned char currentMsgType = MT_NONE; |
Joeatsumi | 0:91d5f51f73e2 | 196 | static int payloadSize = sizeof(UBXMessage); |
Joeatsumi | 0:91d5f51f73e2 | 197 | |
Joeatsumi | 0:91d5f51f73e2 | 198 | CS = 0; //SPIによる読み出しを開始 |
Joeatsumi | 0:91d5f51f73e2 | 199 | |
Joeatsumi | 0:91d5f51f73e2 | 200 | processed_time_before = processing_timer.read_us();// captureing prossing time |
Joeatsumi | 0:91d5f51f73e2 | 201 | |
Joeatsumi | 0:91d5f51f73e2 | 202 | /* |
Joeatsumi | 0:91d5f51f73e2 | 203 | NEO-7Mに(0xFF)を送って、取得した情報を1byteずつ以下の |
Joeatsumi | 0:91d5f51f73e2 | 204 | for文で確認する。 |
Joeatsumi | 0:91d5f51f73e2 | 205 | |
Joeatsumi | 0:91d5f51f73e2 | 206 | */ |
Joeatsumi | 0:91d5f51f73e2 | 207 | for(int buff_counter=1;buff_counter<50;buff_counter++){ |
Joeatsumi | 0:91d5f51f73e2 | 208 | |
Joeatsumi | 0:91d5f51f73e2 | 209 | unsigned char c = spi.write(0xFF); |
Joeatsumi | 0:91d5f51f73e2 | 210 | |
Joeatsumi | 0:91d5f51f73e2 | 211 | if ( fpos < 2 ) { |
Joeatsumi | 0:91d5f51f73e2 | 212 | // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62) |
Joeatsumi | 0:91d5f51f73e2 | 213 | if ( c == UBX_HEADER[fpos] ) |
Joeatsumi | 0:91d5f51f73e2 | 214 | fpos++; |
Joeatsumi | 0:91d5f51f73e2 | 215 | else |
Joeatsumi | 0:91d5f51f73e2 | 216 | fpos = 0; // Reset to beginning state. |
Joeatsumi | 0:91d5f51f73e2 | 217 | } |
Joeatsumi | 0:91d5f51f73e2 | 218 | else { |
Joeatsumi | 0:91d5f51f73e2 | 219 | // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER |
Joeatsumi | 0:91d5f51f73e2 | 220 | // and we are now reading in the bytes that make up the payload. |
Joeatsumi | 0:91d5f51f73e2 | 221 | |
Joeatsumi | 0:91d5f51f73e2 | 222 | // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because |
Joeatsumi | 0:91d5f51f73e2 | 223 | // the struct does not include the initial two-byte header (UBX_HEADER). |
Joeatsumi | 0:91d5f51f73e2 | 224 | if ( (fpos-2) < payloadSize ) |
Joeatsumi | 0:91d5f51f73e2 | 225 | ((unsigned char*)(&ubxMessage))[fpos-2] = c; |
Joeatsumi | 0:91d5f51f73e2 | 226 | |
Joeatsumi | 0:91d5f51f73e2 | 227 | |
Joeatsumi | 0:91d5f51f73e2 | 228 | fpos++; |
Joeatsumi | 0:91d5f51f73e2 | 229 | |
Joeatsumi | 0:91d5f51f73e2 | 230 | if ( fpos == 4 ) { |
Joeatsumi | 0:91d5f51f73e2 | 231 | // We have just received the second byte of the message type header, |
Joeatsumi | 0:91d5f51f73e2 | 232 | // so now we can check to see what kind of message it is. |
Joeatsumi | 0:91d5f51f73e2 | 233 | |
Joeatsumi | 0:91d5f51f73e2 | 234 | if ( compareMsgHeader(NAV_VELNED_HEADER) ) { |
Joeatsumi | 0:91d5f51f73e2 | 235 | currentMsgType = MT_NAV_VELNED; |
Joeatsumi | 0:91d5f51f73e2 | 236 | payloadSize = sizeof(NAV_VELNED); |
Joeatsumi | 0:91d5f51f73e2 | 237 | |
Joeatsumi | 0:91d5f51f73e2 | 238 | } |
Joeatsumi | 0:91d5f51f73e2 | 239 | else if ( compareMsgHeader(NAV_STATUS_HEADER) ) { |
Joeatsumi | 0:91d5f51f73e2 | 240 | currentMsgType = MT_NAV_STATUS; |
Joeatsumi | 0:91d5f51f73e2 | 241 | payloadSize = sizeof(NAV_STATUS); |
Joeatsumi | 0:91d5f51f73e2 | 242 | } |
Joeatsumi | 0:91d5f51f73e2 | 243 | |
Joeatsumi | 0:91d5f51f73e2 | 244 | else if ( compareMsgHeader(NAV_POSLLH_HEADER) ) { |
Joeatsumi | 0:91d5f51f73e2 | 245 | currentMsgType = MT_NAV_POSLLH; |
Joeatsumi | 0:91d5f51f73e2 | 246 | payloadSize = sizeof(NAV_POSLLH); |
Joeatsumi | 0:91d5f51f73e2 | 247 | |
Joeatsumi | 0:91d5f51f73e2 | 248 | } |
Joeatsumi | 0:91d5f51f73e2 | 249 | |
Joeatsumi | 0:91d5f51f73e2 | 250 | else { |
Joeatsumi | 0:91d5f51f73e2 | 251 | // unknown message type, bail |
Joeatsumi | 0:91d5f51f73e2 | 252 | fpos = 0; |
Joeatsumi | 0:91d5f51f73e2 | 253 | continue; |
Joeatsumi | 0:91d5f51f73e2 | 254 | } |
Joeatsumi | 0:91d5f51f73e2 | 255 | } |
Joeatsumi | 0:91d5f51f73e2 | 256 | |
Joeatsumi | 0:91d5f51f73e2 | 257 | if ( fpos == (payloadSize+2) ) { |
Joeatsumi | 0:91d5f51f73e2 | 258 | // All payload bytes have now been received, so we can calculate the |
Joeatsumi | 0:91d5f51f73e2 | 259 | // expected checksum value to compare with the next two incoming bytes. |
Joeatsumi | 0:91d5f51f73e2 | 260 | calcChecksum(checksum, payloadSize); |
Joeatsumi | 0:91d5f51f73e2 | 261 | } |
Joeatsumi | 0:91d5f51f73e2 | 262 | else if ( fpos == (payloadSize+3) ) { |
Joeatsumi | 0:91d5f51f73e2 | 263 | // First byte after the payload, ie. first byte of the checksum. |
Joeatsumi | 0:91d5f51f73e2 | 264 | // Does it match the first byte of the checksum we calculated? |
Joeatsumi | 0:91d5f51f73e2 | 265 | if ( c != checksum[0] ) { |
Joeatsumi | 0:91d5f51f73e2 | 266 | // Checksum doesn't match, reset to beginning state and try again. |
Joeatsumi | 0:91d5f51f73e2 | 267 | fpos = 0; |
Joeatsumi | 0:91d5f51f73e2 | 268 | } |
Joeatsumi | 0:91d5f51f73e2 | 269 | } |
Joeatsumi | 0:91d5f51f73e2 | 270 | else if ( fpos == (payloadSize+4) ) { |
Joeatsumi | 0:91d5f51f73e2 | 271 | // Second byte after the payload, ie. second byte of the checksum. |
Joeatsumi | 0:91d5f51f73e2 | 272 | // Does it match the second byte of the checksum we calculated? |
Joeatsumi | 0:91d5f51f73e2 | 273 | fpos = 0; // We will reset the state regardless of whether the checksum matches. |
Joeatsumi | 0:91d5f51f73e2 | 274 | if ( c == checksum[1] ) { |
Joeatsumi | 0:91d5f51f73e2 | 275 | // Checksum matches, we have a valid message. |
Joeatsumi | 0:91d5f51f73e2 | 276 | if(currentMsgType==MT_NAV_POSLLH){ |
Joeatsumi | 0:91d5f51f73e2 | 277 | latitude=ubxMessage.navPosllh.lat/10000000.0f; |
Joeatsumi | 0:91d5f51f73e2 | 278 | longitude=ubxMessage.navPosllh.lon/10000000.0f; |
Joeatsumi | 0:91d5f51f73e2 | 279 | height_float=float(ubxMessage.navPosllh.height); |
Joeatsumi | 0:91d5f51f73e2 | 280 | |
Joeatsumi | 1:8757d12d193b | 281 | //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float); |
Joeatsumi | 1:8757d12d193b | 282 | |
Joeatsumi | 1:8757d12d193b | 283 | |
Joeatsumi | 0:91d5f51f73e2 | 284 | flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる |
Joeatsumi | 1:8757d12d193b | 285 | //pc.printf("flag_posllh=%d\r\n",flag_posllh); |
Joeatsumi | 1:8757d12d193b | 286 | |
Joeatsumi | 0:91d5f51f73e2 | 287 | } |
Joeatsumi | 0:91d5f51f73e2 | 288 | else if(currentMsgType==MT_NAV_VELNED){ |
Joeatsumi | 0:91d5f51f73e2 | 289 | velN_float=float(ubxMessage.navVelned.velN); |
Joeatsumi | 0:91d5f51f73e2 | 290 | velE_float=float(ubxMessage.navVelned.velE); |
Joeatsumi | 0:91d5f51f73e2 | 291 | velD_float=float(ubxMessage.navVelned.velD); |
Joeatsumi | 0:91d5f51f73e2 | 292 | |
Joeatsumi | 1:8757d12d193b | 293 | //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float); |
Joeatsumi | 0:91d5f51f73e2 | 294 | flag_velned=1;//速度情報を読み取った合図としてフラグを立てる |
Joeatsumi | 1:8757d12d193b | 295 | //pc.printf("flag_velned=%d\r\n",flag_velned); |
Joeatsumi | 0:91d5f51f73e2 | 296 | |
Joeatsumi | 0:91d5f51f73e2 | 297 | } |
Joeatsumi | 0:91d5f51f73e2 | 298 | else if(currentMsgType==MT_NAV_STATUS){ |
Joeatsumi | 0:91d5f51f73e2 | 299 | |
Joeatsumi | 0:91d5f51f73e2 | 300 | } |
Joeatsumi | 1:8757d12d193b | 301 | else{} |
Joeatsumi | 0:91d5f51f73e2 | 302 | //return currentMsgType; |
Joeatsumi | 0:91d5f51f73e2 | 303 | } |
Joeatsumi | 0:91d5f51f73e2 | 304 | } |
Joeatsumi | 0:91d5f51f73e2 | 305 | else if ( fpos > (payloadSize+4) ) { |
Joeatsumi | 0:91d5f51f73e2 | 306 | // We have now read more bytes than both the expected payload and checksum |
Joeatsumi | 0:91d5f51f73e2 | 307 | // together, so something went wrong. Reset to beginning state and try again. |
Joeatsumi | 0:91d5f51f73e2 | 308 | fpos = 0; |
Joeatsumi | 0:91d5f51f73e2 | 309 | } |
Joeatsumi | 0:91d5f51f73e2 | 310 | } |
Joeatsumi | 0:91d5f51f73e2 | 311 | } |
Joeatsumi | 0:91d5f51f73e2 | 312 | |
Joeatsumi | 0:91d5f51f73e2 | 313 | CS = 1; //SPIによる読み出しを終了させる |
Joeatsumi | 0:91d5f51f73e2 | 314 | |
Joeatsumi | 0:91d5f51f73e2 | 315 | |
Joeatsumi | 0:91d5f51f73e2 | 316 | //processGPS()の処理に必要な時間の計測 |
Joeatsumi | 0:91d5f51f73e2 | 317 | //複数のメッセージを読み取る、つまりこの関数をメッセージの数だけwhile内で読み出すとき、 |
Joeatsumi | 0:91d5f51f73e2 | 318 | //この関数の処理時間(processed_time)として保存されるのは |
Joeatsumi | 0:91d5f51f73e2 | 319 | //最後に呼び出されたprocessGPSの処理時間となる。 |
Joeatsumi | 0:91d5f51f73e2 | 320 | processed_time_after = processing_timer.read_us();// captureing prossing time |
Joeatsumi | 0:91d5f51f73e2 | 321 | processed_time=processed_time_after-processed_time_before; |
Joeatsumi | 0:91d5f51f73e2 | 322 | |
Joeatsumi | 1:8757d12d193b | 323 | /*processGPSの処理時間の表示*/ |
Joeatsumi | 1:8757d12d193b | 324 | //pc.printf("processed_time_after(us)=%d;",(processed_time_after)); |
Joeatsumi | 1:8757d12d193b | 325 | //pc.printf("processed_time(us)=%d\r\n",(processed_time)); |
Joeatsumi | 1:8757d12d193b | 326 | //pc.printf("%d,%d\r\n",processed_time_after,processed_time); |
Joeatsumi | 1:8757d12d193b | 327 | |
Joeatsumi | 0:91d5f51f73e2 | 328 | } |
Joeatsumi | 0:91d5f51f73e2 | 329 | |
Joeatsumi | 0:91d5f51f73e2 | 330 | void imu_mesurement() |
Joeatsumi | 0:91d5f51f73e2 | 331 | { |
Joeatsumi | 1:8757d12d193b | 332 | |
Joeatsumi | 1:8757d12d193b | 333 | if(log_switch==1){ |
Joeatsumi | 1:8757d12d193b | 334 | logging_status=1; |
Joeatsumi | 1:8757d12d193b | 335 | }else if(log_switch==0){ |
Joeatsumi | 1:8757d12d193b | 336 | logging_status=0; |
Joeatsumi | 1:8757d12d193b | 337 | }else{} |
Joeatsumi | 2:9216162a9e17 | 338 | |
Joeatsumi | 2:9216162a9e17 | 339 | gyro_val[0]=imu.read_angular_rate_x();//X軸周りの角速度の算出 |
Joeatsumi | 2:9216162a9e17 | 340 | gyro_val[1]=imu.read_angular_rate_y();//Y軸周りの角速度の算出 |
Joeatsumi | 2:9216162a9e17 | 341 | gyro_val[2]=imu.read_angular_rate_z();//Z軸周りの角速度の算出 |
Joeatsumi | 2:9216162a9e17 | 342 | |
Joeatsumi | 2:9216162a9e17 | 343 | acc_val[0]=imu.read_acceleration_x();//X軸の加速度の算出 |
Joeatsumi | 2:9216162a9e17 | 344 | acc_val[1]=imu.read_acceleration_y();//Y軸の加速度の算出 |
Joeatsumi | 2:9216162a9e17 | 345 | acc_val[2]=imu.read_acceleration_z();//Z軸の加速度の算出 |
Joeatsumi | 2:9216162a9e17 | 346 | |
Joeatsumi | 2:9216162a9e17 | 347 | //計測時間の取得 |
Joeatsumi | 2:9216162a9e17 | 348 | measurement_time_g = processing_timer.read(); |
Joeatsumi | 2:9216162a9e17 | 349 | |
Joeatsumi | 2:9216162a9e17 | 350 | if(logging_status==1){ |
Joeatsumi | 2:9216162a9e17 | 351 | fprintf(im,"%f,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,gyro_val[0],gyro_val[1],gyro_val[2],acc_val[0],acc_val[1],acc_val[2]); |
Joeatsumi | 3:d564c0a27ba7 | 352 | //pc.printf("IL\r\n");//imu logging |
Joeatsumi | 2:9216162a9e17 | 353 | }else if(logging_status==0){} |
Joeatsumi | 1:8757d12d193b | 354 | |
Joeatsumi | 0:91d5f51f73e2 | 355 | } |
Joeatsumi | 0:91d5f51f73e2 | 356 | |
Joeatsumi | 3:d564c0a27ba7 | 357 | //==================BME_280の関数=============================== |
Joeatsumi | 3:d564c0a27ba7 | 358 | |
Joeatsumi | 3:d564c0a27ba7 | 359 | void BME_280_initialize() |
Joeatsumi | 3:d564c0a27ba7 | 360 | { |
Joeatsumi | 3:d564c0a27ba7 | 361 | char cmd[18]; |
Joeatsumi | 3:d564c0a27ba7 | 362 | |
Joeatsumi | 3:d564c0a27ba7 | 363 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 364 | //spi.format(8, 0); // 8-bit, mode=0 |
Joeatsumi | 3:d564c0a27ba7 | 365 | //spi.frequency(1000000); // 1MHZ |
Joeatsumi | 3:d564c0a27ba7 | 366 | |
Joeatsumi | 3:d564c0a27ba7 | 367 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 368 | spi.write(0xd0); // chip_id |
Joeatsumi | 3:d564c0a27ba7 | 369 | cmd[0] = spi.write(0); // read chip_id |
Joeatsumi | 3:d564c0a27ba7 | 370 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 371 | |
Joeatsumi | 3:d564c0a27ba7 | 372 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 373 | spi.write(0xf2 & BME280_SPI_MASK); // ctrl_hum |
Joeatsumi | 3:d564c0a27ba7 | 374 | spi.write(0x04); // Humidity oversampling x4 |
Joeatsumi | 3:d564c0a27ba7 | 375 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 376 | |
Joeatsumi | 3:d564c0a27ba7 | 377 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 378 | spi.write(0xf4 & BME280_SPI_MASK); // ctrl_meas |
Joeatsumi | 3:d564c0a27ba7 | 379 | spi.write((4<<5)|(4<<2)|3); // Temparature oversampling x4, Pressure oversampling x4, Normal mode |
Joeatsumi | 3:d564c0a27ba7 | 380 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 381 | |
Joeatsumi | 3:d564c0a27ba7 | 382 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 383 | spi.write(0xf5 & BME280_SPI_MASK); // config |
Joeatsumi | 3:d564c0a27ba7 | 384 | spi.write(0xa0); // Standby 1000ms, Filter off, 4-wire SPI interface |
Joeatsumi | 3:d564c0a27ba7 | 385 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 386 | |
Joeatsumi | 3:d564c0a27ba7 | 387 | wait(1); |
Joeatsumi | 3:d564c0a27ba7 | 388 | |
Joeatsumi | 3:d564c0a27ba7 | 389 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 390 | spi.write(0x88); // read dig_T regs |
Joeatsumi | 3:d564c0a27ba7 | 391 | for(int i = 0; i < 6; i++) |
Joeatsumi | 3:d564c0a27ba7 | 392 | cmd[i] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 393 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 394 | |
Joeatsumi | 3:d564c0a27ba7 | 395 | dig_T1 = (cmd[1] << 8) | cmd[0]; |
Joeatsumi | 3:d564c0a27ba7 | 396 | dig_T2 = (cmd[3] << 8) | cmd[2]; |
Joeatsumi | 3:d564c0a27ba7 | 397 | dig_T3 = (cmd[5] << 8) | cmd[4]; |
Joeatsumi | 3:d564c0a27ba7 | 398 | |
Joeatsumi | 3:d564c0a27ba7 | 399 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 400 | spi.write(0x8e); // read dig_P regs |
Joeatsumi | 3:d564c0a27ba7 | 401 | for(int i = 0; i < 18; i++) |
Joeatsumi | 3:d564c0a27ba7 | 402 | cmd[i] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 403 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 404 | |
Joeatsumi | 3:d564c0a27ba7 | 405 | dig_P1 = (cmd[ 1] << 8) | cmd[ 0]; |
Joeatsumi | 3:d564c0a27ba7 | 406 | dig_P2 = (cmd[ 3] << 8) | cmd[ 2]; |
Joeatsumi | 3:d564c0a27ba7 | 407 | dig_P3 = (cmd[ 5] << 8) | cmd[ 4]; |
Joeatsumi | 3:d564c0a27ba7 | 408 | dig_P4 = (cmd[ 7] << 8) | cmd[ 6]; |
Joeatsumi | 3:d564c0a27ba7 | 409 | dig_P5 = (cmd[ 9] << 8) | cmd[ 8]; |
Joeatsumi | 3:d564c0a27ba7 | 410 | dig_P6 = (cmd[11] << 8) | cmd[10]; |
Joeatsumi | 3:d564c0a27ba7 | 411 | dig_P7 = (cmd[13] << 8) | cmd[12]; |
Joeatsumi | 3:d564c0a27ba7 | 412 | dig_P8 = (cmd[15] << 8) | cmd[14]; |
Joeatsumi | 3:d564c0a27ba7 | 413 | dig_P9 = (cmd[17] << 8) | cmd[16]; |
Joeatsumi | 3:d564c0a27ba7 | 414 | |
Joeatsumi | 3:d564c0a27ba7 | 415 | |
Joeatsumi | 3:d564c0a27ba7 | 416 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 417 | spi.write(0xA1); // read dig_H1 reg |
Joeatsumi | 3:d564c0a27ba7 | 418 | cmd[0] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 419 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 420 | |
Joeatsumi | 3:d564c0a27ba7 | 421 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 422 | spi.write(0xE1); // read dig_H regs |
Joeatsumi | 3:d564c0a27ba7 | 423 | for(int i = 0; i < 7; i++) |
Joeatsumi | 3:d564c0a27ba7 | 424 | cmd[1+i] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 425 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 426 | |
Joeatsumi | 3:d564c0a27ba7 | 427 | dig_H1 = cmd[0]; |
Joeatsumi | 3:d564c0a27ba7 | 428 | dig_H2 = (cmd[2] << 8) | cmd[1]; |
Joeatsumi | 3:d564c0a27ba7 | 429 | dig_H3 = cmd[3]; |
Joeatsumi | 3:d564c0a27ba7 | 430 | dig_H4 = (cmd[4] << 4) | (cmd[5] & 0x0f); |
Joeatsumi | 3:d564c0a27ba7 | 431 | dig_H5 = (cmd[6] << 4) | ((cmd[5]>>4) & 0x0f); |
Joeatsumi | 3:d564c0a27ba7 | 432 | dig_H6 = cmd[7]; |
Joeatsumi | 3:d564c0a27ba7 | 433 | |
Joeatsumi | 3:d564c0a27ba7 | 434 | } |
Joeatsumi | 3:d564c0a27ba7 | 435 | |
Joeatsumi | 3:d564c0a27ba7 | 436 | float BME_280_getTemperature() |
Joeatsumi | 3:d564c0a27ba7 | 437 | { |
Joeatsumi | 3:d564c0a27ba7 | 438 | uint32_t temp_raw; |
Joeatsumi | 3:d564c0a27ba7 | 439 | float tempf; |
Joeatsumi | 3:d564c0a27ba7 | 440 | char cmd[3]; |
Joeatsumi | 3:d564c0a27ba7 | 441 | |
Joeatsumi | 3:d564c0a27ba7 | 442 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 443 | spi.write(0xfa); |
Joeatsumi | 3:d564c0a27ba7 | 444 | for(int i = 0; i < 3; i++) |
Joeatsumi | 3:d564c0a27ba7 | 445 | cmd[i] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 446 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 447 | |
Joeatsumi | 3:d564c0a27ba7 | 448 | temp_raw = (cmd[0] << 12) | (cmd[1] << 4) | (cmd[2] >> 4); |
Joeatsumi | 3:d564c0a27ba7 | 449 | |
Joeatsumi | 3:d564c0a27ba7 | 450 | int32_t temp; |
Joeatsumi | 3:d564c0a27ba7 | 451 | |
Joeatsumi | 3:d564c0a27ba7 | 452 | temp = |
Joeatsumi | 3:d564c0a27ba7 | 453 | (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) + |
Joeatsumi | 3:d564c0a27ba7 | 454 | ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14); |
Joeatsumi | 3:d564c0a27ba7 | 455 | |
Joeatsumi | 3:d564c0a27ba7 | 456 | t_fine = temp; |
Joeatsumi | 3:d564c0a27ba7 | 457 | temp = (temp * 5 + 128) >> 8; |
Joeatsumi | 3:d564c0a27ba7 | 458 | tempf = (float)temp; |
Joeatsumi | 3:d564c0a27ba7 | 459 | |
Joeatsumi | 3:d564c0a27ba7 | 460 | return (tempf/100.0f); |
Joeatsumi | 3:d564c0a27ba7 | 461 | } |
Joeatsumi | 3:d564c0a27ba7 | 462 | |
Joeatsumi | 3:d564c0a27ba7 | 463 | float BME_280_getPressure() |
Joeatsumi | 3:d564c0a27ba7 | 464 | { |
Joeatsumi | 3:d564c0a27ba7 | 465 | uint32_t press_raw; |
Joeatsumi | 3:d564c0a27ba7 | 466 | float pressf; |
Joeatsumi | 3:d564c0a27ba7 | 467 | char cmd[3]; |
Joeatsumi | 3:d564c0a27ba7 | 468 | |
Joeatsumi | 3:d564c0a27ba7 | 469 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 470 | spi.write(0xf7); // press_msb |
Joeatsumi | 3:d564c0a27ba7 | 471 | for(int i = 0; i < 3; i++) |
Joeatsumi | 3:d564c0a27ba7 | 472 | cmd[i] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 473 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 474 | |
Joeatsumi | 3:d564c0a27ba7 | 475 | press_raw = (cmd[0] << 12) | (cmd[1] << 4) | (cmd[2] >> 4); |
Joeatsumi | 3:d564c0a27ba7 | 476 | |
Joeatsumi | 3:d564c0a27ba7 | 477 | int32_t var1, var2; |
Joeatsumi | 3:d564c0a27ba7 | 478 | uint32_t press; |
Joeatsumi | 3:d564c0a27ba7 | 479 | |
Joeatsumi | 3:d564c0a27ba7 | 480 | var1 = (t_fine >> 1) - 64000; |
Joeatsumi | 3:d564c0a27ba7 | 481 | var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6; |
Joeatsumi | 3:d564c0a27ba7 | 482 | var2 = var2 + ((var1 * dig_P5) << 1); |
Joeatsumi | 3:d564c0a27ba7 | 483 | var2 = (var2 >> 2) + (dig_P4 << 16); |
Joeatsumi | 3:d564c0a27ba7 | 484 | var1 = (((dig_P3 * (((var1 >> 2)*(var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18; |
Joeatsumi | 3:d564c0a27ba7 | 485 | var1 = ((32768 + var1) * dig_P1) >> 15; |
Joeatsumi | 3:d564c0a27ba7 | 486 | if (var1 == 0) { |
Joeatsumi | 3:d564c0a27ba7 | 487 | return 0; |
Joeatsumi | 3:d564c0a27ba7 | 488 | } |
Joeatsumi | 3:d564c0a27ba7 | 489 | press = (((1048576 - press_raw) - (var2 >> 12))) * 3125; |
Joeatsumi | 3:d564c0a27ba7 | 490 | if(press < 0x80000000) { |
Joeatsumi | 3:d564c0a27ba7 | 491 | press = (press << 1) / var1; |
Joeatsumi | 3:d564c0a27ba7 | 492 | } else { |
Joeatsumi | 3:d564c0a27ba7 | 493 | press = (press / var1) * 2; |
Joeatsumi | 3:d564c0a27ba7 | 494 | } |
Joeatsumi | 3:d564c0a27ba7 | 495 | var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12; |
Joeatsumi | 3:d564c0a27ba7 | 496 | var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13; |
Joeatsumi | 3:d564c0a27ba7 | 497 | press = (press + ((var1 + var2 + dig_P7) >> 4)); |
Joeatsumi | 3:d564c0a27ba7 | 498 | |
Joeatsumi | 3:d564c0a27ba7 | 499 | pressf = (float)press; |
Joeatsumi | 3:d564c0a27ba7 | 500 | return (pressf/100.0f); |
Joeatsumi | 3:d564c0a27ba7 | 501 | } |
Joeatsumi | 3:d564c0a27ba7 | 502 | |
Joeatsumi | 3:d564c0a27ba7 | 503 | float BME_280_getHumidity() |
Joeatsumi | 3:d564c0a27ba7 | 504 | { |
Joeatsumi | 3:d564c0a27ba7 | 505 | uint32_t hum_raw; |
Joeatsumi | 3:d564c0a27ba7 | 506 | float humf; |
Joeatsumi | 3:d564c0a27ba7 | 507 | char cmd[2]; |
Joeatsumi | 3:d564c0a27ba7 | 508 | |
Joeatsumi | 3:d564c0a27ba7 | 509 | BME_280_CS = 0; |
Joeatsumi | 3:d564c0a27ba7 | 510 | spi.write(0xfd); // hum_msb |
Joeatsumi | 3:d564c0a27ba7 | 511 | for(int i = 0; i < 2; i++) |
Joeatsumi | 3:d564c0a27ba7 | 512 | cmd[i] = spi.write(0); |
Joeatsumi | 3:d564c0a27ba7 | 513 | BME_280_CS = 1; |
Joeatsumi | 3:d564c0a27ba7 | 514 | |
Joeatsumi | 3:d564c0a27ba7 | 515 | hum_raw = (cmd[0] << 8) | cmd[1]; |
Joeatsumi | 3:d564c0a27ba7 | 516 | |
Joeatsumi | 3:d564c0a27ba7 | 517 | int32_t v_x1; |
Joeatsumi | 3:d564c0a27ba7 | 518 | |
Joeatsumi | 3:d564c0a27ba7 | 519 | v_x1 = t_fine - 76800; |
Joeatsumi | 3:d564c0a27ba7 | 520 | v_x1 = (((((hum_raw << 14) -(((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * v_x1)) + |
Joeatsumi | 3:d564c0a27ba7 | 521 | ((int32_t)16384)) >> 15) * (((((((v_x1 * (int32_t)dig_H6) >> 10) * |
Joeatsumi | 3:d564c0a27ba7 | 522 | (((v_x1 * ((int32_t)dig_H3)) >> 11) + 32768)) >> 10) + 2097152) * |
Joeatsumi | 3:d564c0a27ba7 | 523 | (int32_t)dig_H2 + 8192) >> 14)); |
Joeatsumi | 3:d564c0a27ba7 | 524 | v_x1 = (v_x1 - (((((v_x1 >> 15) * (v_x1 >> 15)) >> 7) * (int32_t)dig_H1) >> 4)); |
Joeatsumi | 3:d564c0a27ba7 | 525 | v_x1 = (v_x1 < 0 ? 0 : v_x1); |
Joeatsumi | 3:d564c0a27ba7 | 526 | v_x1 = (v_x1 > 419430400 ? 419430400 : v_x1); |
Joeatsumi | 3:d564c0a27ba7 | 527 | |
Joeatsumi | 3:d564c0a27ba7 | 528 | humf = (float)(v_x1 >> 12); |
Joeatsumi | 3:d564c0a27ba7 | 529 | |
Joeatsumi | 3:d564c0a27ba7 | 530 | return (humf/1024.0f); |
Joeatsumi | 3:d564c0a27ba7 | 531 | } |
Joeatsumi | 3:d564c0a27ba7 | 532 | //========================================================== |
Joeatsumi | 3:d564c0a27ba7 | 533 | |
Joeatsumi | 0:91d5f51f73e2 | 534 | void ublox_logging() |
Joeatsumi | 0:91d5f51f73e2 | 535 | { |
Joeatsumi | 0:91d5f51f73e2 | 536 | //detach the rotary imu mesurement |
Joeatsumi | 0:91d5f51f73e2 | 537 | timer1.detach(); |
Joeatsumi | 1:8757d12d193b | 538 | |
Joeatsumi | 1:8757d12d193b | 539 | processGPS(); |
Joeatsumi | 1:8757d12d193b | 540 | processGPS(); |
Joeatsumi | 1:8757d12d193b | 541 | processGPS(); |
Joeatsumi | 1:8757d12d193b | 542 | |
Joeatsumi | 3:d564c0a27ba7 | 543 | |
Joeatsumi | 3:d564c0a27ba7 | 544 | bem280_tempreture = BME_280_getTemperature(); |
Joeatsumi | 3:d564c0a27ba7 | 545 | bem280_humidity = BME_280_getHumidity(); |
Joeatsumi | 3:d564c0a27ba7 | 546 | bem280_pressure = BME_280_getPressure(); |
Joeatsumi | 3:d564c0a27ba7 | 547 | |
Joeatsumi | 3:d564c0a27ba7 | 548 | thermopile_voltage_1 = 3.3*(thermopile_input_1.read()); |
Joeatsumi | 3:d564c0a27ba7 | 549 | thermopile_voltage_2 = 3.3*(thermopile_input_2.read()); |
Joeatsumi | 3:d564c0a27ba7 | 550 | |
Joeatsumi | 2:9216162a9e17 | 551 | //計測時間の取得 |
Joeatsumi | 2:9216162a9e17 | 552 | measurement_time_g = processing_timer.read(); |
Joeatsumi | 2:9216162a9e17 | 553 | |
Joeatsumi | 3:d564c0a27ba7 | 554 | //サーモパイルセンサと気圧、湿度、温度の記録 |
Joeatsumi | 3:d564c0a27ba7 | 555 | fprintf(th,"%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,bem280_tempreture,bem280_humidity,bem280_pressure,thermopile_voltage_1,thermopile_voltage_2); |
Joeatsumi | 3:d564c0a27ba7 | 556 | |
Joeatsumi | 3:d564c0a27ba7 | 557 | /* |
Joeatsumi | 1:8757d12d193b | 558 | if(logging_status==1){ |
Joeatsumi | 2:9216162a9e17 | 559 | fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float); |
Joeatsumi | 1:8757d12d193b | 560 | }else if(logging_status==0){} |
Joeatsumi | 3:d564c0a27ba7 | 561 | */ |
Joeatsumi | 3:d564c0a27ba7 | 562 | |
Joeatsumi | 0:91d5f51f73e2 | 563 | //位置と速度情報を読み取った場合 |
Joeatsumi | 0:91d5f51f73e2 | 564 | if((flag_posllh==1)&&(flag_velned==1)){ |
Joeatsumi | 3:d564c0a27ba7 | 565 | fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float); |
Joeatsumi | 0:91d5f51f73e2 | 566 | /*フラグを0に戻す*/ |
Joeatsumi | 0:91d5f51f73e2 | 567 | flag_posllh=0; |
Joeatsumi | 0:91d5f51f73e2 | 568 | flag_velned=0; |
Joeatsumi | 0:91d5f51f73e2 | 569 | |
Joeatsumi | 0:91d5f51f73e2 | 570 | }else{} |
Joeatsumi | 1:8757d12d193b | 571 | |
Joeatsumi | 0:91d5f51f73e2 | 572 | //attach a timer for imu mesurement (400 Hz) |
Joeatsumi | 0:91d5f51f73e2 | 573 | timer1.attach(&imu_mesurement, imu_interval); |
Joeatsumi | 0:91d5f51f73e2 | 574 | } |
Joeatsumi | 0:91d5f51f73e2 | 575 | |
Joeatsumi | 0:91d5f51f73e2 | 576 | |
Joeatsumi | 3:d564c0a27ba7 | 577 | |
Joeatsumi | 0:91d5f51f73e2 | 578 | /*--------------------------------------------*/ |
Joeatsumi | 0:91d5f51f73e2 | 579 | int main() { |
Joeatsumi | 0:91d5f51f73e2 | 580 | |
Joeatsumi | 3:d564c0a27ba7 | 581 | CS=1; |
Joeatsumi | 3:d564c0a27ba7 | 582 | BME_280_CS=1; |
Joeatsumi | 3:d564c0a27ba7 | 583 | |
Joeatsumi | 2:9216162a9e17 | 584 | //power on wait 800ms form IMU |
Joeatsumi | 2:9216162a9e17 | 585 | wait(1.0); |
Joeatsumi | 2:9216162a9e17 | 586 | |
Joeatsumi | 2:9216162a9e17 | 587 | //IMU initialize |
Joeatsumi | 2:9216162a9e17 | 588 | imu.power_on_sequence1();//IMUが動作可能かどうかの確認 |
Joeatsumi | 2:9216162a9e17 | 589 | imu.power_on_sequence2();//IMUが動作可能かどうかの確認 |
Joeatsumi | 2:9216162a9e17 | 590 | imu.UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行 |
Joeatsumi | 2:9216162a9e17 | 591 | imu.move_to_sampling_mode();//サンプリングモードへの移行 |
Joeatsumi | 2:9216162a9e17 | 592 | |
Joeatsumi | 3:d564c0a27ba7 | 593 | //ログスイッチの電圧出力の設定 |
Joeatsumi | 3:d564c0a27ba7 | 594 | log_low = 0; |
Joeatsumi | 3:d564c0a27ba7 | 595 | log_high = 1; |
Joeatsumi | 3:d564c0a27ba7 | 596 | |
Joeatsumi | 3:d564c0a27ba7 | 597 | //ロガーの動作状態を見るためのLED |
Joeatsumi | 3:d564c0a27ba7 | 598 | //初期状態ではロガーが記録中の表示をする |
Joeatsumi | 3:d564c0a27ba7 | 599 | myled_1 = 1; |
Joeatsumi | 3:d564c0a27ba7 | 600 | myled_2 = 0; |
Joeatsumi | 3:d564c0a27ba7 | 601 | |
Joeatsumi | 0:91d5f51f73e2 | 602 | //UART initialization |
Joeatsumi | 2:9216162a9e17 | 603 | pc.baud(460800); //460.8 kbps |
Joeatsumi | 0:91d5f51f73e2 | 604 | |
Joeatsumi | 3:d564c0a27ba7 | 605 | |
Joeatsumi | 3:d564c0a27ba7 | 606 | spi.format(8, 0); // data size: 8bit, mode0 |
Joeatsumi | 3:d564c0a27ba7 | 607 | spi.frequency(1000000); // 5.5MHz |
Joeatsumi | 3:d564c0a27ba7 | 608 | |
Joeatsumi | 3:d564c0a27ba7 | 609 | BME_280_initialize(); |
Joeatsumi | 1:8757d12d193b | 610 | |
Joeatsumi | 1:8757d12d193b | 611 | mkdir("/sd/mydir",0777);//SDファイル作成 |
Joeatsumi | 2:9216162a9e17 | 612 | fp = fopen("/sd/mydir/gps.csv", "a");//最初のSDopen時間かかるのでwhile外で行う |
Joeatsumi | 2:9216162a9e17 | 613 | im = fopen("/sd/mydir/imu.csv", "a"); |
Joeatsumi | 3:d564c0a27ba7 | 614 | th = fopen("/sd/mydir/thermopile.csv", "a"); |
Joeatsumi | 1:8757d12d193b | 615 | |
Joeatsumi | 1:8757d12d193b | 616 | if(fp == NULL) { |
Joeatsumi | 1:8757d12d193b | 617 | error("Could not open file for write\n"); |
Joeatsumi | 1:8757d12d193b | 618 | }else{} |
Joeatsumi | 1:8757d12d193b | 619 | |
Joeatsumi | 1:8757d12d193b | 620 | pc.printf("FO\r\n");//file open |
Joeatsumi | 1:8757d12d193b | 621 | logging_status=1; |
Joeatsumi | 1:8757d12d193b | 622 | |
Joeatsumi | 1:8757d12d193b | 623 | wait(0.1); |
Joeatsumi | 1:8757d12d193b | 624 | |
Joeatsumi | 0:91d5f51f73e2 | 625 | //フラグのリセット |
Joeatsumi | 0:91d5f51f73e2 | 626 | flag_posllh=0; |
Joeatsumi | 0:91d5f51f73e2 | 627 | flag_velned=0; |
Joeatsumi | 0:91d5f51f73e2 | 628 | |
Joeatsumi | 0:91d5f51f73e2 | 629 | //------------------------------------------- |
Joeatsumi | 0:91d5f51f73e2 | 630 | //Timer |
Joeatsumi | 0:91d5f51f73e2 | 631 | //------------------------------------------- |
Joeatsumi | 0:91d5f51f73e2 | 632 | //timer1: imu mesurement, 400 Hz |
Joeatsumi | 0:91d5f51f73e2 | 633 | timer1.attach(&imu_mesurement, imu_interval); |
Joeatsumi | 0:91d5f51f73e2 | 634 | //timer2: GNSS mesurement, 5 Hz |
Joeatsumi | 0:91d5f51f73e2 | 635 | timer2.attach(&ublox_logging, gnss_interval); |
Joeatsumi | 0:91d5f51f73e2 | 636 | |
Joeatsumi | 0:91d5f51f73e2 | 637 | processing_timer.start();//timer starts |
Joeatsumi | 0:91d5f51f73e2 | 638 | |
Joeatsumi | 0:91d5f51f73e2 | 639 | while(1) { |
Joeatsumi | 3:d564c0a27ba7 | 640 | //ログスイッチの電圧出力の設定 |
Joeatsumi | 3:d564c0a27ba7 | 641 | //log_low = 0; |
Joeatsumi | 3:d564c0a27ba7 | 642 | //log_high = 1; |
Joeatsumi | 3:d564c0a27ba7 | 643 | |
Joeatsumi | 3:d564c0a27ba7 | 644 | //pc.printf("T2D\r\n"); |
Joeatsumi | 3:d564c0a27ba7 | 645 | myled_1 = 0; |
Joeatsumi | 3:d564c0a27ba7 | 646 | myled_2 = 0; |
Joeatsumi | 3:d564c0a27ba7 | 647 | |
Joeatsumi | 1:8757d12d193b | 648 | timer2.detach(); |
Joeatsumi | 0:91d5f51f73e2 | 649 | |
Joeatsumi | 1:8757d12d193b | 650 | if(logging_status==0){ |
Joeatsumi | 1:8757d12d193b | 651 | fclose(fp); |
Joeatsumi | 2:9216162a9e17 | 652 | fclose(im); |
Joeatsumi | 3:d564c0a27ba7 | 653 | fclose(th); |
Joeatsumi | 3:d564c0a27ba7 | 654 | |
Joeatsumi | 1:8757d12d193b | 655 | pc.printf("FC\r\n"); |
Joeatsumi | 1:8757d12d193b | 656 | timer2.detach(); |
Joeatsumi | 1:8757d12d193b | 657 | timer1.detach(); |
Joeatsumi | 3:d564c0a27ba7 | 658 | |
Joeatsumi | 3:d564c0a27ba7 | 659 | break; |
Joeatsumi | 3:d564c0a27ba7 | 660 | |
Joeatsumi | 1:8757d12d193b | 661 | }else if(logging_status==1){} |
Joeatsumi | 3:d564c0a27ba7 | 662 | //ロガーの動作状態を見るためのLED |
Joeatsumi | 3:d564c0a27ba7 | 663 | //初期状態ではロガーが記録中の表示をする |
Joeatsumi | 3:d564c0a27ba7 | 664 | myled_1 = 1; |
Joeatsumi | 3:d564c0a27ba7 | 665 | myled_2 = 0; |
Joeatsumi | 1:8757d12d193b | 666 | |
Joeatsumi | 1:8757d12d193b | 667 | timer2.attach(&ublox_logging, gnss_interval); |
Joeatsumi | 1:8757d12d193b | 668 | wait(0.8); |
Joeatsumi | 0:91d5f51f73e2 | 669 | // |
Joeatsumi | 0:91d5f51f73e2 | 670 | }//while |
Joeatsumi | 3:d564c0a27ba7 | 671 | |
Joeatsumi | 3:d564c0a27ba7 | 672 | //ロガーの動作状態を見るためのLED |
Joeatsumi | 3:d564c0a27ba7 | 673 | //ロガーが記録を終了した表示をする |
Joeatsumi | 3:d564c0a27ba7 | 674 | myled_1 = 0; |
Joeatsumi | 3:d564c0a27ba7 | 675 | myled_2 = 1; |
Joeatsumi | 0:91d5f51f73e2 | 676 | |
Joeatsumi | 0:91d5f51f73e2 | 677 | } |