• R/O
  • HTTP
  • SSH
  • HTTPS

scilog: Commit

Overoで動作するデータ収録プログラム
サブCPUからGPS時刻付きADデータを取得してSDに記録する。


Commit MetaInfo

Revisão61ae9916f528940690a8ddeb35503931e566698e (tree)
Hora2011-12-20 10:35:52
AutorNaoya Takamura <ntaka206@user...>
CommiterNaoya Takamura

Mensagem de Log

thread_disp() usleep(100msec)追加

Mudança Sumário

Diff

Binary files a/scilog and b/scilog differ
--- a/spi.c
+++ b/spi.c
@@ -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+
--- a/spi.h
+++ b/spi.h
@@ -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
--- a/thread_disp.c
+++ b/thread_disp.c
@@ -3,30 +3,6 @@
33 #include "ad_ring.h"
44 #include "lcd.h"
55
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-
306 /*
317
328 */
@@ -90,5 +66,6 @@ void* thread_disp(void* pParam)
9066 // AD表示
9167 scr_disp_ad(d);
9268 }
69+ usleep(100*1000);
9370 }
9471 }
--- a/thread_rcv.c
+++ b/thread_rcv.c
@@ -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
Show on old repository browser