Atsumi Toda / Mbed 2 deprecated INS_GNSS_logger_1

imu/gnss logger ver1.1

Dependencies:   mbed MPU9250_SPI MG354PDH0 SDFileSystem

main.cpp@2:9216162a9e17, 2021-05-15 (annotated)

Committer:
Joeatsumi
Date:
Sat May 15 08:14:50 2021 +0000
Revision:
2:9216162a9e17
Parent:
1:8757d12d193b
Child:
3:d564c0a27ba7
imu/gnss logger ver1.1

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