#include #include #include #include #include #include #include #if defined(_MSC_VER) && (_MSC_VER < 1600) // @see http://stackoverflow.com/questions/126279/c99-stdint-h-header-and-ms-visual-studio #include #else #include #endif #include #include #include #define IS_LITTLE_ENDIAN 1 #include "SylphideStream.h" #include "SylphideProcessor.h" typedef double float_sylph_t; #include "analyze_common.h" #include "calibration.h" #include "INS.h" using namespace std; struct Options : public GlobalOptions { typedef GlobalOptions super_t; bool page_A; bool page_G; bool page_F; bool page_P; bool page_M; bool page_N; bool page_other; bool time_correction; int page_P_mode, page_F_mode, page_M_mode; int debug_level; RotatedCalibrator::rotate_spec_t *rotate_spec; ostream *out_through; struct { bool valid; time_t utc_time; unsigned int itow_sec; } gps_utc; bool use_calendar_time; int localtime_correction_in_seconds; Options() : super_t(), page_A(false), page_G(false), page_F(false), page_P(false), page_M(false), page_N(false), page_other(false), time_correction(false), page_P_mode(0), page_F_mode(3), page_M_mode(0), debug_level(0), rotate_spec(NULL), out_through(NULL), use_calendar_time(false), localtime_correction_in_seconds(0) { gps_utc.valid = false; } ~Options(){ delete rotate_spec; } template string str_time(const T &itow){ stringstream ss; ss.precision(10); if(use_calendar_time){ // year, month, mday, hour, min, sec if(gps_utc.valid){ T interval(itow - gps_utc.itow_sec); time_t interval_time(interval); time_t current(gps_utc.utc_time + interval_time + localtime_correction_in_seconds); tm *current_tm(gmtime(¤t)); ss << current_tm->tm_year + 1900 << ", " << current_tm->tm_mon + 1 << ", " << current_tm->tm_mday << ", " << current_tm->tm_hour << ", " << current_tm->tm_min << ", " << (interval - interval_time + current_tm->tm_sec); }else{ ss << "0, 0, 0, 0, 0, " << itow; } }else{ ss << itow; } return ss.str(); } /** * コマンドに与えられた設定を読み解く * * @param spec コマンド * @return (bool) 解読にヒットした場合はtrue、さもなければfalse */ bool check_spec(const char *spec){ const char *value; #define CHECK_OPTION(name, novalue, operation, disp) { \ if(value = get_value(spec, #name, novalue)){ \ {operation;} \ std::cerr << #name << ": " << disp << std::endl; \ return true; \ } \ } CHECK_OPTION(page_P_mode, false, page_P_mode = atoi(value), page_P_mode); CHECK_OPTION(page_F_mode, false, page_F_mode = atoi(value), page_F_mode); // page_F_mode = [bit0 => 入力を表示, bit1 => 出力を表示] CHECK_OPTION(page_M_mode, false, page_M_mode = atoi(value), page_M_mode); CHECK_OPTION(page_other, true, page_other = is_true(value), (page_other ? "on" : "off")); CHECK_OPTION(time_correction, true, time_correction = is_true(value), (time_correction ? "on" : "off")); if(value = get_value(spec, "calendar_time")){ // 時刻形式をgpstime(ms)ではなくカレンダー時刻とする int correction_hr(0); if(!is_true(value)){ correction_hr = atoi(value); // ローカルタイムの指定は時間で行う } use_calendar_time = true; localtime_correction_in_seconds = 60 * 60 * correction_hr; cerr << "use_calendar_time: UTC " << (correction_hr >= 0 ? '+' : '-') << correction_hr << " [hr]" << endl; return true; } if(value = get_value(spec, "page", false)){ switch(*value){ case 'A': page_A = true; break; case 'G': page_G = true; break; case 'F': page_F = true; break; case 'M': page_M = true; break; case 'N': page_N = true; break; case 'P': page_P = true; break; default: return false; } return true; } // 互換性のため、in_sylphideと等価のdirect_sylphideを残しておく CHECK_OPTION(direct_sylphide, true, in_sylphide = is_true(value), (in_sylphide ? "on" : "off")); CHECK_OPTION(debug, false, debug_level = atoi(value), debug_level); if(value = get_value(spec, "rotate", false)){ RotatedCalibrator::rotate_spec_t decoded; if(!RotatedCalibrator::decode(value, decoded)){ return false; } cerr << "rotate: " << value << endl; rotate_spec = new RotatedCalibrator::rotate_spec_t(decoded); return true; } if(value = get_value(spec, "out_through", false)){ cerr << "out_through: "; out_through = &(spec2ostream(value)); return true; } #undef CHECK_OPTION return super_t::check_spec(spec); } } options; class StreamProcessor : public SylphideProcessor { protected: int invoked; typedef SylphideProcessor super_t; public: /** * Aページ(AD変換結果)の処理用関数 * Aページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Aページのオブザーバー */ struct HandlerA { int count; HandlerA() : count(0) {} void operator()(const super_t::A_Observer_t &observer){ if(!observer.validate()){return;} float_sylph_t current(observer.fetch_ITOW()); if(!options.is_time_in_range(current)){return;} options.out() << (count++) << ", " << options.str_time(current) << ", "; A_Observer_t::values_t values(observer.fetch_values()); for(int i(0); i < 8; i++){ options.out() << values.values[i] << ", "; } options.out() << values.temperature << endl; } } handler_A; /** * Gページ(u-bloxのGPS)の処理用関数 * Gページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * {class, id} = {0x01, 0x02}のとき位置情報が得られる * {class, id} = {0x01, 0x12}のとき速度情報が得られる * * @param obsrever Gページのオブザーバー */ struct HandlerG { unsigned int itow_ms_0x0102, itow_ms_0x0112; super_t::G_Observer_t::position_t position; super_t::G_Observer_t::position_acc_t position_acc; super_t::G_Observer_t::velocity_t velocity; super_t::G_Observer_t::velocity_acc_t velocity_acc; time_t gpstime_zero; HandlerG() : itow_ms_0x0102(0), itow_ms_0x0112(0), position(0, 0, 0), position_acc(0, 0), velocity(0, 0, 0), velocity_acc(0) { tm tm_utc; tm_utc.tm_hour = tm_utc.tm_min = tm_utc.tm_sec = 0; tm_utc.tm_mday = 6; tm_utc.tm_mon = 0; tm_utc.tm_year = 80; tm_utc.tm_isdst = 0; gpstime_zero = mktime(&tm_utc); // 1980/1/6 00:00:00 } ~HandlerG(){} void operator()(const super_t::G_Observer_t &observer){ if(!observer.validate()){return;} bool change_itow(false); super_t::G_Observer_t::packet_type_t packet_type(observer.packet_type()); switch(packet_type.mclass){ case 0x01: { switch(packet_type.mid){ case 0x02: { position = observer.fetch_position(); position_acc = observer.fetch_position_acc(); itow_ms_0x0102 = observer.fetch_ITOW_ms(); change_itow = true; break; } case 0x12: { velocity = observer.fetch_velocity(); velocity_acc = observer.fetch_velocity_acc(); itow_ms_0x0112 = observer.fetch_ITOW_ms(); change_itow = true; break; } #if 1 case 0x20: { if(!options.use_calendar_time){break;} char buf[2]; observer.inspect(buf, sizeof(buf), 6 + 10); if(!((unsigned char)buf[1] & 0x04)){break;}// Invalid UTC char leap_seconds(buf[0]); observer.inspect(buf, sizeof(buf), 6 + 8); unsigned short gps_week(le_char2_2_num(*buf)); options.gps_utc.itow_sec = (observer.fetch_ITOW_ms() / 1000); options.gps_utc.utc_time = gpstime_zero + (7u * 24 * 60 * 60) * gps_week + options.gps_utc.itow_sec - leap_seconds; // POSIX time ignores leap seconds. options.gps_utc.valid = true; break; } #else case 0x21: { if(!options.use_calendar_time){break;} super_t::G_Observer_t::utc_t gps_utc(observer.fetch_utc()); if(!gps_utc.valid){break;} tm tm_utc; tm_utc.tm_sec = gps_utc.seconds_of_minute; tm_utc.tm_min = gps_utc.minute_of_hour; tm_utc.tm_hour = gps_utc.hour_of_day; tm_utc.tm_mday = gps_utc.day_of_month; tm_utc.tm_mon = gps_utc.month - 1; tm_utc.tm_year = gps_utc.year - 1900; //tm_utc.tm_wday; //tm_utc.tm_yday; tm_utc.tm_isdst = 0; options.gps_utc.utc_time = mktime(&tm_utc); options.gps_utc.itow_sec = (observer.fetch_ITOW_ms() / 1000); options.gps_utc.valid = true; break; } #endif } break; } default: break; } if(!options.page_G){return;} if(change_itow && (itow_ms_0x0102 == itow_ms_0x0112)){ float_sylph_t current(1E-3 * itow_ms_0x0102); if(!options.is_time_in_range(current)){return;} options.out() << options.str_time(current) << ", " << position.latitude << ", " << position.longitude << ", " << position.altitude << ", " << position_acc.horizontal << ", " << position_acc.vertical << ", " << velocity.north << ", " << velocity.east << ", " << velocity.down << ", " << velocity_acc.acc << endl; } } } handler_G; /** * Fページ(FPGAサブシステムからの情報)の処理用関数 * Fページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Fページのオブザーバー */ struct HandlerF { int count; HandlerF() : count(0) {} void operator()(const F_Observer_t &observer){ if(!observer.validate()){return;} float_sylph_t current(observer.fetch_ITOW()); if(!options.is_time_in_range(current)){return;} options.out() << (count++) << ", " << options.str_time(current); F_Observer_t::values_t values(observer.fetch_values()); for(int i = 0; i < 8; i++){ //if(values.servo_in[i] < 1000){values.servo_in[i] += 1000;} if(options.page_F_mode & 0x01){ // ビット0が入力を表示 options.out() << ", " << values.servo_in[i]; } if(options.page_F_mode & 0x02){ // ビット1が出力を表示 options.out() << ", " << values.servo_out[i]; } } options.out() << endl; } } handler_F; struct MultipleSnapshotsHandler { float_sylph_t previous_itow; MultipleSnapshotsHandler() : previous_itow(0) {} float_sylph_t time_check(const float_sylph_t ¤t_itow){ float_sylph_t delta_t(current_itow - previous_itow); previous_itow = current_itow; if((delta_t < 0) || (delta_t > 10)){return 0;} // 異常検出(時刻とび、ITOW一周に対応) return delta_t; } }; struct HandlerP : public MultipleSnapshotsHandler { HandlerP() : MultipleSnapshotsHandler() {} void ms5611_convert( const int32_t &d1, const int32_t &d2, int32_t &pressure, int32_t &temperature, const uint16_t (&coef)[6]) const { int32_t dT(d2 - (coef[4] << 8)); temperature = (int32_t)2000 + (((int64_t)dT * coef[5]) >> 23); int64_t off(((int64_t)coef[1] << 16) + (((int64_t)coef[3] * dT) >> 7)); int64_t sens(((int64_t)coef[0] << 15) + (((int64_t)coef[2] * dT) >> 8)); // Figure 3 if(temperature < 2000){ int32_t t2(((int64_t)dT * dT) << 31); int32_t dT2(temperature - 2000), dT2_2(dT2 * dT2); int32_t off2((dT2_2 * 5) >> 1); int32_t sens2((dT2_2 * 5) >> 2); if(temperature < -1500){ int32_t dT3(temperature + 1500), dT3_2(dT3 * dT3); off2 += dT3_2 * 7; sens2 += (dT3_2 * 11) >> 1; } temperature -= t2; off -= off2; sens -= sens2; } pressure = (int32_t)(((((int64_t)d1 * sens) >> 21) - off) >> 15); } /** * Pページ(エアデータセンサからの情報)の処理用関数 * Pページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Fページのオブザーバー */ void operator()(const P_Observer_t &observer){ if(!observer.validate()){return;} float_sylph_t current(observer.fetch_ITOW()); if(!options.is_time_in_range(current)){return;} float_sylph_t time_diff(options.time_correction ? time_check(current) : 0); P_Observer_t::values_t values(observer.fetch_values()); switch(options.page_P_mode){ case 1 : { float_sylph_t time_step(time_diff / 3); #define s(x) (signed short)(x) options.out() << options.str_time(current - time_step * 2) << ", " << -2 << ", " << s(values.air_speed[0]) << ", " << s(values.air_alpha[0]) << ", " << s(values.air_beta[0]) << ", " << s(values.air_speed[1]) << endl; options.out() << options.str_time(current - time_step) << ", " << -1 << ", " << s(values.air_alpha[1]) << ", " << s(values.air_beta[1]) << ", " << s(values.air_speed[2]) << ", " << s(values.air_alpha[2]) << endl; options.out() << options.str_time(current) << ", " << 0 << ", " << s(values.air_beta[2]) << ", " << s(values.air_speed[3]) << ", " << s(values.air_alpha[3]) << ", " << s(values.air_beta[3]) << endl; #undef s break; } case 2 : char buf[256]; sprintf(buf, "%s, %8.5f, %8.5f, %8.5f, %8.5f", options.str_time(current).c_str(), 0.36 * values.air_beta[2], 0.1 * (short)(values.air_speed[3]), 0.1 * (short)(values.air_alpha[3]), 0.1 * (short)(values.air_beta[3])); options.out() << buf << endl; break; case 3: { // ADSの8ch 16bitsを全て活用する場合 options.out() << options.str_time(current) << ", " << 0; char buf[2]; for(int i = 0; i < 8; i++){ observer.inspect(&(buf[0]), sizeof(buf), 7 + (sizeof(buf) * i)); options.out() << ", " << be_char2_2_num(buf[0]); } options.out() << endl; break; } case 4: { // MS5611 float_sylph_t time_step(time_diff / 3); char buf[4]; for(int i(0), j(-2); i < 3; i++, j++){ options.out() << options.str_time(current + time_step * j) << ", " << j << ", "; observer.inspect(&(buf[0]), sizeof(buf), 7 + (8 * i)); options.out() << be_char4_2_num(buf[0]) << ", "; // (Raw) pressure observer.inspect(&(buf[0]), sizeof(buf), 7 + (8 * i) + 4); options.out() << be_char4_2_num(buf[0]) << endl; // (Raw) temperature } break; } case 5: { // MS5611 with coefficients uint16_t coef[6]; char buf[sizeof(uint16_t)]; for(int i(0); i < sizeof(coef) / sizeof(coef[0]); i++){ observer.inspect(&buf[0], sizeof(buf), 19 + (sizeof(buf) * i)); coef[i] = be_char2_2_num(*buf); } float_sylph_t time_step(time_diff / 2); for(int i(0), j(-1); i < 2; i++, j++){ options.out() << options.str_time(current + time_step * j) << ", " << j << ", "; char buf[2][4]; buf[0][0] = buf[1][0] = 0; observer.inspect(&buf[0][1], 3, 7 + 6 * i); observer.inspect(&buf[1][1], 3, 10 + 6 * i); uint32_t d1(be_char4_2_num(buf[0][0])), d2(be_char4_2_num(buf[1][0])); int32_t pressure, temperature; ms5611_convert(d1, d2, pressure, temperature, coef); options.out() << pressure << ", " << temperature << endl; } break; } default : { float_sylph_t time_step(time_diff / 4); for(int i(0), j(-3); i < 4; i++, j++){ options.out() << options.str_time(current + time_step * j) << ", " << j << ", " << values.air_speed[i] << ", " << values.air_alpha[i] << ", " << values.air_beta[i] << endl; } } } } } handler_P; /** * Mページ(磁気センサからの情報)の処理用関数 * Mページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Mページのオブザーバー */ struct HandlerM : public MultipleSnapshotsHandler { HandlerM() : MultipleSnapshotsHandler() {} void operator()(const M_Observer_t &observer){ if(!observer.validate()){return;} float_sylph_t current(observer.fetch_ITOW()); if(!options.is_time_in_range(current)){return;} float_sylph_t time_step(options.time_correction ? time_check(current) / 4 : 0); M_Observer_t::values_t values(observer.fetch_values()); switch(options.page_M_mode){ case 1: // -atan2(y, x)した結果[deg]を表示 for(int i(0), j(-3); i < 4; i++, j++){ options.out() << options.str_time(current + time_step * j) << ", " << j << ", " << rad2deg(-atan2((double)values.y[i], (double)values.x[i])) << endl; } break; default: for(int i(0), j(-3); i < 4; i++, j++){ options.out() << options.str_time(current + time_step * j) << ", " << j << ", " << values.x[i] << ", " << values.y[i] << ", " << values.z[i] << endl; } } } } handler_M; /** * Nページ(航法情報)の処理用関数 * Nページの内容が正しいかvalidateで確認した後、実処理を行うこと。 * * @param obsrever Nページのオブザーバー */ struct HandlerN { void operator()(const N_Observer_t &observer){ if(!observer.validate()){return;} float_sylph_t current(observer.fetch_ITOW()); if(!options.is_time_in_range(current)){return;} switch(observer.kind()){ case 0: { N_Observer_t::navdata_t values(observer.fetch_navdata()); // 座標変換が必要な場合は行う if(options.rotate_spec){ Quarternion orig_q( INS::euler2q( deg2rad(values.heading), deg2rad(values.pitch), deg2rad(values.roll))); Quarternion q( options.rotate_spec->rotate_mat() * orig_q.getDCM()); values.heading = rad2deg(INS::q2psi(q)); values.pitch = rad2deg(INS::q2theta(q)); values.roll = rad2deg(INS::q2phi(q)); } options.out() << options.str_time(values.itow) << ", " << values.longitude << ", " << values.latitude << ", " << values.altitude << ", " << values.v_north << ", " << values.v_east << ", " << values.v_down << ", " << values.heading << ", " << values.pitch << ", " << values.roll << endl; break; } default: return; } } } handler_N; #if 0 /** * SylphideProtocol形式で入ってくるCページ用の処理器 * */ class SylphideStreamHandler : public Sylphide_Packet_Observer { protected: typedef Sylphide_Packet_Observer super_t; typedef SylphideStreamHandler self_t; public: bool previous_seek; unsigned int invoked; SylphideStreamHandler() : super_t(PAGE_SIZE), previous_seek(false), invoked(0) {} ~SylphideStreamHandler() {} /** * パケットが見つかった際に呼び出される関数 * */ void operator()(const self_t &observer){ if(!observer.validate()){return;} } } handler_C; #endif public: StreamProcessor() : super_t(), invoked(0) { } ~StreamProcessor(){} /** * ファイル等のストリームから1ページ単位で処理を行う関数 * * @param in ストリーム * @return (bool) 処理が成功したかどうか */ void process(istream &in){ char buffer[PAGE_SIZE]; while(true){ int read_count; in.read(buffer, PAGE_SIZE); read_count = in.gcount(); if(in.fail() || (read_count == 0)){return;} invoked++; if(options.out_through){ options.out_through->write(buffer, read_count); } if(options.debug_level){ cerr << "--read-- : " << invoked << " page" << endl; cerr << hex; for(int i(0); i < read_count; i++){ cerr << setfill('0') << setw(2) << (unsigned int)((unsigned char)buffer[i]) << ' '; } cerr << dec; cerr << endl; if(read_count < PAGE_SIZE){ cerr << "--skipped-- : " << invoked << " page ; count = " << read_count << endl; } } switch(buffer[0]){ #define assign_case_cnd(type, mark, cnd) \ case mark: if(cnd){ \ super_t::process_packet( \ buffer, read_count, \ observer_ ## type , previous_seek_next_ ## type, handler_ ## type); \ } \ break; #define assign_case(type, mark) assign_case_cnd(type, mark, options.page_ ## type) assign_case(A, 'A'); assign_case_cnd(G, 'G', true); assign_case(F, 'F'); assign_case(P, 'P'); assign_case(M, 'M'); assign_case(N, 'N'); #undef assign_case #if 0 case 'C': if(options.out_C){ super_t::process_packet( buffer, read_count, handler_C, handler_C.previous_seek, handler_C); } break; #endif default: if(options.page_other){ if(buffer[0] == 'T'){ stringstream ss; ss << hex; for(int i(0); i < read_count; i++){ ss << setfill('0') << setw(2) << (unsigned int)((unsigned char)buffer[i]) << ' '; } ss << endl; options.out() << ss.str(); } } break; } } } }; int main(int argc, char *argv[]){ if(argc < 2){ cerr << "Usage: " << argv[0] << " log.dat [options] [--page=(A|F|M|P)]" << endl; return -1; } // 互換性維持のため if(strstr(argv[0], "log_AD_CSV")){ options.page_A = true; }else if(strstr(argv[0], "log_F_CSV")){ options.page_F = true; }else if(strstr(argv[0], "log_M_CSV")){ options.page_M = true; }else if(strstr(argv[0], "log_P_CSV")){ options.page_P = true; } StreamProcessor processor; int arg_index(2); // 個別の設定(互換性確保) if(options.page_P){ // [option = 0:6bytes/packet; 1:8bytes/packet] options.page_P_mode = atoi(argv[arg_index++]); } // オプションによる出力の設定 for(; arg_index < argc; arg_index++){ if(options.check_spec(argv[arg_index])){continue;} cerr << "Unknown option!! : " << argv[arg_index] << endl; return -1; } options.out().precision(10); if(options.in_sylphide){ SylphideIStream sylph_in(options.spec2istream(argv[1]), PAGE_SIZE); processor.process(sylph_in); }else{ processor.process(options.spec2istream(argv[1])); } return 0; }