From: Naoya Takamura Date: Fri, 9 Dec 2011 08:20:22 +0000 (+0900) Subject: 1sec and high smpling file record add. X-Git-Url: http://git.osdn.net/view?p=scilog%2Fscilog.git;a=commitdiff_plain;h=1502df658e0de66863f3c51d862ce91239f4c0a5 1sec and high smpling file record add. --- diff --git a/Makefile b/Makefile index cbb33cb..aa1ff2f 100644 --- a/Makefile +++ b/Makefile @@ -30,9 +30,9 @@ INCDIR = ${STAGEDIR}/include TARGET = scilog -OBJS = scilog.o spi.o thread_rcv.o +OBJS = scilog.o spi.o thread_rcv.o ring.o ad_ring.o thread_rec.o sts.o ad_file.o conf.o -HDRS = mes_print.h debug_print.h spi.h my_thread.h +HDRS = mes_print.h debug_print.h spi.h my_thread.h ring.h ad_ring.h thread_rec.h sts.h ad_file.h conf.h ${TARGET} : $(OBJS) $(HDRS) ${CC} ${CFLAGS} ${OBJS} -L ${LIBDIR} -o ${TARGET} diff --git a/ad_file.c b/ad_file.c new file mode 100644 index 0000000..fd3ca15 --- /dev/null +++ b/ad_file.c @@ -0,0 +1,260 @@ +/* + 1secファイル + 高速サンプルファイル + +#define DEBUG_FILE_MIN + 定義するとファイルが1日単位ではなく指定された分単位になる + ただし、ファイルのサイズは1日と同じサイズのまま +#deifne DEBUG_FILE_MIN_PERIOD + ファイルの単位 + 単位 分 + +*/ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ad_ring.h" +#include "ad_file.h" +#include "debug_print.h" +#include "conf.h" + +// mkdir_p()で使用する +#define MKDIR_MODE "755" + +/**** util +*/ +/* + 指定さえれたディレクトリを作成 + 親がないときは親まで作る + char *mode + 755とか + return + 0=OK + -1=ERROR +*/ +static int mkdir_p(const char *path, char *mode) +{ + struct stat st; + char cmd[128]; + + // コピー先ディレクトリ存在しなければ作成する + if (stat(path, &st) == -1) { + PDEBUG("mkdir_p(): make dir %s\n", path); + + sprintf(cmd, "mkdir -p -m %s %s > /dev/null 2> /dev/null &", mode, path); + switch(system(cmd)) { + case 127: + // shell exec error + PDEBUG("mkdir_p(): shell exec error\n"); + syslog(LOG_ERR, "mkdir_p(): system() shell exec error"); + return -1; + case -1: + // error + PDEBUG("mkdir_p(): error\n"); + syslog(LOG_ERR, "mkdir_p(): system() error"); + return -1; + default: + // 作成されるまで待つ + sleep(3); + return 0; + } + } + return 0; +} + + +/**** 1秒ファイル関係 ************************************************************ +*/ +/* + 1秒データファイル オープン + + Return: + 1 = Error + 0 = OK + */ +int sec_file_open(FILE **fp, AdData *D) +{ + char szDir[64]; + struct stat st; + char fname[64]; + char path[128]; + + // サブディレクトリ作成 + sprintf(szDir, "%s/%s/%04d/%02d", DIR_DATA, sid_getp(), D->t.tm_year+1900, D->t.tm_mon+1); + // ディレクトリ存在しなければ作成する + if (stat(szDir, &st) == -1) mkdir_p(szDir, MKDIR_MODE); + + // ファイル名作成 +#ifdef DEBUG_FILE_MIN + sprintf(fname, "%04d%02d%02d_%02d%02d.sec", + D->t.tm_year+1900, D->t.tm_mon+1, D->t.tm_mday, D->t.tm_hour, D->t.tm_min / DEBUG_FILE_MIN_PERIOD * DEBUG_FILE_MIN_PERIOD); +#else + sprintf(fname, "%04d%02d%02d.sec", D->t.tm_year+1900, D->t.tm_mon+1, D->t.tm_mday); +#endif + // フルパス作成 + strcpy(path, szDir); + strcat(path, "/"); + strcat(path, fname); + PDEBUGF("path=%s\n", path); + + *fp = fopen(path, "a"); + if (*fp == NULL) { + syslog(LOG_ERR, "%s(): fopen() ERROR. %s", __FUNCTION__, strerror(errno)); + return 1; + } + return 0; +} +/* + Return: + 1 = Error + 0 = OK + */ +int sec_file_out(FILE *fp, char *out, int len) +{ + int ret; + + if (fp == NULL) return 1; + if ((ret = fwrite(out, 1, len, fp)) < len) { + syslog(LOG_ERR, "%s(): fwrite() return %d < %d. %s", __FUNCTION__, ret, len, strerror(errno)); + return 1; + } + return 0; +} +/* + 1秒データファイル クローズ + + Return: + 1 = Error + 0 = OK + */ +int sec_file_close(FILE *fp) +{ + if (fp == NULL) return 1; + fclose(fp); + return 0; +} + +/* + 1secファイル記録用データ作る +*/ +int sec_make_rec_data(AdData *ad, char *buf) +{ + char buf2[32]; + int ch; + + sprintf(buf, "%04d/%02d/%02d %02d:%02d:%02d,%3lu,%02X", + ad->gps.year, ad->gps.month, ad->gps.day, ad->gps.hour, ad->gps.min, ad->gps.sec, + ad->gps.tacc, ad->gps.valid); + for(ch = 0; ch < AD_CHNUM; ch++) { + sprintf(buf2, ",%+7ld", ad->data1sec[ch]); + strcat(buf, buf2); + } + strcat(buf, "\n"); + return 0; +} +/**** 高速サンプルファイル関係 ************************************************************ +*/ +/* + Return: + 1 = Error + 0 = OK + */ +int high_file_open(FILE **fp, AdData *D) +{ + char szDir[64]; + struct stat st; + char fname[64]; + char path[128]; + + // サブディレクトリ作成 + sprintf(szDir, "%s/%s/%04d/%02d", DIR_DATA, sid_getp(), D->t.tm_year+1900, D->t.tm_mon+1); + // ディレクトリ存在しなければ作成する + if (stat(szDir, &st) == -1) mkdir_p(szDir, MKDIR_MODE); + + // ファイル名作成 +#ifdef DEBUG_FILE_MIN + sprintf(fname, "%04d%02d%02d_%02d%02d.sec", + D->t.tm_year+1900, D->t.tm_mon+1, D->t.tm_mday, D->t.tm_hour, D->t.tm_min / DEBUG_FILE_MIN_PERIOD * DEBUG_FILE_MIN_PERIOD); +#else + sprintf(fname, "%04d%02d%02d%02d.high", D->t.tm_year+1900, D->t.tm_mon+1, D->t.tm_mday, D->t.tm_hour); +#endif + // フルパス作成 + strcpy(path, szDir); + strcat(path, "/"); + strcat(path, fname); + PDEBUGF("path=%s\n", path); + + *fp = fopen(path, "a"); + if (*fp == NULL) { + syslog(LOG_ERR, "%s(): fopen() ERROR. %s", __FUNCTION__, strerror(errno)); + return 1; + } + return 0; +} +/* + Return: + 1 = Error + 0 = OK + */ +int high_file_out(FILE *fp, char *out, int len) +{ + int ret; + + if (fp == NULL) return 1; + if ((ret = fwrite(out, 1, len, fp)) < len) { + syslog(LOG_ERR, "%s(): fwrite() return %d < %d. %s", __FUNCTION__, ret, len, strerror(errno)); + return 1; + } + return 0; +} +/* + Return: + 1 = Error + 0 = OK + */ +int high_file_close(FILE *fp) +{ + if (fp == NULL) return 1; + fclose(fp); + return 0; +} + +/* + 記録用データ作る +*/ +int high_make_rec_data(AdData *ad, char *ptr) +{ + unsigned char *src; + int ch, i, j; + + *ptr++ = ad->gps.year & 0xFF; + *ptr++ = (ad->gps.year >> 8) & 0xFF; + *ptr++ = ad->gps.month; + *ptr++ = ad->gps.day; + *ptr++ = ad->gps.hour; + *ptr++ = ad->gps.min; + *ptr++ = ad->gps.sec; + *ptr++ = ad->gps.nano; + *ptr++ = ad->gps.valid; + // + src = ad->data; + for(ch = 0; ch < AD_CHNUM; ch++) { + for(i = 0; i < AD_SAMPLE; i++) { + for(j = 0; j < AD_BYTES; j++) { + *ptr++ = *src++; + } + src++; + } + } + return 0; +} + + + + diff --git a/ad_file.h b/ad_file.h new file mode 100644 index 0000000..f479e2d --- /dev/null +++ b/ad_file.h @@ -0,0 +1,53 @@ +#if !defined(__AD_FILE_H__) +#define __AD_FILE_H__ + +#include "ad_ring.h" + +// データ記録用dir 末尾に/付けない +#ifdef DUMMY + #define DIR_DATA "/home/ntaka/dev/arm2011n/data" +#else + #define DIR_DATA "/home/data" +#endif + +#if 0 +// NAV-TIMEUTCパケットのファイル記録用データタイプ +typedef struct { + unsigned long tow; // ms GPS Millisecond Time of Week + unsigned long tacc; // ns Time Accuracy Estimate + long nano; // ns Nanoseconds of second, range -1e9 .. 1e9 (UTC) + unsigned int year __attribute__((aligned(1))); // 12 UTC + unsigned char month __attribute__((aligned(1))); // 16 + unsigned char day __attribute__((aligned(1))); + unsigned char hour __attribute__((aligned(1))); + unsigned char min __attribute__((aligned(1))); + unsigned char sec __attribute__((aligned(1))); + unsigned char valid __attribute__((aligned(1))); // Validity Flags + // bit0 validTOW 1 = Valid Time of Week + // bit1 validWKN 1 = Valid Week Number + // bit2 validUTC 1 = Valid UTC (Leap Seconds already known) +} UbloxNavTimeUtcRecType __attribute__((aligned(1))); + +// 高速サンプルデータ記録用データタイプ +typedef struct { + UbloxNavTimeUtcRecType gps; // GPSタイムスタンプ + unsigned char data[AD_CHNUM][AD_SAMPLE][AD_BYTES]; // AD +} HighSampleRecType; + +#define HIGH_WRITE_LEN szieof(HighSampleRecType) +#endif + +#define HIGH_WRITE_LEN (9 + AD_CHNUM*AD_SAMPLE*AD_BYTES) // 仮で50Hz 記録周波数で変わるので要変更!!!!!!!!!!!!!! + + +int sec_file_open(FILE **fp, AdData *D); +int sec_file_out(FILE *fp, char *out, int len); +int sec_file_close(FILE *fp); +int sec_make_rec_data(AdData *ad, char *buf); + +int high_file_open(FILE **fp, AdData *D); +int high_file_out(FILE *fp, char *out, int len); +int high_file_close(FILE *fp); +int high_make_rec_data(AdData *ad, char *buf); + +#endif diff --git a/ad_ring.c b/ad_ring.c new file mode 100644 index 0000000..ccec6ef --- /dev/null +++ b/ad_ring.c @@ -0,0 +1,200 @@ +#include +#include + + +#include "ring.h" +#include "ad_ring.h" +#include "mes_print.h" + +// Ring Buffer読み込みポインタ個数 +#define AD_RING_READ_NUM 2 +// Ring Buffer読み込みポインター 1sec記録用 +#define AD_RING_READ_PTR1 0 +// Ring Buffer読み込みポインター 1min記録用 +#define AD_RING_READ_PTR2 1 + +static AdData ad_ring_buf[AD_RING_NUM]; + +// ミューテックス +static pthread_mutex_t mutex_ad_ring = PTHREAD_MUTEX_INITIALIZER; + +// Ring buffer +static RING_T *ring; + +int ad_ring_init(void) +{ +pthread_mutex_lock(&mutex_ad_ring); + ring = ring_create(AD_RING_READ_NUM); + if (ring == NULL) { + PERRF("ERROR ring_create() == NULL\n"); +pthread_mutex_unlock(&mutex_ad_ring); + return -1; + } + ring_init(ring, AD_RING_NUM); +pthread_mutex_unlock(&mutex_ad_ring); + return 0; +} +void ad_ring_clear_sec(void) +{ + ring_clear(ring, AD_RING_READ_PTR1); +} +void ad_ring_clear_high(void) +{ + ring_clear(ring, AD_RING_READ_PTR2); +} +void ad_ring_clear_latest(void) +{ + ring_latest_set(ring, -1); +} + +// 読み出し位置 1secデータ記録で使用 +int ad_ring_read_get_sec(void) +{ + return ring_read_get(ring, AD_RING_READ_PTR1); +} +void ad_ring_read_set_sec(int i) +{ + ring_read_set(ring, AD_RING_READ_PTR1, i); +} +void ad_ring_read_plus_sec(void) +{ + ring_read_plus(ring, AD_RING_READ_PTR1); +} +// 読み出し位置 高速データ記録で使用 +int ad_ring_read_get_high(void) +{ + return ring_read_get(ring, AD_RING_READ_PTR2); +} +void ad_ring_read_set_high(int i) +{ + ring_read_set(ring, AD_RING_READ_PTR2, i); +} +void ad_ring_read_plus_high(void) +{ + ring_read_plus(ring, AD_RING_READ_PTR2); +} + +// 書き込み位置 AD受信で使用 +int ad_ring_write_get(void) +{ + return ring_write_get(ring); +} +void ad_ring_write_plus(void) +{ + ring_write_plus(ring); +} +// 読み込んでいないデータ数 +int ad_ring_num_get_sec(void) +{ + return ring_num_get(ring, AD_RING_READ_PTR1); +} +// 読み込んでいないデータ数 +int ad_ring_num_get_high(void) +{ + return ring_num_get(ring, AD_RING_READ_PTR2); +} +// 最新データ位置 表示 +int ad_ring_latest_get(void) +{ + return ring_latest_get(ring); +} +void ad_ring_latest_set(int i) +{ + ring_latest_set(ring, i); +} +// データ取得 +AdData* ad_ring_get(int ptr) +{ + AdData* p; +pthread_mutex_lock(&mutex_ad_ring); + p = &ad_ring_buf[ptr]; +pthread_mutex_unlock(&mutex_ad_ring); + return p; +} +/* + バッファにデータコピー +*/ +void ad_ring_set(AdData *data) +{ + int i; + +pthread_mutex_lock(&mutex_ad_ring); + i = ring_write_get(ring); + ad_ring_buf[i] = *data; + // 最新データ位置セット + ring_latest_set(ring, i); +pthread_mutex_unlock(&mutex_ad_ring); +} + +/* + パケットバッファフル? + 1=Full + 0=not Full +*/ +int ad_ring_is_full(void) +{ + return ring_is_full(ring, AD_RING_READ_PTR1); +} + + +/**** CUnit test +*/ +#ifdef CUNIT +static void test_ad_ring(void) +{ + ad_ring_init(); + + // CLEAR + ad_ring_clear_sec(); + CU_ASSERT(ad_ring_read_get_sec() == 0); + ad_ring_clear_high(); + CU_ASSERT(ad_ring_read_get_high() == 0); + ad_ring_clear_latest(); + CU_ASSERT(ad_ring_latest_get() == -1); + + // PLUS + ad_ring_read_plus_sec(); + CU_ASSERT(ad_ring_read_get_sec() == 1); + ad_ring_read_plus_high(); + CU_ASSERT(ad_ring_read_get_high() == 1); + ad_ring_write_plus(); + CU_ASSERT(ad_ring_write_get() == 1); + + // 境界値でPLUS + ad_ring_read_set_sec(AD_RING_NUM - 1); + ad_ring_read_plus_sec(); + CU_ASSERT(ad_ring_read_get_sec() == 0); + + ad_ring_read_set_high(AD_RING_NUM - 1); + ad_ring_read_plus_high(); + CU_ASSERT(ad_ring_read_get_high() == 0); + + ring->write = AD_RING_NUM - 1; + ad_ring_write_plus(); + CU_ASSERT(ad_ring_write_get() == 0); + + // + ring->write = 1; + ring->read[AD_RING_READ_PTR1] = 0; + CU_ASSERT(ad_ring_num_get_sec() == 1); + + ring->write = 0; + ring->read[AD_RING_READ_PTR1] = AD_RING_NUM - 1; + CU_ASSERT(ad_ring_num_get_sec() == 1); + + ring->write = 1; + ring->read[AD_RING_READ_PTR2] = 0; + CU_ASSERT(ad_ring_num_get_high() == 1); + + ring->write = 0; + ring->read[AD_RING_READ_PTR2] = AD_RING_NUM - 1; + CU_ASSERT(ad_ring_num_get_high() == 1); + +} + +void ad_ring_test(CU_pSuite test_suite) +{ + CU_add_test(test_suite, "test_ad_ring", test_ad_ring); + +} +#endif diff --git a/ad_ring.h b/ad_ring.h new file mode 100644 index 0000000..f63ddd0 --- /dev/null +++ b/ad_ring.h @@ -0,0 +1,70 @@ +#if !defined(__AVG_RING_H__) +#define __AVG_RING_H__ + +#include + +// リングバッファサイズ 個数 +#define AD_RING_NUM (60 * 5) + +#define AD_CHNUM 6 // AD CH数 +#define AD_SAMPLE 50 // AD ICの出力レート [Hz] +#define AD_BYTES 3 // AD 1sample dataのバイト数 + +// NAV-TIMEUTCパケットのデータ格納 +typedef struct { + unsigned long tow; // ms GPS Millisecond Time of Week + unsigned long tacc; // ns Time Accuracy Estimate + long nano; // ns Nanoseconds of second, range -1e9 .. 1e9 (UTC) + unsigned int year; // UTC + unsigned char month; // + unsigned char day; + unsigned char hour; + unsigned char min; + unsigned char sec; + unsigned char valid; // Validity Flags + // bit0 validTOW 1 = Valid Time of Week + // bit1 validWKN 1 = Valid Week Number + // bit2 validUTC 1 = Valid UTC (Leap Seconds already known) +} UbloxNavTimeUtc; + +// リングバッファのデータタイプ +typedef struct { + UbloxNavTimeUtc gps; // GPSタイムスタンプ SPIで受信 + struct tm t; // タイムスタンプ struct tmの月は(0〜11)なので注意 年は1900からのオフセット + long data1sec[AD_CHNUM]; // 1sec平均値 SPIで受信 + long data[AD_CHNUM][AD_SAMPLE]; // AD_SAMPLE[Hz]の生データ SPIで受信 + long avg[AD_CHNUM][AD_SAMPLE]; // 平均後データ +} AdData; + + + +int ad_ring_init(void); + +void ad_ring_clear_sec(void); +void ad_ring_clear_high(void); +void ad_ring_clear_latest(void); + +int ad_ring_read_get_sec(void); +void ad_ring_read_set_sec(int i); +void ad_ring_read_plus_sec(void); +int ad_ring_num_get_sec(void); + +int ad_ring_read_get_high(void); +void ad_ring_read_set_high(int i); +void ad_ring_read_plus_high(void); +int ad_ring_num_get_high(void); + +int ad_ring_write_get(void); +void ad_ring_write_plus(void); +int ad_ring_latest_get(void); +void ad_ring_latest_set(int i); +AdData* ad_ring_get(int ptr); +void ad_ring_set(AdData *data); +int ad_ring_full(void); + +#ifdef CUNIT + #include + void ad_ring_test(CU_pSuite test_suite); +#endif + +#endif diff --git a/conf.c b/conf.c new file mode 100644 index 0000000..da6e9af --- /dev/null +++ b/conf.c @@ -0,0 +1,154 @@ +#include +#include +#include +#include +#include + +#include "conf.h" +#include "debug_print.h" + +// +/**** Station ID *************************************************** +*/ +#define SID_MAX 16 +static char gsid[SID_MAX + 1]; + +// ミューテックス +static pthread_mutex_t mutex_sid = PTHREAD_MUTEX_INITIALIZER; + +void sid_set(char *s) +{ +pthread_mutex_lock(&mutex_sid); + strncpy(gsid, s, SID_MAX); + gsid[SID_MAX] = 0; +pthread_mutex_unlock(&mutex_sid); +} +void sid_get(char *s) +{ +pthread_mutex_lock(&mutex_sid); + strncpy(s, gsid, SID_MAX); + s[SID_MAX] = 0; +pthread_mutex_unlock(&mutex_sid); +} +char *sid_getp(void) +{ + return gsid; +} + +// +/**** 設定ファイル ******************************************* +*/ +/* + 文字列の前から空白を取り除く +*/ +static void trims_space(char *name) +{ + char c; + char *name0; + int len; + + name0 = name; + len = strlen(name); + + // 最初の非空白文字までポインタ移動 + while (*name) { + c = *name; + if (c != ' ') break; + name++; + len--; + } + if (len < 0) return; + // 移動 + if (name0 != name) memmove(name0, name, len); + // 末尾に0入れる + name0[len] = 0; +} +/* + 文字列の後ろから空白を取り除く +*/ +static void trime_space(char *name) +{ + char c; + int len, i; + + // 文字列末尾 + len = strlen(name); + name += (len - 1); + // 最初の非空白文字までポインタ移動 + for(i = len - 1; i >= 0; i--) { + c = *name; + if (c != ' ') break; + name--; + } + // 末尾に0入れる + *(name + 1) = 0; +} +/* + 文字列の前後から空白を取り除く +*/ +static void trim_space(char *name) +{ + if (name == NULL) return; + trims_space(name); + trime_space(name); +} +/* + 文字列の末尾からCR LFを除く +*/ +static void trim_crlf(char *name) +{ + int i; + int len; + char *src, *dst; + + if (name == NULL) return; + len = strlen(name); + src = name; + dst = name; + for(i = 0; i < len; i++) { + if (*src != 0x0d && *src != 0x0a) { + *dst = *src; + dst++; + } + src++; + } + *dst = 0; +} + +/* + 読み込み + +return + -1=ERR + 0=OK +*/ +int conf_read(void) +{ + FILE *fp; + char buf[256]; + char buf2[256]; + + fp = fopen(CONF_FILE, "rt"); + if (fp == NULL) { + syslog(LOG_ERR, "conf_read(): conf file not found. %s", CONF_FILE); + return -1; + } + + syslog(LOG_INFO, "conf_read():"); + while(fgets(buf, sizeof(buf), fp)) { + // CR LF除く + trim_crlf(buf); + trim_space(buf); + PDEBUG(buf); + PDEBUG("\n"); + // Station ID + if (sscanf(buf, "sid = %s", buf2) == 1) { + sid_set(buf2); + syslog(LOG_INFO, "sid=%s", sid_getp()); + } + } + fclose(fp); + return 0; +} + + diff --git a/conf.h b/conf.h new file mode 100644 index 0000000..ed313ef --- /dev/null +++ b/conf.h @@ -0,0 +1,13 @@ +#if !defined(__CONF_H__) +#define __CONF_H__ + +// 設定ファイル +#define CONF_FILE "/etc/meas.conf" + +void sid_set(char *s); +void sid_get(char *s); +char *sid_getp(void); +int conf_read(void); + + +#endif diff --git a/makefile.host b/makefile.host new file mode 100755 index 0000000..0f9a208 --- /dev/null +++ b/makefile.host @@ -0,0 +1,35 @@ +CC = gcc + +# -DDUMMY ダミーデータで動作する +all: CFLAGS = -DDUMMY +cunit: CFLAGS = -DCUNIT + +LDFLAGS = +LIBS = -lm -lpthread +TEST_LIBS = $(LIBS) -lcunit + +SRCS0 = thread_rcv.c spi.c ring.c ad_ring.c rec.c sts.c ad_file.c +SRCS = scilog.c $(SRCS0) +TEST_SRCS = test.c $(SRCS0) + +HDRS = mes_print.h debug_print.h my_thread.h spi.h ring.h ad_ring.h rec.h sts.h ad_file.h + +OBJS = $(SRCS:.c=.o) +TEST_OBJS = $(TEST_SRCS:.c=.o) + +PROGRAM = scilog +TEST_PROG = test + +all: $(PROGRAM) + +cunit: $(TEST_PROG) + +$(PROGRAM): $(OBJS) $(HDRS) + $(CC) $(OBJS) $(LDFLAGS) $(LIBS) -o $(PROGRAM) + +$(TEST_PROG): $(TEST_OBJS) $(HDRS) + $(CC) $(TEST_OBJS) $(LDFLAGS) $(TEST_LIBS) -o $(TEST_PROG) + +clean:; rm -f *.o *~ $(PROGRAM) $(TEST_PROG) + +### End of Makefile diff --git a/my_thread.h b/my_thread.h index de034b9..10976f1 100644 --- a/my_thread.h +++ b/my_thread.h @@ -3,4 +3,9 @@ void* thread_rcv(void* pParam); +#ifdef CUNIT + #include + void thread_rcv_test(CU_pSuite test_suite); +#endif + #endif diff --git a/ring.c b/ring.c new file mode 100644 index 0000000..c13422e --- /dev/null +++ b/ring.c @@ -0,0 +1,142 @@ +/* + Ring Buffer用基本機能 + write位置 + read位置 +*/ +#include +#include +#include + +#include "ring.h" + +#define READ_NUM_MAX 256 +// ミューテックス +static pthread_mutex_t mutex_ring = PTHREAD_MUTEX_INITIALIZER; + +/* + int read_num + 読み込みポインタの個数 +*/ +RING_T* ring_create(int read_num) +{ + RING_T *t; + if (read_num <= 0) return NULL; + if (read_num > READ_NUM_MAX) return NULL; + + t = malloc(sizeof(RING_T)); + if (t != NULL) { + // readポインタ確保 + t->read = malloc(sizeof(int) * read_num); + if (t->read == NULL) { + free(t); + return NULL; + } + t->read_num = read_num; + } + return t; +} +void ring_destroy(RING_T *t) +{ + free(t->read); + free(t); +} +/* + int num: バッファの個数 +*/ +void ring_init(RING_T *t, int num) +{ + int i; + +pthread_mutex_lock(&mutex_ring); + t->write = 0; + for(i = 0; i < t->read_num; i++) { + t->read[i] = 0; + } + t->latest = -1; + t->num = num; +pthread_mutex_unlock(&mutex_ring); +} +void ring_clear(RING_T *t, int no) +{ + ring_read_set(t, no, ring_write_get(t)); +} +/* + 読み出し位置 + int no + readポインタ番号 +*/ +int ring_read_get(RING_T *t, int no) +{ + int i; +pthread_mutex_lock(&mutex_ring); + i = t->read[no]; +pthread_mutex_unlock(&mutex_ring); + return i; +} +void ring_read_set(RING_T *t, int no, int i) +{ +pthread_mutex_lock(&mutex_ring); + t->read[no] = i; +pthread_mutex_unlock(&mutex_ring); +} +void ring_read_plus(RING_T *t, int no) +{ +pthread_mutex_lock(&mutex_ring); + t->read[no]++; + if (t->read[no] >= t->num) t->read[no] = 0; +pthread_mutex_unlock(&mutex_ring); +} + +// 書き込み位置 +int ring_write_get(RING_T *t) +{ + int i; +pthread_mutex_lock(&mutex_ring); + i = t->write; +pthread_mutex_unlock(&mutex_ring); + return i; +} +void ring_write_plus(RING_T *t) +{ +pthread_mutex_lock(&mutex_ring); + t->write++; + if (t->write >= t->num) t->write = 0; +pthread_mutex_unlock(&mutex_ring); +} +// 読み込んでいないデータ数 +int ring_num_get(RING_T *t, int no) +{ + int i; + +pthread_mutex_lock(&mutex_ring); + i = t->write - t->read[no]; + if (i < 0) i += t->num; +pthread_mutex_unlock(&mutex_ring); + return i; +} +// 最新データ位置 +int ring_latest_get(RING_T *t) +{ + int i; +pthread_mutex_lock(&mutex_ring); + i = t->latest; +pthread_mutex_unlock(&mutex_ring); + return i; +} +void ring_latest_set(RING_T *t, int i) +{ +pthread_mutex_lock(&mutex_ring); + t->latest = i; +pthread_mutex_unlock(&mutex_ring); +} + +/* + パケットバッファフル? + 1=Full + 0=not Full +*/ +int ring_is_full(RING_T *t, int no) +{ + if (ring_num_get(t, no) >= t->num - 1) return 1; + return 0; +} diff --git a/ring.h b/ring.h new file mode 100644 index 0000000..21337eb --- /dev/null +++ b/ring.h @@ -0,0 +1,33 @@ + + +#if !defined(__RING_H__) +#define __RING_H__ + +typedef struct { + // 読み出し位置 複数設定できる + int *read; + // 書き込み位置 + int write; + // 最新データ位置 + int latest; + // バッファ個数 + int num; + // readポインタ個数 + int read_num; +} RING_T; + +RING_T* ring_create(int read_num); +void ring_destroy(RING_T *t); +void ring_init(RING_T *t, int num); +void ring_clear(RING_T *t, int no); +int ring_read_get(RING_T *t, int no); +void ring_read_set(RING_T *t, int no, int i); +void ring_read_plus(RING_T *t, int no); +int ring_write_get(RING_T *t); +void ring_write_plus(RING_T *t); +int ring_num_get(RING_T *t, int no); +int ring_latest_get(RING_T *t); +void ring_latest_set(RING_T *t, int i); +int ring_is_full(RING_T *t, int no); + +#endif diff --git a/scilog b/scilog index 4d6aa72..f26b489 100755 Binary files a/scilog and b/scilog differ diff --git a/scilog.c b/scilog.c index 756cd89..74e5bb6 100644 --- a/scilog.c +++ b/scilog.c @@ -22,6 +22,11 @@ #include "my_thread.h" #include "spi.h" +#include "ad_ring.h" +#include "sts.h" +#include "thread_rec.h" +#include "conf.h" +#include "ad_file.h" // debug_print.h内で#define DEBUG_PRINTしているので // リリース時は、debug_print.hでコメントする @@ -39,7 +44,7 @@ /**** 設定デフォルト値 */ -//#define SID_DEF "DEF" +#define SID_DEF "DEF" // @@ -83,11 +88,18 @@ int main (int argc, char *argv[]) // lcd_print("sciLogger"); // lcd_pos(0, 3); + ad_ring_init(); + sts_init(); + +//printf("%d\n", sizeof(UbloxNavTimeUtcRecType)); +//printf("%d\n", sizeof(HighSampleRecType)); +//goto END; + // デフォルト設定 -// sid_set(SID_DEF); + sid_set(SID_DEF); // 設定ファイル読み込み -// conf_read(); -// PDEBUG("sid=%s\n", sid_getp()); + conf_read(); + PDEBUG("sid=%s\n", sid_getp()); // lcd_print("*"); PDEBUG("sciLogger %s START\n", VERSION); @@ -116,9 +128,7 @@ int main (int argc, char *argv[]) // /**** メインループ 記録 ************************ */ - while(1) { - sleep(1); - } // メインループ終わり + thread_rec(NULL); END: sig_handler(0); diff --git a/spi.h b/spi.h index be41d62..2105079 100644 --- a/spi.h +++ b/spi.h @@ -18,36 +18,17 @@ // SPIで実際に受信したデータ長を取得 #define CMD_RECEIVED_LEN_GET 20 -// SPIで受信するデータ長 固定長 +// SPIで受信するデータ長 固定長 $含む #define SPI_DATA_LEN 945 // SPI受信データの先頭文字 #define SPI_HEAD_CHAR '$' -// NAV-TIMEUTCパケットのデータ格納 -typedef struct { - unsigned long tow; // 0 ms GPS Millisecond Time of Week - unsigned long tacc; // 4 ns Time Accuracy Estimate - long nano; // 8 ns Nanoseconds of second, range -1e9 .. 1e9 (UTC) - unsigned int year; // 12 UTC - unsigned char month; // 16 - unsigned char day; - unsigned char hour; - unsigned char min; - unsigned char sec; - unsigned char valid; // Validity Flags - // bit0 validTOW 1 = Valid Time of Week - // bit1 validWKN 1 = Valid Week Number - // bit2 validUTC 1 = Valid UTC (Leap Seconds already known) -} UbloxNavTimeUtc; +// SPI受信データの先頭からのオフセット +#define SPI_OFS_GPS (18-17) +#define SPI_OFS_1SEC (38-17) +#define SPI_OFS_DATA (62-17) -#define AD_CHNUM 6 -#define AD_SAMPLE 50 // AD ICの出力レート [Hz] -typedef struct { - UbloxNavTimeUtc t; // タイムスタンプ - long data1sec[AD_CHNUM]; // 1sec平均値 - long data[AD_CHNUM][AD_SAMPLE]; // AD_SAMPLE[Hz]のデータ -} ADbufType; int spi_get_fd(void); diff --git a/sts.c b/sts.c new file mode 100644 index 0000000..4d2caaa --- /dev/null +++ b/sts.c @@ -0,0 +1,40 @@ +#include "sts.h" + +/* + ステータス +*/ +static int sts; + +void sts_init(void) +{ + sts = STS_TIME_FIX; +} +/* + ステータス返す +*/ +int sts_get(void) +{ + return sts; +} + +// LCD表示用 +char *sts_get_str(void) +{ + switch(sts) { + case STS_TIME_FIX: + return "WAIT TIME FIX"; + case STS_REC_INIT: + return "REC INIT "; + case STS_REC: + return "RECORDING "; + case STS_REC_OFF: + return "RECORD OFF "; + case STS_FILE_ERR: + return "WRITE ERROR "; + } + return "UNKNOWN "; +} +void sts_set(int sts0) +{ + sts = sts0; +} diff --git a/sts.h b/sts.h new file mode 100644 index 0000000..830d45a --- /dev/null +++ b/sts.h @@ -0,0 +1,17 @@ +#if !defined(__STS_H__) +#define __STS_H__ + +#define STS_TIME_FIX 1 +#define STS_REC_INIT 3 +#define STS_REC 4 +#define STS_REC_OFF 5 +#define STS_FILE_ERR 6 + + +void sts_init(void); +int sts_get(void); +char *sts_get_str(void); +void sts_set(int sts0); + +#endif + diff --git a/test b/test new file mode 100755 index 0000000..49390a4 Binary files /dev/null and b/test differ diff --git a/test.c b/test.c new file mode 100644 index 0000000..31418a4 --- /dev/null +++ b/test.c @@ -0,0 +1,23 @@ + +#include +#include +#include + +#include "my_thread.h" +#include "spi.h" +#include "ad_ring.h" + +int main (int argc, char *argv[]) +{ + CU_pSuite test_suite; + CU_initialize_registry(); + test_suite = CU_add_suite("scilog", NULL, NULL); + + thread_rcv_test(test_suite); + ad_ring_test(test_suite); + + CU_console_run_tests(); + + CU_cleanup_registry(); + +} diff --git a/thread_rcv.c b/thread_rcv.c index daeff85..75e2403 100644 --- a/thread_rcv.c +++ b/thread_rcv.c @@ -5,18 +5,38 @@ #include #include #include +#include #include "spi.h" +#include "ad_ring.h" - -static int decode(char *buf, ADbufType *ad) +/* + Little Endian + 3byte -> signed long(4byte) +*/ +static long b3_to_long32(unsigned char *ptr) +{ + char buf[4]; + + buf[0] = *ptr++; + buf[1] = *ptr++; + buf[2] = *ptr; + if (*ptr & 0x80) { + buf[3] = 0xFF; + } else { + buf[3] = 0; + } + return *((long*)buf); +} +static int decode(char *buf, AdData *ad) { UbloxNavTimeUtc *gps; u_int8_t *ptr; int ch; + int i; ptr = (u_int8_t*)buf; - gps = &(ad->t); + gps = &(ad->gps); if (*ptr++ != SPI_HEAD_CHAR) return -1; // GPS @@ -36,18 +56,42 @@ static int decode(char *buf, ADbufType *ad) ptr += 4; } // 50Hz data + for(ch = 0; ch < AD_CHNUM; ch++) { + for(i = 0; i < AD_SAMPLE; i++) { + ad->data[ch][i] = b3_to_long32(ptr); + ptr += 3; + } + } + // Checksum + // gps->struct tm + ad->t.tm_year = gps->year - 1900; + ad->t.tm_mon = gps->month - 1; // struct tmの月は(0〜11)なので注意 + ad->t.tm_mday = gps->day; + ad->t.tm_hour = gps->hour; + ad->t.tm_min = gps->min; + ad->t.tm_sec = gps->sec; + return 0; } +/* + int freq + 平均化後の周波数 Hz +*/ +static void do_avg(int freq, AdData *ad) +{ +// 仮コード + memcpy(ad->avg, ad->data, AD_CHNUM*AD_SAMPLE*AD_BYTES); +} void* thread_rcv(void* pParam) { while(1) { fd_set fds; char buf[SPI_DATA_LEN+256]; int i; - int ch; +// int ch; int fd_spi; -ADbufType ad; + AdData ad, *ad_ptr; fd_spi = spi_get_fd(); // select の準備 @@ -67,7 +111,11 @@ ADbufType ad; // ring_zero(); // データ取得 spi_rx_get(buf); + // デコード decode(buf, &ad); + // 平均 仮コード 要変更 + do_avg(AD_SAMPLE, &ad); +#if 0 printf("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X", ad.t.year, ad.t.month, ad.t.day, ad.t.hour, ad.t.min, ad.t.sec, ad.t.nano, ad.t.tow, ad.t.tacc, ad.t.valid); @@ -75,6 +123,22 @@ for(ch = 0; ch < AD_CHNUM; ch++) { printf(",%+7ld", ad.data1sec[ch]); } printf("\r\n"); +for(ch = 0; ch < AD_CHNUM; ch++) { + printf(",%+7ld", ad.data[ch][0]); +} +printf("\r\n"); +for(ch = 0; ch < AD_CHNUM; ch++) { + printf(",%+7ld", ad.data[ch][49]); +} +printf("\r\n"); +#endif + // ADリングバッファに書き込み + ad_ptr = ad_ring_get(ad_ring_write_get()); + *ad_ptr = ad; + // 最新データ位置 + ad_ring_latest_set(ad_ring_write_get()); + // 書き込み位置+1 + ad_ring_write_plus(); } // while((i = sub_dnum_get()) > 0) { } // if(FD_ISSET(fd_sub, &fds)) { @@ -82,3 +146,60 @@ printf("\r\n"); return NULL; } +// +/**** CUnit test +*/ +#ifdef CUNIT +#include + +static void test_b3_to_long32(void) +{ + long l; + char buf[SPI_DATA_LEN+256]; + AdData ad; + int i; + + buf[0] = 0x56; + buf[1] = 0x34; + buf[2] = 0x12; + l = b3_to_long32(buf); + CU_ASSERT(l == 0x00123456); + + buf[0] = 0x56; + buf[1] = 0x34; + buf[2] = 0x82; + l = b3_to_long32(buf); + CU_ASSERT(l == (long)0xFF823456); + + buf[0] = 0xFF; + buf[1] = 0xFF; + buf[2] = 0xFF; + l = b3_to_long32(buf); + CU_ASSERT(l == -1); + + buf[0] = '$'; + i = SPI_OFS_DATA; + buf[i++] = 0x56; + buf[i++] = 0x34; + buf[i++] = 0x12; + decode(buf, &ad); + CU_ASSERT(ad.data[0][0] == 0x123456); + + buf[0] = '$'; + i = SPI_OFS_DATA + AD_CHNUM * AD_SAMPLE * 3; + i -= 3; + buf[i++] = 0x56; + buf[i++] = 0x34; + buf[i++] = 0x12; + decode(buf, &ad); + CU_ASSERT(ad.data[AD_CHNUM-1][AD_SAMPLE-1] == 0x123456); +} + +void thread_rcv_test(CU_pSuite test_suite) +{ + + CU_add_test(test_suite, "test_b3_to_long32", test_b3_to_long32); + +} + +#endif diff --git a/thread_rec.c b/thread_rec.c new file mode 100644 index 0000000..fd431b3 --- /dev/null +++ b/thread_rec.c @@ -0,0 +1,251 @@ + +#include +#include +#include +#include + +#include "ad_ring.h" +#include "sts.h" +#include "debug_print.h" +#include "ad_file.h" + +// 1secファイル書き込み間隔 sec +#define SEC_FLUSH_NUM 1 +//#define SEC_FLUSH_NUM 60 + +// 高速サンプルファイル書き込み間隔 sec +#define HIGH_FLUSH_NUM 1 + +// 日付変化検出用 +static struct tm high_tm; +static struct tm sec_tm; + +// +/**** 時刻校正待ち + パケット中の時刻が有効になるのを待つ + + Return: + 次の状態 +*/ +int ProcTimeFix(void) +{ + AdData *d; + static int latest = -1; + /* + 時刻校正終わったら、記録開始 + */ + if (latest == ad_ring_latest_get()) return STS_TIME_FIX; + latest = ad_ring_latest_get(); + if (latest >= 0) { + // 最新データの時刻取得 + d = ad_ring_get(latest); + // GPS Validチェック + if (d->gps.valid & 0x07) { + PDEBUG("ProcTimeFix: rec init.\n"); + return STS_REC_INIT; + } + } + return STS_TIME_FIX; +} + +// +/**** 記録初期化 + + Return: + 次の状態 +*/ +int ProcRecInit(void) +{ + AdData *d; + + if (ad_ring_latest_get() >= 0) { + // 記録開始 + ad_ring_clear_sec(); + ad_ring_clear_high(); + + // 最新データの時刻取得 + d = ad_ring_get(ad_ring_latest_get()); + // 日付変化検出用変数初期化 + high_tm = d->t; + sec_tm = d->t; + + PDEBUG("ProcRecInit: rec start.\n"); + return STS_REC; + } + return STS_REC_INIT; +} + +/**** 記録中 + 定期的にカードに書き込み + + Return: + 次の状態 +*/ +int ProcRec(void) +{ + AdData *d; + FILE *fp = NULL; + char buf[HIGH_WRITE_LEN+128]; + +// +/**** 1secデータ記録 +*/ + /* + パケットバッファにデータたまった? + */ + if (ad_ring_num_get_sec() >= SEC_FLUSH_NUM) { + // データ取り出し + d = ad_ring_get(ad_ring_read_get_sec()); + // ファイルオープン + sec_file_open(&fp, d); + + // 書き込みループ + while(ad_ring_num_get_sec() > 0) { + // データ取り出し + d = ad_ring_get(ad_ring_read_get_sec()); +PDEBUG("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X", + d->gps.year, d->gps.month, d->gps.day, d->gps.hour, d->gps.min, d->gps.sec, d->gps.nano, + d->gps.tow, d->gps.tacc, d->gps.valid); +int ch; +for(ch = 0; ch < AD_CHNUM; ch++) { + PDEBUG(",%+7ld", d->data1sec[ch]); +} +PDEBUG("\r\n"); + + // 日にち変わった ファイル切換する +#ifdef DEBUG_FILE_MIN + // デバッグ用 ?分ごとファイル切り替え + if ((d->t.tm_min % DEBUG_FILE_MIN_PERIOD) == 0 && (d->t.tm_sec == 0) && (d->t.tm_min != sec_tm.tm_min)) { +#else + if (d->t.tm_year != sec_tm.tm_year || d->t.tm_mon != sec_tm.tm_mon || d->t.tm_mday != sec_tm.tm_mday) { +#endif + // 日付変わったのでファイル切り替える + // 前のファイル閉じる + if (fp != NULL) sec_file_close(fp); + // ファイルオープン + if (sec_file_open(&fp, d)) { + PDEBUG("REC: FG file change ERROR\n"); + syslog(LOG_INFO, "REC: FG file change ERROR\n"); + } else { + PDEBUG("REC: FG file change OK\n"); + syslog(LOG_INFO, "REC: FG file change OK\n"); + } + } + // ファイルに書き込むデータ作成 + sec_make_rec_data(d, buf); + // ファイルに1データ書き込み Open/Closeなし + if (sec_file_out(fp, buf, strlen(buf))) { + /* 書き込みエラー */ + // 読み出し位置クリア + ad_ring_clear_sec(); + // ファイル閉じる + sec_file_close(fp); + // + PDEBUG("ProcRec: 1sec file write err\n"); + syslog(LOG_INFO, "REC: 1sec file write error\n"); + return STS_FILE_ERR; + } + // 読み込み位置進める + ad_ring_read_plus_sec(); + // タイムスタンプ記録 + sec_tm = d->t; + } + // ファイル閉じる + sec_file_close(fp); + // + PDEBUG("ProcRec: 1sec file write.\n"); + } +// +/**** 高速サンプルデータ記録 +*/ + /* + パケットバッファにデータたまった? + */ + if (ad_ring_num_get_high() >= HIGH_FLUSH_NUM) { + // データ取り出し + d = ad_ring_get(ad_ring_read_get_high()); + // ファイルオープン + high_file_open(&fp, d); + + // 書き込みループ + while(ad_ring_num_get_high() > 0) { + // データ取り出し + d = ad_ring_get(ad_ring_read_get_high()); + // 時が変わった ファイル切換する +#ifdef DEBUG_FILE_MIN + // デバッグ用 ?分ごとファイル切り替え + if ((d->t.tm_min % DEBUG_FILE_MIN_PERIOD) == 0 && (d->t.tm_high == 0) && (d->t.tm_min != hightm.tm_min)) { +#else + if (d->t.tm_year != high_tm.tm_year || d->t.tm_mon != high_tm.tm_mon || d->t.tm_mday != high_tm.tm_mday + || d->t.tm_hour != high_tm.tm_hour) { +#endif + // 時が変わったのでファイル切り替える + // 前のファイル閉じる + if (fp != NULL) high_file_close(fp); + // ファイルオープン + if (high_file_open(&fp, d)) { + PDEBUG("REC: FG file change ERROR\n"); + syslog(LOG_INFO, "REC: HIGH file change ERROR\n"); + } else { + PDEBUG("REC: FG file change OK\n"); + syslog(LOG_INFO, "REC: HIGH file change OK\n"); + } + } + // ファイルに書き込むデータ作成 + high_make_rec_data(d, buf); + // ファイルに1データ書き込み Open/Closeなし + if (high_file_out(fp, buf, HIGH_WRITE_LEN)) { + /* 書き込みエラー */ + // 読み出し位置クリア + ad_ring_clear_high(); + // ファイル閉じる + high_file_close(fp); + // + PDEBUG("ProcRec: HIGH file write err\n"); + syslog(LOG_INFO, "REC: HIGH file write error\n"); + return STS_FILE_ERR; + } + // 読み込み位置進める + ad_ring_read_plus_high(); + // タイムスタンプ記録 + high_tm = d->t; + } + // ファイル閉じる + high_file_close(fp); + // + PDEBUG("ProcRec: HIGH file write.\n"); + } + + return STS_REC; +} + +void* thread_rec(void* pParam) +{ + while(1) { + usleep(10*1000); + switch (sts_get()) { + /* 時刻校正中 */ + case STS_TIME_FIX: + sts_set(ProcTimeFix()); + break; + /* 記録初期化 */ + case STS_REC_INIT: + sts_set(ProcRecInit()); + break; + /* 記録中 */ + case STS_REC: + sts_set(ProcRec()); + break; + /* 記録OFF中 */ + case STS_REC_OFF: +// sts_set(ProcRecOff()); + break; + /* ファイルエラー発生中 */ + case STS_FILE_ERR: +// sts_set(ProcErr()); + break; + default: + break; + } + } // メインループ終わり +} diff --git a/thread_rec.h b/thread_rec.h new file mode 100644 index 0000000..dec9fe6 --- /dev/null +++ b/thread_rec.h @@ -0,0 +1,11 @@ +#if !defined(__REC_H__) +#define __REC_H__ + + + +int ProcTimeFix(void); +int ProcRecInit(void); +int ProcRec(void); +void* thread_rec(void* pParam); + +#endif