Atsumi Toda / Mbed 2 deprecated INS_GNSS_logger_1

imu/gnss logger ver1.1

Dependencies:   mbed MPU9250_SPI MG354PDH0 SDFileSystem

main.cpp@4:d49256697f27, 2021-08-29 (annotated)

Committer:
Joeatsumi
Date:
Sun Aug 29 05:30:41 2021 +0000
Revision:
4:d49256697f27
Parent:
3:d564c0a27ba7
Child:
5:38613e30475e
Thermopile logging is 200Hz.

Who changed what in which revision?

UserRevisionLine numberNew 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 4:d49256697f27 5 //2021/08/29 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 4:d49256697f27 347 thermopile_voltage_1 = 3.3*(thermopile_input_1.read());//サーモパイルセンサ_1出力の計測
Joeatsumi 4:d49256697f27 348 thermopile_voltage_2 = 3.3*(thermopile_input_2.read());//サーモパイルセンサ_2出力の計測
Joeatsumi 4:d49256697f27 349
Joeatsumi 2:9216162a9e17 350 //計測時間の取得
Joeatsumi 2:9216162a9e17 351 measurement_time_g = processing_timer.read();
Joeatsumi 2:9216162a9e17 352
Joeatsumi 2:9216162a9e17 353 if(logging_status==1){
Joeatsumi 4:d49256697f27 354 fprintf(im,"%f,%f,%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],thermopile_voltage_1,thermopile_voltage_2);
Joeatsumi 3:d564c0a27ba7 355 //pc.printf("IL\r\n");//imu logging
Joeatsumi 2:9216162a9e17 356 }else if(logging_status==0){}
Joeatsumi 1:8757d12d193b 357
Joeatsumi 0:91d5f51f73e2 358 }
Joeatsumi 0:91d5f51f73e2 359
Joeatsumi 3:d564c0a27ba7 360 //==================BME_280の関数===============================
Joeatsumi 3:d564c0a27ba7 361
Joeatsumi 3:d564c0a27ba7 362 void BME_280_initialize()
Joeatsumi 3:d564c0a27ba7 363 {
Joeatsumi 3:d564c0a27ba7 364 char cmd[18];
Joeatsumi 3:d564c0a27ba7 365
Joeatsumi 3:d564c0a27ba7 366 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 367 //spi.format(8, 0); // 8-bit, mode=0
Joeatsumi 3:d564c0a27ba7 368 //spi.frequency(1000000); // 1MHZ
Joeatsumi 3:d564c0a27ba7 369
Joeatsumi 3:d564c0a27ba7 370 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 371 spi.write(0xd0); // chip_id
Joeatsumi 3:d564c0a27ba7 372 cmd[0] = spi.write(0); // read chip_id
Joeatsumi 3:d564c0a27ba7 373 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 374
Joeatsumi 3:d564c0a27ba7 375 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 376 spi.write(0xf2 & BME280_SPI_MASK); // ctrl_hum
Joeatsumi 3:d564c0a27ba7 377 spi.write(0x04); // Humidity oversampling x4
Joeatsumi 3:d564c0a27ba7 378 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 379
Joeatsumi 3:d564c0a27ba7 380 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 381 spi.write(0xf4 & BME280_SPI_MASK); // ctrl_meas
Joeatsumi 3:d564c0a27ba7 382 spi.write((4<<5)|(4<<2)|3); // Temparature oversampling x4, Pressure oversampling x4, Normal mode
Joeatsumi 3:d564c0a27ba7 383 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 384
Joeatsumi 3:d564c0a27ba7 385 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 386 spi.write(0xf5 & BME280_SPI_MASK); // config
Joeatsumi 3:d564c0a27ba7 387 spi.write(0xa0); // Standby 1000ms, Filter off, 4-wire SPI interface
Joeatsumi 3:d564c0a27ba7 388 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 389
Joeatsumi 3:d564c0a27ba7 390 wait(1);
Joeatsumi 3:d564c0a27ba7 391
Joeatsumi 3:d564c0a27ba7 392 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 393 spi.write(0x88); // read dig_T regs
Joeatsumi 3:d564c0a27ba7 394 for(int i = 0; i < 6; i++)
Joeatsumi 3:d564c0a27ba7 395 cmd[i] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 396 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 397
Joeatsumi 3:d564c0a27ba7 398 dig_T1 = (cmd[1] << 8) | cmd[0];
Joeatsumi 3:d564c0a27ba7 399 dig_T2 = (cmd[3] << 8) | cmd[2];
Joeatsumi 3:d564c0a27ba7 400 dig_T3 = (cmd[5] << 8) | cmd[4];
Joeatsumi 3:d564c0a27ba7 401
Joeatsumi 3:d564c0a27ba7 402 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 403 spi.write(0x8e); // read dig_P regs
Joeatsumi 3:d564c0a27ba7 404 for(int i = 0; i < 18; i++)
Joeatsumi 3:d564c0a27ba7 405 cmd[i] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 406 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 407
Joeatsumi 3:d564c0a27ba7 408 dig_P1 = (cmd[ 1] << 8) | cmd[ 0];
Joeatsumi 3:d564c0a27ba7 409 dig_P2 = (cmd[ 3] << 8) | cmd[ 2];
Joeatsumi 3:d564c0a27ba7 410 dig_P3 = (cmd[ 5] << 8) | cmd[ 4];
Joeatsumi 3:d564c0a27ba7 411 dig_P4 = (cmd[ 7] << 8) | cmd[ 6];
Joeatsumi 3:d564c0a27ba7 412 dig_P5 = (cmd[ 9] << 8) | cmd[ 8];
Joeatsumi 3:d564c0a27ba7 413 dig_P6 = (cmd[11] << 8) | cmd[10];
Joeatsumi 3:d564c0a27ba7 414 dig_P7 = (cmd[13] << 8) | cmd[12];
Joeatsumi 3:d564c0a27ba7 415 dig_P8 = (cmd[15] << 8) | cmd[14];
Joeatsumi 3:d564c0a27ba7 416 dig_P9 = (cmd[17] << 8) | cmd[16];
Joeatsumi 3:d564c0a27ba7 417
Joeatsumi 3:d564c0a27ba7 418
Joeatsumi 3:d564c0a27ba7 419 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 420 spi.write(0xA1); // read dig_H1 reg
Joeatsumi 3:d564c0a27ba7 421 cmd[0] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 422 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 423
Joeatsumi 3:d564c0a27ba7 424 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 425 spi.write(0xE1); // read dig_H regs
Joeatsumi 3:d564c0a27ba7 426 for(int i = 0; i < 7; i++)
Joeatsumi 3:d564c0a27ba7 427 cmd[1+i] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 428 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 429
Joeatsumi 3:d564c0a27ba7 430 dig_H1 = cmd[0];
Joeatsumi 3:d564c0a27ba7 431 dig_H2 = (cmd[2] << 8) | cmd[1];
Joeatsumi 3:d564c0a27ba7 432 dig_H3 = cmd[3];
Joeatsumi 3:d564c0a27ba7 433 dig_H4 = (cmd[4] << 4) | (cmd[5] & 0x0f);
Joeatsumi 3:d564c0a27ba7 434 dig_H5 = (cmd[6] << 4) | ((cmd[5]>>4) & 0x0f);
Joeatsumi 3:d564c0a27ba7 435 dig_H6 = cmd[7];
Joeatsumi 3:d564c0a27ba7 436
Joeatsumi 3:d564c0a27ba7 437 }
Joeatsumi 3:d564c0a27ba7 438
Joeatsumi 3:d564c0a27ba7 439 float BME_280_getTemperature()
Joeatsumi 3:d564c0a27ba7 440 {
Joeatsumi 3:d564c0a27ba7 441 uint32_t temp_raw;
Joeatsumi 3:d564c0a27ba7 442 float tempf;
Joeatsumi 3:d564c0a27ba7 443 char cmd[3];
Joeatsumi 3:d564c0a27ba7 444
Joeatsumi 3:d564c0a27ba7 445 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 446 spi.write(0xfa);
Joeatsumi 3:d564c0a27ba7 447 for(int i = 0; i < 3; i++)
Joeatsumi 3:d564c0a27ba7 448 cmd[i] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 449 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 450
Joeatsumi 3:d564c0a27ba7 451 temp_raw = (cmd[0] << 12) | (cmd[1] << 4) | (cmd[2] >> 4);
Joeatsumi 3:d564c0a27ba7 452
Joeatsumi 3:d564c0a27ba7 453 int32_t temp;
Joeatsumi 3:d564c0a27ba7 454
Joeatsumi 3:d564c0a27ba7 455 temp =
Joeatsumi 3:d564c0a27ba7 456 (((((temp_raw >> 3) - (dig_T1 << 1))) * dig_T2) >> 11) +
Joeatsumi 3:d564c0a27ba7 457 ((((((temp_raw >> 4) - dig_T1) * ((temp_raw >> 4) - dig_T1)) >> 12) * dig_T3) >> 14);
Joeatsumi 3:d564c0a27ba7 458
Joeatsumi 3:d564c0a27ba7 459 t_fine = temp;
Joeatsumi 3:d564c0a27ba7 460 temp = (temp * 5 + 128) >> 8;
Joeatsumi 3:d564c0a27ba7 461 tempf = (float)temp;
Joeatsumi 3:d564c0a27ba7 462
Joeatsumi 3:d564c0a27ba7 463 return (tempf/100.0f);
Joeatsumi 3:d564c0a27ba7 464 }
Joeatsumi 3:d564c0a27ba7 465
Joeatsumi 3:d564c0a27ba7 466 float BME_280_getPressure()
Joeatsumi 3:d564c0a27ba7 467 {
Joeatsumi 3:d564c0a27ba7 468 uint32_t press_raw;
Joeatsumi 3:d564c0a27ba7 469 float pressf;
Joeatsumi 3:d564c0a27ba7 470 char cmd[3];
Joeatsumi 3:d564c0a27ba7 471
Joeatsumi 3:d564c0a27ba7 472 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 473 spi.write(0xf7); // press_msb
Joeatsumi 3:d564c0a27ba7 474 for(int i = 0; i < 3; i++)
Joeatsumi 3:d564c0a27ba7 475 cmd[i] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 476 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 477
Joeatsumi 3:d564c0a27ba7 478 press_raw = (cmd[0] << 12) | (cmd[1] << 4) | (cmd[2] >> 4);
Joeatsumi 3:d564c0a27ba7 479
Joeatsumi 3:d564c0a27ba7 480 int32_t var1, var2;
Joeatsumi 3:d564c0a27ba7 481 uint32_t press;
Joeatsumi 3:d564c0a27ba7 482
Joeatsumi 3:d564c0a27ba7 483 var1 = (t_fine >> 1) - 64000;
Joeatsumi 3:d564c0a27ba7 484 var2 = (((var1 >> 2) * (var1 >> 2)) >> 11) * dig_P6;
Joeatsumi 3:d564c0a27ba7 485 var2 = var2 + ((var1 * dig_P5) << 1);
Joeatsumi 3:d564c0a27ba7 486 var2 = (var2 >> 2) + (dig_P4 << 16);
Joeatsumi 3:d564c0a27ba7 487 var1 = (((dig_P3 * (((var1 >> 2)*(var1 >> 2)) >> 13)) >> 3) + ((dig_P2 * var1) >> 1)) >> 18;
Joeatsumi 3:d564c0a27ba7 488 var1 = ((32768 + var1) * dig_P1) >> 15;
Joeatsumi 3:d564c0a27ba7 489 if (var1 == 0) {
Joeatsumi 3:d564c0a27ba7 490 return 0;
Joeatsumi 3:d564c0a27ba7 491 }
Joeatsumi 3:d564c0a27ba7 492 press = (((1048576 - press_raw) - (var2 >> 12))) * 3125;
Joeatsumi 3:d564c0a27ba7 493 if(press < 0x80000000) {
Joeatsumi 3:d564c0a27ba7 494 press = (press << 1) / var1;
Joeatsumi 3:d564c0a27ba7 495 } else {
Joeatsumi 3:d564c0a27ba7 496 press = (press / var1) * 2;
Joeatsumi 3:d564c0a27ba7 497 }
Joeatsumi 3:d564c0a27ba7 498 var1 = ((int32_t)dig_P9 * ((int32_t)(((press >> 3) * (press >> 3)) >> 13))) >> 12;
Joeatsumi 3:d564c0a27ba7 499 var2 = (((int32_t)(press >> 2)) * (int32_t)dig_P8) >> 13;
Joeatsumi 3:d564c0a27ba7 500 press = (press + ((var1 + var2 + dig_P7) >> 4));
Joeatsumi 3:d564c0a27ba7 501
Joeatsumi 3:d564c0a27ba7 502 pressf = (float)press;
Joeatsumi 3:d564c0a27ba7 503 return (pressf/100.0f);
Joeatsumi 3:d564c0a27ba7 504 }
Joeatsumi 3:d564c0a27ba7 505
Joeatsumi 3:d564c0a27ba7 506 float BME_280_getHumidity()
Joeatsumi 3:d564c0a27ba7 507 {
Joeatsumi 3:d564c0a27ba7 508 uint32_t hum_raw;
Joeatsumi 3:d564c0a27ba7 509 float humf;
Joeatsumi 3:d564c0a27ba7 510 char cmd[2];
Joeatsumi 3:d564c0a27ba7 511
Joeatsumi 3:d564c0a27ba7 512 BME_280_CS = 0;
Joeatsumi 3:d564c0a27ba7 513 spi.write(0xfd); // hum_msb
Joeatsumi 3:d564c0a27ba7 514 for(int i = 0; i < 2; i++)
Joeatsumi 3:d564c0a27ba7 515 cmd[i] = spi.write(0);
Joeatsumi 3:d564c0a27ba7 516 BME_280_CS = 1;
Joeatsumi 3:d564c0a27ba7 517
Joeatsumi 3:d564c0a27ba7 518 hum_raw = (cmd[0] << 8) | cmd[1];
Joeatsumi 3:d564c0a27ba7 519
Joeatsumi 3:d564c0a27ba7 520 int32_t v_x1;
Joeatsumi 3:d564c0a27ba7 521
Joeatsumi 3:d564c0a27ba7 522 v_x1 = t_fine - 76800;
Joeatsumi 3:d564c0a27ba7 523 v_x1 = (((((hum_raw << 14) -(((int32_t)dig_H4) << 20) - (((int32_t)dig_H5) * v_x1)) +
Joeatsumi 3:d564c0a27ba7 524 ((int32_t)16384)) >> 15) * (((((((v_x1 * (int32_t)dig_H6) >> 10) *
Joeatsumi 3:d564c0a27ba7 525 (((v_x1 * ((int32_t)dig_H3)) >> 11) + 32768)) >> 10) + 2097152) *
Joeatsumi 3:d564c0a27ba7 526 (int32_t)dig_H2 + 8192) >> 14));
Joeatsumi 3:d564c0a27ba7 527 v_x1 = (v_x1 - (((((v_x1 >> 15) * (v_x1 >> 15)) >> 7) * (int32_t)dig_H1) >> 4));
Joeatsumi 3:d564c0a27ba7 528 v_x1 = (v_x1 < 0 ? 0 : v_x1);
Joeatsumi 3:d564c0a27ba7 529 v_x1 = (v_x1 > 419430400 ? 419430400 : v_x1);
Joeatsumi 3:d564c0a27ba7 530
Joeatsumi 3:d564c0a27ba7 531 humf = (float)(v_x1 >> 12);
Joeatsumi 3:d564c0a27ba7 532
Joeatsumi 3:d564c0a27ba7 533 return (humf/1024.0f);
Joeatsumi 3:d564c0a27ba7 534 }
Joeatsumi 3:d564c0a27ba7 535 //==========================================================
Joeatsumi 3:d564c0a27ba7 536
Joeatsumi 0:91d5f51f73e2 537 void ublox_logging()
Joeatsumi 0:91d5f51f73e2 538 {
Joeatsumi 0:91d5f51f73e2 539 //detach the rotary imu mesurement
Joeatsumi 0:91d5f51f73e2 540 timer1.detach();
Joeatsumi 1:8757d12d193b 541
Joeatsumi 1:8757d12d193b 542 processGPS();
Joeatsumi 1:8757d12d193b 543 processGPS();
Joeatsumi 1:8757d12d193b 544 processGPS();
Joeatsumi 1:8757d12d193b 545
Joeatsumi 3:d564c0a27ba7 546
Joeatsumi 3:d564c0a27ba7 547 bem280_tempreture = BME_280_getTemperature();
Joeatsumi 3:d564c0a27ba7 548 bem280_humidity = BME_280_getHumidity();
Joeatsumi 3:d564c0a27ba7 549 bem280_pressure = BME_280_getPressure();
Joeatsumi 3:d564c0a27ba7 550
Joeatsumi 2:9216162a9e17 551 //計測時間の取得
Joeatsumi 2:9216162a9e17 552 measurement_time_g = processing_timer.read();
Joeatsumi 2:9216162a9e17 553
Joeatsumi 4:d49256697f27 554 //気圧、湿度、温度の記録
Joeatsumi 4:d49256697f27 555 fprintf(th,"%f,%f,%f,%f\r\n",measurement_time_g,bem280_tempreture,bem280_humidity,bem280_pressure);
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 }