Overoで動作するデータ収録プログラム
サブCPUからGPS時刻付きADデータを取得してSDに記録する。
Revision | 61ae9916f528940690a8ddeb35503931e566698e (tree) |
---|---|
Time | 2011-12-20 10:35:52 |
Author | Naoya Takamura <ntaka206@user...> |
Commiter | Naoya Takamura |
thread_disp() usleep(100msec)追加
@@ -1,101 +1,101 @@ | ||
1 | -#include <stdio.h> | |
2 | -#include <unistd.h> | |
3 | -#include <sys/ioctl.h> | |
4 | -//#include <termios.h> | |
5 | -#include <fcntl.h> | |
6 | -#include <string.h> | |
7 | - | |
8 | -#include "spi.h" | |
9 | -#include "ad_ring.h" | |
10 | - | |
11 | -static int fd_spi = -1; | |
12 | - | |
13 | -int spi_get_fd(void) | |
14 | -{ | |
15 | - return fd_spi; | |
16 | -} | |
17 | -int spi_open(char *dev) | |
18 | -{ | |
19 | - fd_spi = open(dev, O_RDWR); | |
20 | - return fd_spi; | |
21 | -} | |
22 | -/* | |
23 | - デバイスへのioctl | |
24 | -*/ | |
25 | -int spi_ioctl(int cmd, void *p) | |
26 | -{ | |
27 | - return ioctl(fd_spi, cmd, p); | |
28 | -} | |
29 | - | |
30 | -int spi_close(void) | |
31 | -{ | |
32 | - return close(fd_spi); | |
33 | -} | |
34 | - | |
35 | -// 送信データ長セット | |
36 | -void spi_tx_len_set(int len) | |
37 | -{ | |
38 | - spi_ioctl(CMD_TX_LEN, &len); | |
39 | -} | |
40 | -// 同期シリアル受信データ取得 | |
41 | -void spi_rx_get(char *p) | |
42 | -{ | |
43 | - spi_ioctl(CMD_RX_GET, p); | |
44 | -} | |
45 | - | |
46 | -// 同期シリアル送信データセット | |
47 | -void spi_tx_set(char *p) | |
48 | -{ | |
49 | - spi_ioctl(CMD_TX_SET, p); | |
50 | -} | |
51 | -// リングバッファクリア | |
52 | -void spi_buf_clear(void) | |
53 | -{ | |
54 | - spi_ioctl(CMD_BUF_CLEAR, NULL); | |
55 | -} | |
56 | - | |
57 | -// リングバッファのデータ数get | |
58 | -int spi_dnum_get(void) | |
59 | -{ | |
60 | - int i = 0; | |
61 | - spi_ioctl(CMD_DNUM_GET, &i); | |
62 | - return i; | |
63 | -} | |
64 | -// SPIで実際に受信したデータ長を取得 | |
65 | -int spi_rcvd_len_get(void) | |
66 | -{ | |
67 | - int i = 0; | |
68 | - spi_ioctl(CMD_RECEIVED_LEN_GET, &i); | |
69 | - return i; | |
70 | -} | |
71 | -// コマンド送信 | |
72 | -void spi_cmd_send(char cmd_code, char *cmd_data, int data_len) | |
73 | -{ | |
74 | - char cmd[SPI_CMD_LEN]; | |
75 | - | |
76 | - memset(cmd, 0, SPI_CMD_LEN); | |
77 | - | |
78 | - cmd[SPI_CMD_OFS_HEAD] = SPI_HEAD_CHAR; | |
79 | - cmd[SPI_CMD_OFS_CMDCODE] = cmd_code; | |
80 | - | |
81 | - // data | |
82 | - if (data_len > (SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA)) { | |
83 | - data_len = SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA; | |
84 | - } | |
85 | - memcpy(&cmd[SPI_CMD_OFS_CMDDATA], cmd_data, data_len); | |
86 | - | |
87 | - spi_tx_set(cmd); | |
88 | -} | |
89 | -// Gainコマンド送信 | |
90 | -void spi_cmd_send_gain(int gain) | |
91 | -{ | |
92 | - char cmd[SPI_CMD_LEN]; | |
93 | - int i; | |
94 | - | |
95 | - memset(cmd, 0, SPI_CMD_LEN); | |
96 | - | |
97 | - for(i = 0; i < AD_CHNUM; i++) | |
98 | - cmd[i] = gain; | |
99 | - spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM); | |
100 | -} | |
101 | - | |
1 | +#include <stdio.h> | |
2 | +#include <unistd.h> | |
3 | +#include <sys/ioctl.h> | |
4 | +//#include <termios.h> | |
5 | +#include <fcntl.h> | |
6 | +#include <string.h> | |
7 | + | |
8 | +#include "spi.h" | |
9 | +#include "ad_ring.h" | |
10 | + | |
11 | +static int fd_spi = -1; | |
12 | + | |
13 | +int spi_get_fd(void) | |
14 | +{ | |
15 | + return fd_spi; | |
16 | +} | |
17 | +int spi_open(char *dev) | |
18 | +{ | |
19 | + fd_spi = open(dev, O_RDWR); | |
20 | + return fd_spi; | |
21 | +} | |
22 | +/* | |
23 | + デバイスへのioctl | |
24 | +*/ | |
25 | +int spi_ioctl(int cmd, void *p) | |
26 | +{ | |
27 | + return ioctl(fd_spi, cmd, p); | |
28 | +} | |
29 | + | |
30 | +int spi_close(void) | |
31 | +{ | |
32 | + return close(fd_spi); | |
33 | +} | |
34 | + | |
35 | +// 送信データ長セット | |
36 | +void spi_tx_len_set(int len) | |
37 | +{ | |
38 | + spi_ioctl(CMD_TX_LEN, &len); | |
39 | +} | |
40 | +// 同期シリアル受信データ取得 | |
41 | +void spi_rx_get(char *p) | |
42 | +{ | |
43 | + spi_ioctl(CMD_RX_GET, p); | |
44 | +} | |
45 | + | |
46 | +// 同期シリアル送信データセット | |
47 | +void spi_tx_set(char *p) | |
48 | +{ | |
49 | + spi_ioctl(CMD_TX_SET, p); | |
50 | +} | |
51 | +// リングバッファクリア | |
52 | +void spi_buf_clear(void) | |
53 | +{ | |
54 | + spi_ioctl(CMD_BUF_CLEAR, NULL); | |
55 | +} | |
56 | + | |
57 | +// リングバッファのデータ数get | |
58 | +int spi_dnum_get(void) | |
59 | +{ | |
60 | + int i = 0; | |
61 | + spi_ioctl(CMD_DNUM_GET, &i); | |
62 | + return i; | |
63 | +} | |
64 | +// SPIで実際に受信したデータ長を取得 | |
65 | +int spi_rcvd_len_get(void) | |
66 | +{ | |
67 | + int i = 0; | |
68 | + spi_ioctl(CMD_RECEIVED_LEN_GET, &i); | |
69 | + return i; | |
70 | +} | |
71 | +// コマンド送信 | |
72 | +void spi_cmd_send(char cmd_code, char *cmd_data, int data_len) | |
73 | +{ | |
74 | + char cmd[SPI_CMD_LEN]; | |
75 | + | |
76 | + memset(cmd, 0, SPI_CMD_LEN); | |
77 | + | |
78 | + cmd[SPI_CMD_OFS_HEAD] = SPI_HEAD_CHAR; | |
79 | + cmd[SPI_CMD_OFS_CMDCODE] = cmd_code; | |
80 | + | |
81 | + // data | |
82 | + if (data_len > (SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA)) { | |
83 | + data_len = SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA; | |
84 | + } | |
85 | + memcpy(&cmd[SPI_CMD_OFS_CMDDATA], cmd_data, data_len); | |
86 | + | |
87 | + spi_tx_set(cmd); | |
88 | +} | |
89 | +// Gainコマンド送信 | |
90 | +void spi_cmd_send_gain(int gain) | |
91 | +{ | |
92 | + char cmd[SPI_CMD_LEN]; | |
93 | + int i; | |
94 | + | |
95 | + memset(cmd, 0, SPI_CMD_LEN); | |
96 | + | |
97 | + for(i = 0; i < AD_CHNUM; i++) | |
98 | + cmd[i] = gain; | |
99 | + spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM); | |
100 | +} | |
101 | + |
@@ -1,77 +1,77 @@ | ||
1 | -#if !defined(__SPI_H__) | |
2 | -#define __SPI_H__ | |
3 | - | |
4 | -// ioctlコマンド | |
5 | -// SPI受信データ取得 | |
6 | -#define CMD_RX_GET 11 | |
7 | -// SPI送信データ長セット | |
8 | -#define CMD_TX_LEN 12 | |
9 | -// SPI送信データセット PICへのコマンド | |
10 | -#define CMD_TX_SET 13 | |
11 | -// リングバッファにあるデータ数を返す | |
12 | -#define CMD_DNUM_GET 14 | |
13 | -// リングバッファクリア | |
14 | -#define CMD_BUF_CLEAR 15 | |
15 | -// SPIで実際に受信したデータ長を取得 | |
16 | -#define CMD_RECEIVED_LEN_GET 20 | |
17 | - | |
18 | -// SPIで受信するデータ長 固定長 $含む | |
19 | -#define SPI_DATA_LEN (965) | |
20 | -// SPI受信データの先頭文字 送信コマンドでも使用 | |
21 | -#define SPI_HEAD_CHAR '$' | |
22 | - | |
23 | -// SPI受信データ 応答データ 長さ | |
24 | -#define SPI_RESDATA_LEN 17 | |
25 | - | |
26 | -// SPI受信データ Checksum 長さ | |
27 | -#define SPI_CHECKSUM_LEN 2 | |
28 | - | |
29 | -// SPI受信データの先頭からのオフセット | |
30 | -#define SPI_OFS_RESCODE 1 | |
31 | -#define SPI_OFS_RESDATA 2 | |
32 | -#define SPI_OFS_GPS (19) | |
33 | -#define SPI_OFS_1SEC (39) | |
34 | -#define SPI_OFS_DATA (63) | |
35 | -#define SPI_OFS_SUM 963 | |
36 | - | |
37 | -// SPI送信コマンド長 固定長 $含む | |
38 | -#define SPI_CMD_LEN 20 | |
39 | - | |
40 | -// SPI送信コマンド フィールドのオフセット位置 | |
41 | -#define SPI_CMD_OFS_HEAD 0 | |
42 | -#define SPI_CMD_OFS_CMDCODE 1 | |
43 | -#define SPI_CMD_OFS_CMDDATA 2 | |
44 | - | |
45 | -// SPIコマンドコード | |
46 | -#define SPI_CMDCODE_GAIN 1 | |
47 | - | |
48 | -// SPIコマンドGainの設定値 | |
49 | -#define SPI_CMD_GAIN_1P8 0 // 1/8 | |
50 | -#define SPI_CMD_GAIN_1P4 1 // 1/4 | |
51 | -#define SPI_CMD_GAIN_1P2 2 // 1/2 | |
52 | -#define SPI_CMD_GAIN_1 3 | |
53 | -#define SPI_CMD_GAIN_2 4 | |
54 | -#define SPI_CMD_GAIN_4 5 | |
55 | -#define SPI_CMD_GAIN_8 6 | |
56 | -#define SPI_CMD_GAIN_16 7 | |
57 | -#define SPI_CMD_GAIN_32 8 | |
58 | -#define SPI_CMD_GAIN_64 9 | |
59 | -#define SPI_CMD_GAIN_128 10 | |
60 | - | |
61 | - | |
62 | -int spi_get_fd(void); | |
63 | -int spi_open(char *dev); | |
64 | -int spi_ioctl(int cmd, void *p); | |
65 | -int spi_close(void); | |
66 | - | |
67 | -void spi_tx_len_set(int len); | |
68 | -void spi_rx_get(char *p); | |
69 | -void spi_tx_set(char *p); | |
70 | -void spi_buf_clear(void); | |
71 | -int spi_dnum_get(void); | |
72 | -int spi_rcvd_len_get(void); | |
73 | - | |
74 | -void spi_cmd_send(char cmd_code, char *cmd_data, int data_len); | |
75 | -void spi_cmd_send_gain(int gain); | |
76 | - | |
77 | -#endif | |
1 | +#if !defined(__SPI_H__) | |
2 | +#define __SPI_H__ | |
3 | + | |
4 | +// ioctlコマンド | |
5 | +// SPI受信データ取得 | |
6 | +#define CMD_RX_GET 11 | |
7 | +// SPI送信データ長セット | |
8 | +#define CMD_TX_LEN 12 | |
9 | +// SPI送信データセット PICへのコマンド | |
10 | +#define CMD_TX_SET 13 | |
11 | +// リングバッファにあるデータ数を返す | |
12 | +#define CMD_DNUM_GET 14 | |
13 | +// リングバッファクリア | |
14 | +#define CMD_BUF_CLEAR 15 | |
15 | +// SPIで実際に受信したデータ長を取得 | |
16 | +#define CMD_RECEIVED_LEN_GET 20 | |
17 | + | |
18 | +// SPIで受信するデータ長 固定長 $含む | |
19 | +#define SPI_DATA_LEN (965) | |
20 | +// SPI受信データの先頭文字 送信コマンドでも使用 | |
21 | +#define SPI_HEAD_CHAR '$' | |
22 | + | |
23 | +// SPI受信データ 応答データ 長さ | |
24 | +#define SPI_RESDATA_LEN 17 | |
25 | + | |
26 | +// SPI受信データ Checksum 長さ | |
27 | +#define SPI_CHECKSUM_LEN 2 | |
28 | + | |
29 | +// SPI受信データの先頭からのオフセット | |
30 | +#define SPI_OFS_RESCODE 1 | |
31 | +#define SPI_OFS_RESDATA 2 | |
32 | +#define SPI_OFS_GPS (19) | |
33 | +#define SPI_OFS_1SEC (39) | |
34 | +#define SPI_OFS_DATA (63) | |
35 | +#define SPI_OFS_SUM 963 | |
36 | + | |
37 | +// SPI送信コマンド長 固定長 $含む | |
38 | +#define SPI_CMD_LEN 20 | |
39 | + | |
40 | +// SPI送信コマンド フィールドのオフセット位置 | |
41 | +#define SPI_CMD_OFS_HEAD 0 | |
42 | +#define SPI_CMD_OFS_CMDCODE 1 | |
43 | +#define SPI_CMD_OFS_CMDDATA 2 | |
44 | + | |
45 | +// SPIコマンドコード | |
46 | +#define SPI_CMDCODE_GAIN 1 | |
47 | + | |
48 | +// SPIコマンドGainの設定値 | |
49 | +#define SPI_CMD_GAIN_1P8 0 // 1/8 | |
50 | +#define SPI_CMD_GAIN_1P4 1 // 1/4 | |
51 | +#define SPI_CMD_GAIN_1P2 2 // 1/2 | |
52 | +#define SPI_CMD_GAIN_1 3 | |
53 | +#define SPI_CMD_GAIN_2 4 | |
54 | +#define SPI_CMD_GAIN_4 5 | |
55 | +#define SPI_CMD_GAIN_8 6 | |
56 | +#define SPI_CMD_GAIN_16 7 | |
57 | +#define SPI_CMD_GAIN_32 8 | |
58 | +#define SPI_CMD_GAIN_64 9 | |
59 | +#define SPI_CMD_GAIN_128 10 | |
60 | + | |
61 | + | |
62 | +int spi_get_fd(void); | |
63 | +int spi_open(char *dev); | |
64 | +int spi_ioctl(int cmd, void *p); | |
65 | +int spi_close(void); | |
66 | + | |
67 | +void spi_tx_len_set(int len); | |
68 | +void spi_rx_get(char *p); | |
69 | +void spi_tx_set(char *p); | |
70 | +void spi_buf_clear(void); | |
71 | +int spi_dnum_get(void); | |
72 | +int spi_rcvd_len_get(void); | |
73 | + | |
74 | +void spi_cmd_send(char cmd_code, char *cmd_data, int data_len); | |
75 | +void spi_cmd_send_gain(int gain); | |
76 | + | |
77 | +#endif |
@@ -3,30 +3,6 @@ | ||
3 | 3 | #include "ad_ring.h" |
4 | 4 | #include "lcd.h" |
5 | 5 | |
6 | -static void scr_init(void) | |
7 | -{ | |
8 | - int iLeft = 0; | |
9 | - int iY = 0; | |
10 | - | |
11 | - lcd_clear(); | |
12 | - | |
13 | - lcd_pos(iLeft, iY); | |
14 | - lcd_print("TIME:"); | |
15 | - iY++; | |
16 | -// lcd_pos(iLeft, iY); | |
17 | -// lcd_print("STS:"); | |
18 | -// iY++; | |
19 | - lcd_pos(iLeft, iY); | |
20 | - lcd_print("HX:"); | |
21 | - iY++; | |
22 | - lcd_pos(iLeft, iY); | |
23 | - lcd_print("HY:"); | |
24 | - iY++; | |
25 | - lcd_pos(iLeft, iY); | |
26 | - lcd_print("HZ:"); | |
27 | - iY++; | |
28 | -} | |
29 | - | |
30 | 6 | /* |
31 | 7 | |
32 | 8 | */ |
@@ -90,5 +66,6 @@ void* thread_disp(void* pParam) | ||
90 | 66 | // AD表示 |
91 | 67 | scr_disp_ad(d); |
92 | 68 | } |
69 | + usleep(100*1000); | |
93 | 70 | } |
94 | 71 | } |
@@ -1,270 +1,270 @@ | ||
1 | -#include <stdio.h> | |
2 | -//#include <unistd.h> | |
3 | -//#include <sys/ioctl.h> | |
4 | -//#include <termios.h> | |
5 | -#include <fcntl.h> | |
6 | -#include <syslog.h> | |
7 | -#include <sys/types.h> | |
8 | -#include <string.h> | |
9 | - | |
10 | -#include "debug_print.h" | |
11 | -#include "spi.h" | |
12 | -#include "ad_ring.h" | |
13 | -#include "conf.h" | |
14 | - | |
15 | -/* | |
16 | - Little Endian | |
17 | - 3byte -> signed long(4byte) | |
18 | -*/ | |
19 | -static long b3_to_long32(unsigned char *ptr) | |
20 | -{ | |
21 | - char buf[4]; | |
22 | - | |
23 | - buf[0] = *ptr++; | |
24 | - buf[1] = *ptr++; | |
25 | - buf[2] = *ptr; | |
26 | - if (*ptr & 0x80) { | |
27 | - buf[3] = 0xFF; | |
28 | - } else { | |
29 | - buf[3] = 0; | |
30 | - } | |
31 | - return *((long*)buf); | |
32 | -} | |
33 | -static int decode(char *buf, AdData *ad) | |
34 | -{ | |
35 | - UbloxNavTimeUtc *gps; | |
36 | - u_int8_t *ptr; | |
37 | - int ch; | |
38 | - int i; | |
39 | - | |
40 | - ptr = (u_int8_t*)buf; | |
41 | - gps = &(ad->gps); | |
42 | - | |
43 | - if (*ptr++ != SPI_HEAD_CHAR) return -1; | |
44 | - // Response Code | |
45 | - ad->rescode = *ptr++; | |
46 | - // Response Data | |
47 | - memcpy(ad->resdata, ptr, SPI_RESDATA_LEN); | |
48 | - ptr += SPI_RESDATA_LEN; | |
49 | - // GPS | |
50 | - gps->tow = *((int32_t*)ptr); ptr += 4; | |
51 | - gps->tacc = *((int32_t*)ptr); ptr += 4; | |
52 | - gps->nano = *((int32_t*)ptr); ptr += 4; | |
53 | - gps->year = *((int16_t*)ptr); ptr += 2; | |
54 | - gps->month = *((u_int8_t*)ptr); ptr++; | |
55 | - gps->day = *((u_int8_t*)ptr); ptr++; | |
56 | - gps->hour = *((u_int8_t*)ptr); ptr++; | |
57 | - gps->min = *((u_int8_t*)ptr); ptr++; | |
58 | - gps->sec = *((u_int8_t*)ptr); ptr++; | |
59 | - gps->valid = *((u_int8_t*)ptr); ptr++; | |
60 | - // 1sec平均 | |
61 | - for(ch = 0; ch < AD_CHNUM; ch++) { | |
62 | - ad->data1sec[ch] = *((int32_t*)ptr); | |
63 | - ptr += 4; | |
64 | - } | |
65 | - // 50Hz data | |
66 | - for(ch = 0; ch < AD_CHNUM; ch++) { | |
67 | - for(i = 0; i < AD_SAMPLE; i++) { | |
68 | - ad->data[ch][i] = b3_to_long32(ptr); | |
69 | - ptr += 3; | |
70 | - } | |
71 | - } | |
72 | - // Checksum | |
73 | - ad->checksum = *((u_int16_t*)ptr); | |
74 | - | |
75 | - // gps->struct tm | |
76 | - ad->t.tm_year = gps->year - 1900; | |
77 | - ad->t.tm_mon = gps->month - 1; // struct tmの月は(0〜11)なので注意 | |
78 | - ad->t.tm_mday = gps->day; | |
79 | - ad->t.tm_hour = gps->hour; | |
80 | - ad->t.tm_min = gps->min; | |
81 | - ad->t.tm_sec = gps->sec; | |
82 | - | |
83 | - if (gps->year == 0) return -1; | |
84 | - | |
85 | - return 0; | |
86 | -} | |
87 | -/* | |
88 | - int freq | |
89 | - 平均化後の周波数 Hz | |
90 | -*/ | |
91 | -static void do_avg(int freq, AdData *ad) | |
92 | -{ | |
93 | - int ch, i, j; | |
94 | - long add; | |
95 | - int avg_freq; | |
96 | - int avg_num; | |
97 | - | |
98 | - avg_freq = conf_freq_get(); | |
99 | - avg_num = AD_SAMPLE/avg_freq; | |
100 | - for(ch = 0; ch < AD_CHNUM; ch++) { | |
101 | - for(i = 0; i < avg_freq; i++) { | |
102 | - add = 0; | |
103 | - for(j = 0; j < avg_num; j++) { | |
104 | - add += ad->data[ch][i*avg_num + j]; | |
105 | - } | |
106 | - ad->avg[ch][i] = add / avg_num; | |
107 | - } | |
108 | - } | |
109 | -} | |
110 | -static unsigned int sum_calc(char *buf) | |
111 | -{ | |
112 | - int i; | |
113 | - u_int8_t suma = 0; | |
114 | - u_int8_t sumb = 0; | |
115 | - u_int16_t uint_sum; | |
116 | - | |
117 | - for(i = SPI_OFS_RESCODE; i < SPI_OFS_SUM; i++) { | |
118 | - suma += (u_int8_t)buf[i]; | |
119 | - sumb += suma; | |
120 | - } | |
121 | - uint_sum = ((sumb << 8) & 0xFF00U) | suma; | |
122 | - return uint_sum; | |
123 | -} | |
124 | - | |
125 | -void* thread_rcv(void* pParam) | |
126 | -{ | |
127 | - | |
128 | - fd_set fds; | |
129 | - char buf[SPI_DATA_LEN+256]; | |
130 | - int i; | |
131 | - int fd_spi; | |
132 | - AdData ad, *ad_ptr; | |
133 | - unsigned int sum; | |
134 | -#if 0 | |
135 | -char cmd[SPI_CMD_LEN]; | |
136 | -char c; | |
137 | -c=0; | |
138 | -#endif | |
139 | - // spiドライバのリングバッファクリア | |
140 | - spi_buf_clear(); | |
141 | - // spi送信コマンド長セット | |
142 | - spi_tx_len_set(SPI_CMD_LEN); | |
143 | - | |
144 | - while(1) { | |
145 | - fd_spi = spi_get_fd(); | |
146 | - // select の準備 | |
147 | - FD_ZERO(&fds); | |
148 | - // FDセット | |
149 | - FD_SET(fd_spi, &fds); | |
150 | - // 受信を待つ タイムアウト無し | |
151 | - i = select(fd_spi + 1, &fds, NULL, NULL, NULL); // 読みselect | |
152 | - if(i <= 0) syslog(LOG_ERR, "%s: select returned with signal or error. ret=%d\n", __FUNCTION__, i); | |
153 | - if(FD_ISSET(fd_spi, &fds)) { | |
154 | -#if 0 | |
155 | -// SPI送信データセット | |
156 | -if (c++ % 5 == 0) { | |
157 | - memset(cmd, 0, SPI_CMD_LEN); | |
158 | - for(i = 0; i < AD_CHNUM; i++) | |
159 | - cmd[i] = SPI_CMD_GAIN_128; | |
160 | - spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM); | |
161 | -} | |
162 | -#endif | |
163 | - // 受信した | |
164 | - i = spi_dnum_get(); | |
165 | -//PDEBUG("thread_rcv(): wakeup dnum=%d rcv_len=%d\n", i, spi_rcvd_len_get()); | |
166 | - while(i-- > 0) { | |
167 | - memset(&ad, 0, sizeof(ad)); | |
168 | - // データ取得 | |
169 | - spi_rx_get(buf); | |
170 | - // デコード | |
171 | - if (decode(buf, &ad)) continue; | |
172 | - // chekcsum check | |
173 | - sum = sum_calc(buf); | |
174 | - if (sum != ad.checksum) { | |
175 | - PDEBUG("thread_rcv(): SUM ERR! CALC=%04X RCV=%04X\r\n", sum, ad.checksum); | |
176 | - } | |
177 | - // 平均 | |
178 | - ad.freq = conf_freq_get(); | |
179 | - do_avg(AD_SAMPLE, &ad); | |
180 | -#if 1 | |
181 | -int ch; | |
182 | -PDEBUG("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X,%d", | |
183 | - ad.gps.year, ad.gps.month, ad.gps.day, ad.gps.hour, ad.gps.min, ad.gps.sec, ad.gps.nano, | |
184 | - ad.gps.tow, ad.gps.tacc, ad.gps.valid, ad.rescode); | |
185 | -for(ch = 0; ch < AD_CHNUM; ch++) { | |
186 | - PDEBUG(",%+7ld", ad.data1sec[ch]); | |
187 | -} | |
188 | -PDEBUG("\r\n"); | |
189 | -#endif | |
190 | -#if 0 | |
191 | -for(ch = 0; ch < AD_CHNUM; ch++) { | |
192 | - PDEBUG(",%+7ld", ad.data[ch][0]); | |
193 | -} | |
194 | -PDEBUG("\r\n"); | |
195 | -for(ch = 0; ch < AD_CHNUM; ch++) { | |
196 | - PDEBUG(",%+7ld", ad.data[ch][49]); | |
197 | -} | |
198 | -PDEBUG("\r\n"); | |
199 | -#endif | |
200 | - // ADリングバッファに書き込み | |
201 | - ad_ptr = ad_ring_get(ad_ring_write_get()); | |
202 | - *ad_ptr = ad; | |
203 | - // 最新データ位置 | |
204 | - ad_ring_latest_set(ad_ring_write_get()); | |
205 | - // 書き込み位置+1 | |
206 | - ad_ring_write_plus(); | |
207 | - | |
208 | - } // while((i = sub_dnum_get()) > 0) { | |
209 | - } // if(FD_ISSET(fd_sub, &fds)) { | |
210 | - } // while(1) { | |
211 | - return NULL; | |
212 | -} | |
213 | - | |
214 | -// | |
215 | -/**** CUnit test | |
216 | -*/ | |
217 | -#ifdef CUNIT | |
218 | -#include <CUnit/CUnit.h> | |
219 | - | |
220 | -static void test_b3_to_long32(void) | |
221 | -{ | |
222 | - long l; | |
223 | - char buf[SPI_DATA_LEN+256]; | |
224 | - AdData ad; | |
225 | - int i; | |
226 | - | |
227 | - buf[0] = 0x56; | |
228 | - buf[1] = 0x34; | |
229 | - buf[2] = 0x12; | |
230 | - l = b3_to_long32(buf); | |
231 | - CU_ASSERT(l == 0x00123456); | |
232 | - | |
233 | - buf[0] = 0x56; | |
234 | - buf[1] = 0x34; | |
235 | - buf[2] = 0x82; | |
236 | - l = b3_to_long32(buf); | |
237 | - CU_ASSERT(l == (long)0xFF823456); | |
238 | - | |
239 | - buf[0] = 0xFF; | |
240 | - buf[1] = 0xFF; | |
241 | - buf[2] = 0xFF; | |
242 | - l = b3_to_long32(buf); | |
243 | - CU_ASSERT(l == -1); | |
244 | - | |
245 | - buf[0] = '$'; | |
246 | - i = SPI_OFS_DATA; | |
247 | - buf[i++] = 0x56; | |
248 | - buf[i++] = 0x34; | |
249 | - buf[i++] = 0x12; | |
250 | - decode(buf, &ad); | |
251 | - CU_ASSERT(ad.data[0][0] == 0x123456); | |
252 | - | |
253 | - buf[0] = '$'; | |
254 | - i = SPI_OFS_DATA + AD_CHNUM * AD_SAMPLE * 3; | |
255 | - i -= 3; | |
256 | - buf[i++] = 0x56; | |
257 | - buf[i++] = 0x34; | |
258 | - buf[i++] = 0x12; | |
259 | - decode(buf, &ad); | |
260 | - CU_ASSERT(ad.data[AD_CHNUM-1][AD_SAMPLE-1] == 0x123456); | |
261 | -} | |
262 | - | |
263 | -void thread_rcv_test(CU_pSuite test_suite) | |
264 | -{ | |
265 | - | |
266 | - CU_add_test(test_suite, "test_b3_to_long32", test_b3_to_long32); | |
267 | - | |
268 | -} | |
269 | - | |
270 | -#endif | |
1 | +#include <stdio.h> | |
2 | +//#include <unistd.h> | |
3 | +//#include <sys/ioctl.h> | |
4 | +//#include <termios.h> | |
5 | +#include <fcntl.h> | |
6 | +#include <syslog.h> | |
7 | +#include <sys/types.h> | |
8 | +#include <string.h> | |
9 | + | |
10 | +#include "debug_print.h" | |
11 | +#include "spi.h" | |
12 | +#include "ad_ring.h" | |
13 | +#include "conf.h" | |
14 | + | |
15 | +/* | |
16 | + Little Endian | |
17 | + 3byte -> signed long(4byte) | |
18 | +*/ | |
19 | +static long b3_to_long32(unsigned char *ptr) | |
20 | +{ | |
21 | + char buf[4]; | |
22 | + | |
23 | + buf[0] = *ptr++; | |
24 | + buf[1] = *ptr++; | |
25 | + buf[2] = *ptr; | |
26 | + if (*ptr & 0x80) { | |
27 | + buf[3] = 0xFF; | |
28 | + } else { | |
29 | + buf[3] = 0; | |
30 | + } | |
31 | + return *((long*)buf); | |
32 | +} | |
33 | +static int decode(char *buf, AdData *ad) | |
34 | +{ | |
35 | + UbloxNavTimeUtc *gps; | |
36 | + u_int8_t *ptr; | |
37 | + int ch; | |
38 | + int i; | |
39 | + | |
40 | + ptr = (u_int8_t*)buf; | |
41 | + gps = &(ad->gps); | |
42 | + | |
43 | + if (*ptr++ != SPI_HEAD_CHAR) return -1; | |
44 | + // Response Code | |
45 | + ad->rescode = *ptr++; | |
46 | + // Response Data | |
47 | + memcpy(ad->resdata, ptr, SPI_RESDATA_LEN); | |
48 | + ptr += SPI_RESDATA_LEN; | |
49 | + // GPS | |
50 | + gps->tow = *((int32_t*)ptr); ptr += 4; | |
51 | + gps->tacc = *((int32_t*)ptr); ptr += 4; | |
52 | + gps->nano = *((int32_t*)ptr); ptr += 4; | |
53 | + gps->year = *((int16_t*)ptr); ptr += 2; | |
54 | + gps->month = *((u_int8_t*)ptr); ptr++; | |
55 | + gps->day = *((u_int8_t*)ptr); ptr++; | |
56 | + gps->hour = *((u_int8_t*)ptr); ptr++; | |
57 | + gps->min = *((u_int8_t*)ptr); ptr++; | |
58 | + gps->sec = *((u_int8_t*)ptr); ptr++; | |
59 | + gps->valid = *((u_int8_t*)ptr); ptr++; | |
60 | + // 1sec平均 | |
61 | + for(ch = 0; ch < AD_CHNUM; ch++) { | |
62 | + ad->data1sec[ch] = *((int32_t*)ptr); | |
63 | + ptr += 4; | |
64 | + } | |
65 | + // 50Hz data | |
66 | + for(ch = 0; ch < AD_CHNUM; ch++) { | |
67 | + for(i = 0; i < AD_SAMPLE; i++) { | |
68 | + ad->data[ch][i] = b3_to_long32(ptr); | |
69 | + ptr += 3; | |
70 | + } | |
71 | + } | |
72 | + // Checksum | |
73 | + ad->checksum = *((u_int16_t*)ptr); | |
74 | + | |
75 | + // gps->struct tm | |
76 | + ad->t.tm_year = gps->year - 1900; | |
77 | + ad->t.tm_mon = gps->month - 1; // struct tmの月は(0〜11)なので注意 | |
78 | + ad->t.tm_mday = gps->day; | |
79 | + ad->t.tm_hour = gps->hour; | |
80 | + ad->t.tm_min = gps->min; | |
81 | + ad->t.tm_sec = gps->sec; | |
82 | + | |
83 | + if (gps->year == 0) return -1; | |
84 | + | |
85 | + return 0; | |
86 | +} | |
87 | +/* | |
88 | + int freq | |
89 | + 平均化後の周波数 Hz | |
90 | +*/ | |
91 | +static void do_avg(int freq, AdData *ad) | |
92 | +{ | |
93 | + int ch, i, j; | |
94 | + long add; | |
95 | + int avg_freq; | |
96 | + int avg_num; | |
97 | + | |
98 | + avg_freq = conf_freq_get(); | |
99 | + avg_num = AD_SAMPLE/avg_freq; | |
100 | + for(ch = 0; ch < AD_CHNUM; ch++) { | |
101 | + for(i = 0; i < avg_freq; i++) { | |
102 | + add = 0; | |
103 | + for(j = 0; j < avg_num; j++) { | |
104 | + add += ad->data[ch][i*avg_num + j]; | |
105 | + } | |
106 | + ad->avg[ch][i] = add / avg_num; | |
107 | + } | |
108 | + } | |
109 | +} | |
110 | +static unsigned int sum_calc(char *buf) | |
111 | +{ | |
112 | + int i; | |
113 | + u_int8_t suma = 0; | |
114 | + u_int8_t sumb = 0; | |
115 | + u_int16_t uint_sum; | |
116 | + | |
117 | + for(i = SPI_OFS_RESCODE; i < SPI_OFS_SUM; i++) { | |
118 | + suma += (u_int8_t)buf[i]; | |
119 | + sumb += suma; | |
120 | + } | |
121 | + uint_sum = ((sumb << 8) & 0xFF00U) | suma; | |
122 | + return uint_sum; | |
123 | +} | |
124 | + | |
125 | +void* thread_rcv(void* pParam) | |
126 | +{ | |
127 | + | |
128 | + fd_set fds; | |
129 | + char buf[SPI_DATA_LEN+256]; | |
130 | + int i; | |
131 | + int fd_spi; | |
132 | + AdData ad, *ad_ptr; | |
133 | + unsigned int sum; | |
134 | +#if 0 | |
135 | +char cmd[SPI_CMD_LEN]; | |
136 | +char c; | |
137 | +c=0; | |
138 | +#endif | |
139 | + // spiドライバのリングバッファクリア | |
140 | + spi_buf_clear(); | |
141 | + // spi送信コマンド長セット | |
142 | + spi_tx_len_set(SPI_CMD_LEN); | |
143 | + | |
144 | + while(1) { | |
145 | + fd_spi = spi_get_fd(); | |
146 | + // select の準備 | |
147 | + FD_ZERO(&fds); | |
148 | + // FDセット | |
149 | + FD_SET(fd_spi, &fds); | |
150 | + // 受信を待つ タイムアウト無し | |
151 | + i = select(fd_spi + 1, &fds, NULL, NULL, NULL); // 読みselect | |
152 | + if(i <= 0) syslog(LOG_ERR, "%s: select returned with signal or error. ret=%d\n", __FUNCTION__, i); | |
153 | + if(FD_ISSET(fd_spi, &fds)) { | |
154 | +#if 0 | |
155 | +// SPI送信データセット | |
156 | +if (c++ % 5 == 0) { | |
157 | + memset(cmd, 0, SPI_CMD_LEN); | |
158 | + for(i = 0; i < AD_CHNUM; i++) | |
159 | + cmd[i] = SPI_CMD_GAIN_128; | |
160 | + spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM); | |
161 | +} | |
162 | +#endif | |
163 | + // 受信した | |
164 | + i = spi_dnum_get(); | |
165 | +PDEBUG("thread_rcv(): wakeup dnum=%d rcv_len=%d\n", i, spi_rcvd_len_get()); | |
166 | + while(i-- > 0) { | |
167 | + memset(&ad, 0, sizeof(ad)); | |
168 | + // データ取得 | |
169 | + spi_rx_get(buf); | |
170 | + // デコード | |
171 | + if (decode(buf, &ad)) continue; | |
172 | + // chekcsum check | |
173 | + sum = sum_calc(buf); | |
174 | + if (sum != ad.checksum) { | |
175 | + PDEBUG("thread_rcv(): SUM ERR! CALC=%04X RCV=%04X\r\n", sum, ad.checksum); | |
176 | + } | |
177 | + // 平均 | |
178 | + ad.freq = conf_freq_get(); | |
179 | + do_avg(AD_SAMPLE, &ad); | |
180 | +#if 1 | |
181 | +int ch; | |
182 | +PDEBUG("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X,%d", | |
183 | + ad.gps.year, ad.gps.month, ad.gps.day, ad.gps.hour, ad.gps.min, ad.gps.sec, ad.gps.nano, | |
184 | + ad.gps.tow, ad.gps.tacc, ad.gps.valid, ad.rescode); | |
185 | +for(ch = 0; ch < AD_CHNUM; ch++) { | |
186 | + PDEBUG(",%+7ld", ad.data1sec[ch]); | |
187 | +} | |
188 | +PDEBUG("\r\n"); | |
189 | +#endif | |
190 | +#if 0 | |
191 | +for(ch = 0; ch < AD_CHNUM; ch++) { | |
192 | + PDEBUG(",%+7ld", ad.data[ch][0]); | |
193 | +} | |
194 | +PDEBUG("\r\n"); | |
195 | +for(ch = 0; ch < AD_CHNUM; ch++) { | |
196 | + PDEBUG(",%+7ld", ad.data[ch][49]); | |
197 | +} | |
198 | +PDEBUG("\r\n"); | |
199 | +#endif | |
200 | + // ADリングバッファに書き込み | |
201 | + ad_ptr = ad_ring_get(ad_ring_write_get()); | |
202 | + *ad_ptr = ad; | |
203 | + // 最新データ位置 | |
204 | + ad_ring_latest_set(ad_ring_write_get()); | |
205 | + // 書き込み位置+1 | |
206 | + ad_ring_write_plus(); | |
207 | + | |
208 | + } // while((i = sub_dnum_get()) > 0) { | |
209 | + } // if(FD_ISSET(fd_sub, &fds)) { | |
210 | + } // while(1) { | |
211 | + return NULL; | |
212 | +} | |
213 | + | |
214 | +// | |
215 | +/**** CUnit test | |
216 | +*/ | |
217 | +#ifdef CUNIT | |
218 | +#include <CUnit/CUnit.h> | |
219 | + | |
220 | +static void test_b3_to_long32(void) | |
221 | +{ | |
222 | + long l; | |
223 | + char buf[SPI_DATA_LEN+256]; | |
224 | + AdData ad; | |
225 | + int i; | |
226 | + | |
227 | + buf[0] = 0x56; | |
228 | + buf[1] = 0x34; | |
229 | + buf[2] = 0x12; | |
230 | + l = b3_to_long32(buf); | |
231 | + CU_ASSERT(l == 0x00123456); | |
232 | + | |
233 | + buf[0] = 0x56; | |
234 | + buf[1] = 0x34; | |
235 | + buf[2] = 0x82; | |
236 | + l = b3_to_long32(buf); | |
237 | + CU_ASSERT(l == (long)0xFF823456); | |
238 | + | |
239 | + buf[0] = 0xFF; | |
240 | + buf[1] = 0xFF; | |
241 | + buf[2] = 0xFF; | |
242 | + l = b3_to_long32(buf); | |
243 | + CU_ASSERT(l == -1); | |
244 | + | |
245 | + buf[0] = '$'; | |
246 | + i = SPI_OFS_DATA; | |
247 | + buf[i++] = 0x56; | |
248 | + buf[i++] = 0x34; | |
249 | + buf[i++] = 0x12; | |
250 | + decode(buf, &ad); | |
251 | + CU_ASSERT(ad.data[0][0] == 0x123456); | |
252 | + | |
253 | + buf[0] = '$'; | |
254 | + i = SPI_OFS_DATA + AD_CHNUM * AD_SAMPLE * 3; | |
255 | + i -= 3; | |
256 | + buf[i++] = 0x56; | |
257 | + buf[i++] = 0x34; | |
258 | + buf[i++] = 0x12; | |
259 | + decode(buf, &ad); | |
260 | + CU_ASSERT(ad.data[AD_CHNUM-1][AD_SAMPLE-1] == 0x123456); | |
261 | +} | |
262 | + | |
263 | +void thread_rcv_test(CU_pSuite test_suite) | |
264 | +{ | |
265 | + | |
266 | + CU_add_test(test_suite, "test_b3_to_long32", test_b3_to_long32); | |
267 | + | |
268 | +} | |
269 | + | |
270 | +#endif |