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@4:6f167145f4b0, 2022-01-14 (annotated)

Committer:
Joeatsumi
Date:
Fri Jan 14 15:09:03 2022 +0000
Revision:
4:6f167145f4b0
Parent:
3:32843000531e 
stable vertion

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