Atsumi Toda / Mbed 2 deprecated INS_GNSS_logger_1

imu/gnss logger ver1.1

Dependencies:   mbed MPU9250_SPI MG354PDH0 SDFileSystem

main.cpp@5:38613e30475e, 2021-11-16 (annotated)

Committer:
Joeatsumi
Date:
Tue Nov 16 04:03:44 2021 +0000
Revision:
5:38613e30475e
Parent:
4:d49256697f27 
Magnetometer measurement was added.

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