Atsumi Toda / Mbed 2 deprecated UBX_GPS_muliti_SPI_4_logger_ver

5Hz GNSS logger GNSS logging program with ublox neo7M. This utilizes ticker and timer.

Dependencies:   mbed MG354PDH0 SDFileSystem

main.cpp@3:32843000531e, 2022-01-14 (annotated)

Committer:
Joeatsumi
Date:
Fri Jan 14 13:31:58 2022 +0000
Revision:
3:32843000531e
Parent:
2:940851e53dde
Child:
4:6f167145f4b0
modified vertion of IMU/GNSS logger

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:91d5f51f73e2 1 //=========================================================
Joeatsumi 3:32843000531e 2 //IMU/GNSS logger with ublox-NEO7M
Joeatsumi 0:91d5f51f73e2 3 //MPU board: mbed LPC1768
Joeatsumi 0:91d5f51f73e2 4 //GNSS module: ublox-NEO7M
Joeatsumi 3:32843000531e 5 //2022/01/14 A.Toda
Joeatsumi 0:91d5f51f73e2 6 //========================================================
Joeatsumi 0:91d5f51f73e2 7 #include "mbed.h"
Joeatsumi 3:32843000531e 8 #include "MG354PDH0.h"
Joeatsumi 1:8757d12d193b 9 #include "SDFileSystem.h"
Joeatsumi 0:91d5f51f73e2 10 //=========================================================
Joeatsumi 0:91d5f51f73e2 11 //Port Setting
Joeatsumi 3:32843000531e 12 Serial pc(USBTX, USBRX); // tx, rx // PCとの通信用シリアルポート
Joeatsumi 3:32843000531e 13 Serial epson_imu(p9, p10); // IMU通信用シリアルポート
Joeatsumi 3:32843000531e 14 SPI spi(p11, p12, p13); // mosi, miso, sclk SPIを使用するセンサのバス
Joeatsumi 2:940851e53dde 15 DigitalOut CS(p14); // NEO-7MのCSピン
Joeatsumi 3:32843000531e 16
Joeatsumi 3:32843000531e 17 DigitalIn log_switch(p15); //記録終了用のスイッチ
Joeatsumi 3:32843000531e 18
Joeatsumi 3:32843000531e 19 AnalogIn thermopile_input_1(p20); //アナログ電圧の読み取り
Joeatsumi 3:32843000531e 20 AnalogIn thermopile_input_2(p19); //アナログ電圧の読み取り
Joeatsumi 1:8757d12d193b 21
Joeatsumi 3:32843000531e 22 DigitalOut myled_1(LED1);
Joeatsumi 3:32843000531e 23 DigitalOut myled_2(LED2);
Joeatsumi 3:32843000531e 24 DigitalOut myled_3(LED3);
Joeatsumi 3:32843000531e 25 DigitalOut myled_4(LED4);
Joeatsumi 3:32843000531e 26
Joeatsumi 3:32843000531e 27 //ライブラリとポートの対応づけ
Joeatsumi 1:8757d12d193b 28 SDFileSystem sd(p5, p6, p7, p8, "sd"); // mosi, miso, sclk, cs
Joeatsumi 3:32843000531e 29 MG354PDH0 imu(&epson_imu); // IMU
Joeatsumi 1:8757d12d193b 30
Joeatsumi 1:8757d12d193b 31 //ファイルポインタ
Joeatsumi 1:8757d12d193b 32 FILE *fp;
Joeatsumi 1:8757d12d193b 33 FILE *im;
Joeatsumi 0:91d5f51f73e2 34
Joeatsumi 0:91d5f51f73e2 35 //=========================================================
Joeatsumi 3:32843000531e 36 //IMUの変数
Joeatsumi 3:32843000531e 37 float IMU_BUFFER = 50;//慣性センサをsdに記録する前にためておくバッファの大きさ
Joeatsumi 3:32843000531e 38 float time_i[7];//慣性センサの計測時刻
Joeatsumi 3:32843000531e 39 float gyro_val[7][3];//角速度の値
Joeatsumi 3:32843000531e 40 float acc_val[7][3];//加速度の値
Joeatsumi 3:32843000531e 41
Joeatsumi 3:32843000531e 42 int imu_counter;//何回分の慣性センサ計測値がバッファにたまっている貯まっているかのカウント
Joeatsumi 3:32843000531e 43 //=========================================================
Joeatsumi 0:91d5f51f73e2 44 //受信したメッセージから抽出したい情報
Joeatsumi 0:91d5f51f73e2 45 float latitude,longitude,height_float; //緯度、経度、高度
Joeatsumi 0:91d5f51f73e2 46 int gps_Fix; // GPSの測位状態この値が3ならば3D Fix状態である
Joeatsumi 0:91d5f51f73e2 47 float velN_float,velE_float,velD_float; // NED座標系に置ける速度
Joeatsumi 0:91d5f51f73e2 48
Joeatsumi 0:91d5f51f73e2 49 //=========================================================
Joeatsumi 0:91d5f51f73e2 50 //UBXデータを処理したかどうかのフラグ
Joeatsumi 0:91d5f51f73e2 51 int flag_posllh,flag_velned;
Joeatsumi 0:91d5f51f73e2 52
Joeatsumi 0:91d5f51f73e2 53 //=========================================================
Joeatsumi 0:91d5f51f73e2 54 //処理時間計測の為のタイマー
Joeatsumi 0:91d5f51f73e2 55 Timer processing_timer;
Joeatsumi 0:91d5f51f73e2 56
Joeatsumi 0:91d5f51f73e2 57 //=========================================================
Joeatsumi 0:91d5f51f73e2 58 //処理時間
Joeatsumi 3:32843000531e 59 int processed_time,processed_time_before,processed_time_after,measurement_time_g,measurement_time_i;
Joeatsumi 0:91d5f51f73e2 60
Joeatsumi 0:91d5f51f73e2 61 //=========================================================
Joeatsumi 0:91d5f51f73e2 62 //Ticker
Joeatsumi 0:91d5f51f73e2 63 Ticker timer1; //
Joeatsumi 0:91d5f51f73e2 64 Ticker timer2; //
Joeatsumi 0:91d5f51f73e2 65
Joeatsumi 0:91d5f51f73e2 66 //=========================================================
Joeatsumi 0:91d5f51f73e2 67 //Logging variables
Joeatsumi 3:32843000531e 68
Joeatsumi 3:32843000531e 69 //Logging counter 100Hzの割り込みで何回目であるかを0から99でカウント
Joeatsumi 3:32843000531e 70 int logging_counter;
Joeatsumi 3:32843000531e 71
Joeatsumi 3:32843000531e 72 float imu_mesurement_freq = 100.0; //Hz
Joeatsumi 1:8757d12d193b 73 float gnss_mesurement_freq = 5.0; //theta_update_freq;
Joeatsumi 0:91d5f51f73e2 74
Joeatsumi 0:91d5f51f73e2 75 float imu_interval = 1.0f/imu_mesurement_freq; //sec
Joeatsumi 0:91d5f51f73e2 76 float gnss_interval = 1.0f/gnss_mesurement_freq; //sec
Joeatsumi 0:91d5f51f73e2 77
Joeatsumi 1:8757d12d193b 78 int logging_status;
Joeatsumi 0:91d5f51f73e2 79 //=========================================================
Joeatsumi 0:91d5f51f73e2 80 //Header char
Joeatsumi 0:91d5f51f73e2 81 const unsigned char UBX_HEADER[] = { 0xB5, 0x62 };
Joeatsumi 0:91d5f51f73e2 82 const unsigned char NAV_POSLLH_HEADER[] = { 0x01, 0x02 };
Joeatsumi 0:91d5f51f73e2 83 const unsigned char NAV_STATUS_HEADER[] = { 0x01, 0x03 };
Joeatsumi 0:91d5f51f73e2 84
Joeatsumi 0:91d5f51f73e2 85 const unsigned char NAV_VELNED_HEADER[] = { 0x01, 0x12 };
Joeatsumi 0:91d5f51f73e2 86
Joeatsumi 0:91d5f51f73e2 87 enum _ubxMsgType {
Joeatsumi 0:91d5f51f73e2 88 MT_NONE,
Joeatsumi 0:91d5f51f73e2 89 MT_NAV_POSLLH,
Joeatsumi 0:91d5f51f73e2 90 MT_NAV_STATUS,
Joeatsumi 0:91d5f51f73e2 91 MT_NAV_VELNED
Joeatsumi 0:91d5f51f73e2 92 };
Joeatsumi 0:91d5f51f73e2 93
Joeatsumi 0:91d5f51f73e2 94 //=========================================================
Joeatsumi 0:91d5f51f73e2 95 //メッセージの構造体
Joeatsumi 0:91d5f51f73e2 96 struct NAV_POSLLH {
Joeatsumi 0:91d5f51f73e2 97 unsigned char cls;
Joeatsumi 0:91d5f51f73e2 98 unsigned char id;
Joeatsumi 0:91d5f51f73e2 99 unsigned short len;
Joeatsumi 0:91d5f51f73e2 100 unsigned long iTOW;
Joeatsumi 0:91d5f51f73e2 101 long lon;
Joeatsumi 0:91d5f51f73e2 102 long lat;
Joeatsumi 0:91d5f51f73e2 103 long height;
Joeatsumi 0:91d5f51f73e2 104 long hMSL;
Joeatsumi 0:91d5f51f73e2 105 unsigned long hAcc;
Joeatsumi 0:91d5f51f73e2 106 unsigned long vAcc;
Joeatsumi 0:91d5f51f73e2 107 };
Joeatsumi 0:91d5f51f73e2 108
Joeatsumi 0:91d5f51f73e2 109 struct NAV_STATUS {
Joeatsumi 0:91d5f51f73e2 110 unsigned char cls;
Joeatsumi 0:91d5f51f73e2 111 unsigned char id;
Joeatsumi 0:91d5f51f73e2 112 unsigned short len;
Joeatsumi 0:91d5f51f73e2 113 unsigned long iTOW;
Joeatsumi 0:91d5f51f73e2 114 unsigned char gpsFix;
Joeatsumi 0:91d5f51f73e2 115 char flags;
Joeatsumi 0:91d5f51f73e2 116 char fixStat;
Joeatsumi 0:91d5f51f73e2 117 char flags2;
Joeatsumi 0:91d5f51f73e2 118 unsigned long ttff;
Joeatsumi 0:91d5f51f73e2 119 unsigned long msss;
Joeatsumi 0:91d5f51f73e2 120 };
Joeatsumi 0:91d5f51f73e2 121
Joeatsumi 0:91d5f51f73e2 122 struct NAV_VELNED {
Joeatsumi 0:91d5f51f73e2 123 unsigned char cls;
Joeatsumi 0:91d5f51f73e2 124 unsigned char id;
Joeatsumi 0:91d5f51f73e2 125 unsigned short len;
Joeatsumi 0:91d5f51f73e2 126 unsigned long iTOW;
Joeatsumi 0:91d5f51f73e2 127 signed long velN;
Joeatsumi 0:91d5f51f73e2 128 signed long velE;
Joeatsumi 0:91d5f51f73e2 129 signed long velD;
Joeatsumi 0:91d5f51f73e2 130 unsigned long speed;
Joeatsumi 0:91d5f51f73e2 131 unsigned long gSpeed;
Joeatsumi 0:91d5f51f73e2 132 signed long heading;
Joeatsumi 0:91d5f51f73e2 133 unsigned long sAcc;
Joeatsumi 0:91d5f51f73e2 134 unsigned long cAcc;
Joeatsumi 0:91d5f51f73e2 135
Joeatsumi 0:91d5f51f73e2 136 };
Joeatsumi 0:91d5f51f73e2 137
Joeatsumi 0:91d5f51f73e2 138 //=========================================================
Joeatsumi 0:91d5f51f73e2 139 //受信したメッセージを格納する為の共用体
Joeatsumi 0:91d5f51f73e2 140 union UBXMessage {
Joeatsumi 0:91d5f51f73e2 141 NAV_VELNED navVelned;//payload size is 36bytes
Joeatsumi 0:91d5f51f73e2 142 NAV_POSLLH navPosllh;//payload size is 28bytes
Joeatsumi 0:91d5f51f73e2 143 NAV_STATUS navStatus;//payload size is 16bytes
Joeatsumi 0:91d5f51f73e2 144 };
Joeatsumi 0:91d5f51f73e2 145
Joeatsumi 0:91d5f51f73e2 146 UBXMessage ubxMessage;
Joeatsumi 0:91d5f51f73e2 147
Joeatsumi 0:91d5f51f73e2 148 // The last two bytes of the message is a checksum value, used to confirm that the received payload is valid.
Joeatsumi 0:91d5f51f73e2 149 // The procedure used to calculate this is given as pseudo-code in the uBlox manual.
Joeatsumi 0:91d5f51f73e2 150 void calcChecksum(unsigned char* CK, int msgSize) {
Joeatsumi 0:91d5f51f73e2 151 memset(CK, 0, 2);
Joeatsumi 0:91d5f51f73e2 152 for (int i = 0; i < msgSize; i++) {
Joeatsumi 0:91d5f51f73e2 153 CK[0] += ((unsigned char*)(&ubxMessage))[i];
Joeatsumi 0:91d5f51f73e2 154 CK[1] += CK[0];
Joeatsumi 0:91d5f51f73e2 155 }
Joeatsumi 0:91d5f51f73e2 156 }
Joeatsumi 0:91d5f51f73e2 157
Joeatsumi 0:91d5f51f73e2 158 //=========================================================
Joeatsumi 0:91d5f51f73e2 159 // Compares the first two bytes of the ubxMessage struct with a specific message header.
Joeatsumi 0:91d5f51f73e2 160 // Returns true if the two bytes match.
Joeatsumi 0:91d5f51f73e2 161 bool compareMsgHeader(const unsigned char* msgHeader) {
Joeatsumi 0:91d5f51f73e2 162 unsigned char* ptr = (unsigned char*)(&ubxMessage);
Joeatsumi 0:91d5f51f73e2 163 return ptr[0] == msgHeader[0] && ptr[1] == msgHeader[1];
Joeatsumi 0:91d5f51f73e2 164 }
Joeatsumi 0:91d5f51f73e2 165
Joeatsumi 0:91d5f51f73e2 166 //=========================================================
Joeatsumi 0:91d5f51f73e2 167 // Reads in bytes from the GPS module and checks to see if a valid message has been constructed.
Joeatsumi 0:91d5f51f73e2 168 // Returns the type of the message found if successful, or MT_NONE if no message was found.
Joeatsumi 0:91d5f51f73e2 169 // After a successful return the contents of the ubxMessage union will be valid, for the
Joeatsumi 0:91d5f51f73e2 170 // message type that was found. Note that further calls to this function can invalidate the
Joeatsumi 0:91d5f51f73e2 171 // message content, so you must use the obtained values before calling this function again.
Joeatsumi 0:91d5f51f73e2 172 void processGPS() {
Joeatsumi 0:91d5f51f73e2 173
Joeatsumi 0:91d5f51f73e2 174 static int fpos = 0;
Joeatsumi 0:91d5f51f73e2 175 static unsigned char checksum[2];
Joeatsumi 0:91d5f51f73e2 176
Joeatsumi 0:91d5f51f73e2 177 static unsigned char currentMsgType = MT_NONE;
Joeatsumi 0:91d5f51f73e2 178 static int payloadSize = sizeof(UBXMessage);
Joeatsumi 0:91d5f51f73e2 179
Joeatsumi 0:91d5f51f73e2 180 CS = 0; //SPIによる読み出しを開始
Joeatsumi 0:91d5f51f73e2 181
Joeatsumi 0:91d5f51f73e2 182 processed_time_before = processing_timer.read_us();// captureing prossing time
Joeatsumi 0:91d5f51f73e2 183
Joeatsumi 0:91d5f51f73e2 184 /*
Joeatsumi 0:91d5f51f73e2 185 NEO-7Mに(0xFF)を送って、取得した情報を1byteずつ以下の
Joeatsumi 0:91d5f51f73e2 186 for文で確認する。
Joeatsumi 0:91d5f51f73e2 187
Joeatsumi 0:91d5f51f73e2 188 */
Joeatsumi 0:91d5f51f73e2 189 for(int buff_counter=1;buff_counter<50;buff_counter++){
Joeatsumi 0:91d5f51f73e2 190
Joeatsumi 0:91d5f51f73e2 191 unsigned char c = spi.write(0xFF);
Joeatsumi 0:91d5f51f73e2 192
Joeatsumi 0:91d5f51f73e2 193 if ( fpos < 2 ) {
Joeatsumi 0:91d5f51f73e2 194 // For the first two bytes we are simply looking for a match with the UBX header bytes (0xB5,0x62)
Joeatsumi 0:91d5f51f73e2 195 if ( c == UBX_HEADER[fpos] )
Joeatsumi 0:91d5f51f73e2 196 fpos++;
Joeatsumi 0:91d5f51f73e2 197 else
Joeatsumi 0:91d5f51f73e2 198 fpos = 0; // Reset to beginning state.
Joeatsumi 0:91d5f51f73e2 199 }
Joeatsumi 0:91d5f51f73e2 200 else {
Joeatsumi 0:91d5f51f73e2 201 // If we come here then fpos >= 2, which means we have found a match with the UBX_HEADER
Joeatsumi 0:91d5f51f73e2 202 // and we are now reading in the bytes that make up the payload.
Joeatsumi 0:91d5f51f73e2 203
Joeatsumi 0:91d5f51f73e2 204 // Place the incoming byte into the ubxMessage struct. The position is fpos-2 because
Joeatsumi 0:91d5f51f73e2 205 // the struct does not include the initial two-byte header (UBX_HEADER).
Joeatsumi 0:91d5f51f73e2 206 if ( (fpos-2) < payloadSize )
Joeatsumi 0:91d5f51f73e2 207 ((unsigned char*)(&ubxMessage))[fpos-2] = c;
Joeatsumi 0:91d5f51f73e2 208
Joeatsumi 0:91d5f51f73e2 209
Joeatsumi 0:91d5f51f73e2 210 fpos++;
Joeatsumi 0:91d5f51f73e2 211
Joeatsumi 0:91d5f51f73e2 212 if ( fpos == 4 ) {
Joeatsumi 0:91d5f51f73e2 213 // We have just received the second byte of the message type header,
Joeatsumi 0:91d5f51f73e2 214 // so now we can check to see what kind of message it is.
Joeatsumi 0:91d5f51f73e2 215
Joeatsumi 0:91d5f51f73e2 216 if ( compareMsgHeader(NAV_VELNED_HEADER) ) {
Joeatsumi 0:91d5f51f73e2 217 currentMsgType = MT_NAV_VELNED;
Joeatsumi 0:91d5f51f73e2 218 payloadSize = sizeof(NAV_VELNED);
Joeatsumi 0:91d5f51f73e2 219
Joeatsumi 0:91d5f51f73e2 220 }
Joeatsumi 0:91d5f51f73e2 221 else if ( compareMsgHeader(NAV_STATUS_HEADER) ) {
Joeatsumi 0:91d5f51f73e2 222 currentMsgType = MT_NAV_STATUS;
Joeatsumi 0:91d5f51f73e2 223 payloadSize = sizeof(NAV_STATUS);
Joeatsumi 0:91d5f51f73e2 224 }
Joeatsumi 0:91d5f51f73e2 225
Joeatsumi 0:91d5f51f73e2 226 else if ( compareMsgHeader(NAV_POSLLH_HEADER) ) {
Joeatsumi 0:91d5f51f73e2 227 currentMsgType = MT_NAV_POSLLH;
Joeatsumi 0:91d5f51f73e2 228 payloadSize = sizeof(NAV_POSLLH);
Joeatsumi 0:91d5f51f73e2 229
Joeatsumi 0:91d5f51f73e2 230 }
Joeatsumi 0:91d5f51f73e2 231
Joeatsumi 0:91d5f51f73e2 232 else {
Joeatsumi 0:91d5f51f73e2 233 // unknown message type, bail
Joeatsumi 0:91d5f51f73e2 234 fpos = 0;
Joeatsumi 0:91d5f51f73e2 235 continue;
Joeatsumi 0:91d5f51f73e2 236 }
Joeatsumi 0:91d5f51f73e2 237 }
Joeatsumi 0:91d5f51f73e2 238
Joeatsumi 0:91d5f51f73e2 239 if ( fpos == (payloadSize+2) ) {
Joeatsumi 0:91d5f51f73e2 240 // All payload bytes have now been received, so we can calculate the
Joeatsumi 0:91d5f51f73e2 241 // expected checksum value to compare with the next two incoming bytes.
Joeatsumi 0:91d5f51f73e2 242 calcChecksum(checksum, payloadSize);
Joeatsumi 0:91d5f51f73e2 243 }
Joeatsumi 0:91d5f51f73e2 244 else if ( fpos == (payloadSize+3) ) {
Joeatsumi 0:91d5f51f73e2 245 // First byte after the payload, ie. first byte of the checksum.
Joeatsumi 0:91d5f51f73e2 246 // Does it match the first byte of the checksum we calculated?
Joeatsumi 0:91d5f51f73e2 247 if ( c != checksum[0] ) {
Joeatsumi 0:91d5f51f73e2 248 // Checksum doesn't match, reset to beginning state and try again.
Joeatsumi 0:91d5f51f73e2 249 fpos = 0;
Joeatsumi 0:91d5f51f73e2 250 }
Joeatsumi 0:91d5f51f73e2 251 }
Joeatsumi 0:91d5f51f73e2 252 else if ( fpos == (payloadSize+4) ) {
Joeatsumi 0:91d5f51f73e2 253 // Second byte after the payload, ie. second byte of the checksum.
Joeatsumi 0:91d5f51f73e2 254 // Does it match the second byte of the checksum we calculated?
Joeatsumi 0:91d5f51f73e2 255 fpos = 0; // We will reset the state regardless of whether the checksum matches.
Joeatsumi 0:91d5f51f73e2 256 if ( c == checksum[1] ) {
Joeatsumi 0:91d5f51f73e2 257 // Checksum matches, we have a valid message.
Joeatsumi 0:91d5f51f73e2 258 if(currentMsgType==MT_NAV_POSLLH){
Joeatsumi 0:91d5f51f73e2 259 latitude=ubxMessage.navPosllh.lat/10000000.0f;
Joeatsumi 0:91d5f51f73e2 260 longitude=ubxMessage.navPosllh.lon/10000000.0f;
Joeatsumi 0:91d5f51f73e2 261 height_float=float(ubxMessage.navPosllh.height);
Joeatsumi 0:91d5f51f73e2 262
Joeatsumi 1:8757d12d193b 263 //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float);
Joeatsumi 1:8757d12d193b 264
Joeatsumi 1:8757d12d193b 265
Joeatsumi 0:91d5f51f73e2 266 flag_posllh=1;//位置情報を読み取った合図としてフラグを立てる
Joeatsumi 1:8757d12d193b 267 //pc.printf("flag_posllh=%d\r\n",flag_posllh);
Joeatsumi 1:8757d12d193b 268
Joeatsumi 0:91d5f51f73e2 269 }
Joeatsumi 0:91d5f51f73e2 270 else if(currentMsgType==MT_NAV_VELNED){
Joeatsumi 0:91d5f51f73e2 271 velN_float=float(ubxMessage.navVelned.velN);
Joeatsumi 0:91d5f51f73e2 272 velE_float=float(ubxMessage.navVelned.velE);
Joeatsumi 0:91d5f51f73e2 273 velD_float=float(ubxMessage.navVelned.velD);
Joeatsumi 0:91d5f51f73e2 274
Joeatsumi 1:8757d12d193b 275 //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float);
Joeatsumi 0:91d5f51f73e2 276 flag_velned=1;//速度情報を読み取った合図としてフラグを立てる
Joeatsumi 1:8757d12d193b 277 //pc.printf("flag_velned=%d\r\n",flag_velned);
Joeatsumi 0:91d5f51f73e2 278
Joeatsumi 0:91d5f51f73e2 279 }
Joeatsumi 0:91d5f51f73e2 280 else if(currentMsgType==MT_NAV_STATUS){
Joeatsumi 0:91d5f51f73e2 281
Joeatsumi 0:91d5f51f73e2 282 }
Joeatsumi 1:8757d12d193b 283 else{}
Joeatsumi 0:91d5f51f73e2 284 //return currentMsgType;
Joeatsumi 0:91d5f51f73e2 285 }
Joeatsumi 0:91d5f51f73e2 286 }
Joeatsumi 0:91d5f51f73e2 287 else if ( fpos > (payloadSize+4) ) {
Joeatsumi 0:91d5f51f73e2 288 // We have now read more bytes than both the expected payload and checksum
Joeatsumi 0:91d5f51f73e2 289 // together, so something went wrong. Reset to beginning state and try again.
Joeatsumi 0:91d5f51f73e2 290 fpos = 0;
Joeatsumi 0:91d5f51f73e2 291 }
Joeatsumi 0:91d5f51f73e2 292 }
Joeatsumi 0:91d5f51f73e2 293 }
Joeatsumi 0:91d5f51f73e2 294
Joeatsumi 0:91d5f51f73e2 295 CS = 1; //SPIによる読み出しを終了させる
Joeatsumi 0:91d5f51f73e2 296
Joeatsumi 0:91d5f51f73e2 297
Joeatsumi 0:91d5f51f73e2 298 //processGPS()の処理に必要な時間の計測
Joeatsumi 0:91d5f51f73e2 299 //複数のメッセージを読み取る、つまりこの関数をメッセージの数だけwhile内で読み出すとき、
Joeatsumi 0:91d5f51f73e2 300 //この関数の処理時間(processed_time)として保存されるのは
Joeatsumi 0:91d5f51f73e2 301 //最後に呼び出されたprocessGPSの処理時間となる。
Joeatsumi 0:91d5f51f73e2 302 processed_time_after = processing_timer.read_us();// captureing prossing time
Joeatsumi 0:91d5f51f73e2 303 processed_time=processed_time_after-processed_time_before;
Joeatsumi 0:91d5f51f73e2 304
Joeatsumi 1:8757d12d193b 305 /*processGPSの処理時間の表示*/
Joeatsumi 1:8757d12d193b 306 //pc.printf("processed_time_after(us)=%d;",(processed_time_after));
Joeatsumi 3:32843000531e 307 //pc.printf("processed_time(us)=%d\r\n",(processed_time));
Joeatsumi 1:8757d12d193b 308 //pc.printf("%d,%d\r\n",processed_time_after,processed_time);
Joeatsumi 1:8757d12d193b 309
Joeatsumi 0:91d5f51f73e2 310 }
Joeatsumi 0:91d5f51f73e2 311
Joeatsumi 2:940851e53dde 312 void imu_mesurement(){
Joeatsumi 2:940851e53dde 313
Joeatsumi 1:8757d12d193b 314 if(log_switch==1){
Joeatsumi 1:8757d12d193b 315 logging_status=1;
Joeatsumi 1:8757d12d193b 316 }else if(log_switch==0){
Joeatsumi 1:8757d12d193b 317 logging_status=0;
Joeatsumi 1:8757d12d193b 318 }else{}
Joeatsumi 1:8757d12d193b 319
Joeatsumi 0:91d5f51f73e2 320 }
Joeatsumi 0:91d5f51f73e2 321
Joeatsumi 0:91d5f51f73e2 322 void ublox_logging()
Joeatsumi 0:91d5f51f73e2 323 {
Joeatsumi 1:8757d12d193b 324
Joeatsumi 3:32843000531e 325 //============慣性センサのロギング============
Joeatsumi 3:32843000531e 326 time_i[imu_counter] = processing_timer.read_us();//慣性センサの計測時刻
Joeatsumi 3:32843000531e 327 gyro_val[imu_counter][0]=imu.read_angular_rate_x();//X軸周りの角速度の算出
Joeatsumi 3:32843000531e 328 gyro_val[imu_counter][1]=imu.read_angular_rate_y();//Y軸周りの角速度の算出
Joeatsumi 3:32843000531e 329 gyro_val[imu_counter][2]=imu.read_angular_rate_z();//Z軸周りの角速度の算出
Joeatsumi 3:32843000531e 330
Joeatsumi 3:32843000531e 331 acc_val[imu_counter][0]=imu.read_acceleration_x();//X軸の加速度の算出
Joeatsumi 3:32843000531e 332 acc_val[imu_counter][1]=imu.read_acceleration_y();//Y軸の加速度の算出
Joeatsumi 3:32843000531e 333 acc_val[imu_counter][2]=imu.read_acceleration_z();//Z軸の加速度の算出
Joeatsumi 3:32843000531e 334 //=========================================
Joeatsumi 3:32843000531e 335
Joeatsumi 3:32843000531e 336 if(((imu_counter+1)%5)==0){
Joeatsumi 3:32843000531e 337 for (int i = 0; i < imu_counter; i++){
Joeatsumi 3:32843000531e 338 fprintf(im,"%f,%f,%f,%f,%f,%f,%f\r\n",time_i[i],gyro_val[i][0],gyro_val[i][1],gyro_val[i][2],acc_val[i][0],acc_val[i][1],acc_val[i][2]);
Joeatsumi 3:32843000531e 339 }
Joeatsumi 3:32843000531e 340 imu_counter = 0;
Joeatsumi 3:32843000531e 341 }else{
Joeatsumi 3:32843000531e 342 imu_counter++;
Joeatsumi 3:32843000531e 343 }
Joeatsumi 1:8757d12d193b 344
Joeatsumi 3:32843000531e 345 if(((logging_counter+1)%20)==0){
Joeatsumi 3:32843000531e 346 //detach the rotary imu mesurement
Joeatsumi 3:32843000531e 347
Joeatsumi 3:32843000531e 348 processGPS();
Joeatsumi 3:32843000531e 349 processGPS();
Joeatsumi 3:32843000531e 350 processGPS();
Joeatsumi 3:32843000531e 351
Joeatsumi 3:32843000531e 352 measurement_time_g = processing_timer.read_us();
Joeatsumi 3:32843000531e 353 //pc.printf("%d\r\n",measurement_time_g);// captureing prossing time
Joeatsumi 0:91d5f51f73e2 354
Joeatsumi 3:32843000531e 355 /*
Joeatsumi 3:32843000531e 356 if(logging_status==1){
Joeatsumi 3:32843000531e 357 fprintf(fp, "%d,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float);
Joeatsumi 3:32843000531e 358 }else if(logging_status==0){}
Joeatsumi 3:32843000531e 359 */
Joeatsumi 3:32843000531e 360
Joeatsumi 3:32843000531e 361 //位置と速度情報を読み取った場合
Joeatsumi 3:32843000531e 362 if((flag_posllh==1)&&(flag_velned==1)){
Joeatsumi 3:32843000531e 363 fprintf(fp, "%d,%f,%f,%f,%f,%f,%f\r\n",measurement_time_g,latitude,longitude,height_float,velN_float,velE_float,velD_float);
Joeatsumi 3:32843000531e 364 pc.printf("U\r\n");
Joeatsumi 3:32843000531e 365 /*Teratermでロギングする用の表示*/
Joeatsumi 3:32843000531e 366 //pc.printf("%f,%f,%f,%f,%f,%f\r\n",latitude,longitude,height_float,velN_float,velE_float,velD_float);
Joeatsumi 3:32843000531e 367 /*計測ではなくデバッグ用の表示*/
Joeatsumi 3:32843000531e 368 //pc.printf("latitude=%f,longitude=%f,height=%f\r\n",latitude,longitude,height_float);
Joeatsumi 3:32843000531e 369 //pc.printf("velN=%f,velE=%f,velD=%f\r\n",velN_float,velE_float,velD_float);
Joeatsumi 1:8757d12d193b 370
Joeatsumi 3:32843000531e 371
Joeatsumi 3:32843000531e 372 /*processGPSの処理時間の表示*/
Joeatsumi 3:32843000531e 373 //pc.printf("processed_time_before(us)=%d\r\n",(processed_time_before));
Joeatsumi 3:32843000531e 374 //pc.printf("processed_time_after(us)=%d\r\n",(processed_time_after));
Joeatsumi 3:32843000531e 375 //pc.printf("processed_time(us)=%d\r\n",processed_time);
Joeatsumi 3:32843000531e 376
Joeatsumi 3:32843000531e 377 /*フラグを0に戻す*/
Joeatsumi 3:32843000531e 378 flag_posllh=0;
Joeatsumi 3:32843000531e 379 flag_velned=0;
Joeatsumi 3:32843000531e 380 }else{}//if((flag_posllh==1)&&(flag_velned==1)){
Joeatsumi 3:32843000531e 381 }else{}//if(((logging_counter+1)%20)==0){
Joeatsumi 3:32843000531e 382
Joeatsumi 3:32843000531e 383
Joeatsumi 3:32843000531e 384 if((logging_counter+1)>=100){
Joeatsumi 3:32843000531e 385 logging_counter = 0;
Joeatsumi 3:32843000531e 386 }else{
Joeatsumi 3:32843000531e 387 logging_counter++;
Joeatsumi 3:32843000531e 388 }
Joeatsumi 3:32843000531e 389
Joeatsumi 3:32843000531e 390 if(log_switch==1){
Joeatsumi 3:32843000531e 391 logging_status=1;
Joeatsumi 3:32843000531e 392 }else if(log_switch==0){
Joeatsumi 3:32843000531e 393 logging_status=0;
Joeatsumi 3:32843000531e 394 fclose(fp);
Joeatsumi 3:32843000531e 395 fclose(im);
Joeatsumi 3:32843000531e 396 pc.printf("FC\r\n");
Joeatsumi 3:32843000531e 397 timer1.detach();
Joeatsumi 1:8757d12d193b 398
Joeatsumi 3:32843000531e 399 //ロガーの動作状態を見るためのLED
Joeatsumi 3:32843000531e 400 //ロガーが記録を終了した表示をする
Joeatsumi 3:32843000531e 401 myled_1 = 0;
Joeatsumi 3:32843000531e 402 myled_2 = 1;
Joeatsumi 3:32843000531e 403
Joeatsumi 0:91d5f51f73e2 404 }else{}
Joeatsumi 1:8757d12d193b 405
Joeatsumi 3:32843000531e 406
Joeatsumi 3:32843000531e 407
Joeatsumi 0:91d5f51f73e2 408 }
Joeatsumi 0:91d5f51f73e2 409
Joeatsumi 0:91d5f51f73e2 410
Joeatsumi 0:91d5f51f73e2 411 /*--------------------------------------------*/
Joeatsumi 0:91d5f51f73e2 412 int main() {
Joeatsumi 0:91d5f51f73e2 413
Joeatsumi 3:32843000531e 414 //power on wait 800ms form IMU
Joeatsumi 3:32843000531e 415 wait(1.0);
Joeatsumi 3:32843000531e 416
Joeatsumi 3:32843000531e 417 //IMU initialize
Joeatsumi 3:32843000531e 418 imu.power_on_sequence1();//IMUが動作可能かどうかの確認
Joeatsumi 3:32843000531e 419 imu.power_on_sequence2();//IMUが動作可能かどうかの確認
Joeatsumi 3:32843000531e 420 imu.UART_CTRL_write();//IMUのボーレートを480600,手動モードへ移行
Joeatsumi 3:32843000531e 421 imu.move_to_sampling_mode();//サンプリングモードへの移行
Joeatsumi 3:32843000531e 422
Joeatsumi 0:91d5f51f73e2 423 //UART initialization
Joeatsumi 0:91d5f51f73e2 424 pc.baud(460800); //115.2 kbps
Joeatsumi 0:91d5f51f73e2 425
Joeatsumi 2:940851e53dde 426 spi.frequency(1000000);
Joeatsumi 1:8757d12d193b 427
Joeatsumi 1:8757d12d193b 428 mkdir("/sd/mydir",0777);//SDファイル作成
Joeatsumi 3:32843000531e 429 fp = fopen("/sd/mydir/gps.csv", "a");//最初のSDopen時間かかるのでwhile外で行う
Joeatsumi 3:32843000531e 430 im = fopen("/sd/mydir/imu.csv", "a");
Joeatsumi 1:8757d12d193b 431
Joeatsumi 1:8757d12d193b 432 if(fp == NULL) {
Joeatsumi 1:8757d12d193b 433 error("Could not open file for write\n");
Joeatsumi 1:8757d12d193b 434 }else{}
Joeatsumi 1:8757d12d193b 435
Joeatsumi 3:32843000531e 436 //ロガーの動作状態を見るためのLED
Joeatsumi 3:32843000531e 437 //初期状態ではロガーが記録中の表示をする
Joeatsumi 3:32843000531e 438 myled_1 = 1;
Joeatsumi 3:32843000531e 439 myled_2 = 0;
Joeatsumi 3:32843000531e 440
Joeatsumi 1:8757d12d193b 441 pc.printf("FO\r\n");//file open
Joeatsumi 1:8757d12d193b 442 logging_status=1;
Joeatsumi 1:8757d12d193b 443
Joeatsumi 1:8757d12d193b 444 wait(0.1);
Joeatsumi 1:8757d12d193b 445
Joeatsumi 0:91d5f51f73e2 446 //フラグのリセット
Joeatsumi 0:91d5f51f73e2 447 flag_posllh=0;
Joeatsumi 0:91d5f51f73e2 448 flag_velned=0;
Joeatsumi 0:91d5f51f73e2 449
Joeatsumi 3:32843000531e 450 imu_counter = 0;
Joeatsumi 3:32843000531e 451 logging_counter = 0;
Joeatsumi 3:32843000531e 452
Joeatsumi 0:91d5f51f73e2 453 //-------------------------------------------
Joeatsumi 0:91d5f51f73e2 454 //Timer
Joeatsumi 0:91d5f51f73e2 455 //-------------------------------------------
Joeatsumi 3:32843000531e 456 //timer1: imu mesurement, 100 Hz
Joeatsumi 3:32843000531e 457 timer1.attach(&ublox_logging, imu_interval);
Joeatsumi 0:91d5f51f73e2 458
Joeatsumi 0:91d5f51f73e2 459 processing_timer.start();//timer starts
Joeatsumi 0:91d5f51f73e2 460
Joeatsumi 3:32843000531e 461 while(1) {
Joeatsumi 3:32843000531e 462 }//while
Joeatsumi 0:91d5f51f73e2 463
Joeatsumi 0:91d5f51f73e2 464 }