-#include <stdio.h>\r
-#include <unistd.h>\r
-#include <sys/ioctl.h>\r
-//#include <termios.h>\r
-#include <fcntl.h>\r
-#include <string.h>\r
-\r
-#include "spi.h"\r
-#include "ad_ring.h"\r
-\r
-static int fd_spi = -1;\r
-\r
-int spi_get_fd(void)\r
-{\r
- return fd_spi;\r
-}\r
-int spi_open(char *dev)\r
-{\r
- fd_spi = open(dev, O_RDWR);\r
- return fd_spi;\r
-}\r
-/*\r
- デバイスへのioctl\r
-*/\r
-int spi_ioctl(int cmd, void *p)\r
-{\r
- return ioctl(fd_spi, cmd, p);\r
-}\r
-\r
-int spi_close(void)\r
-{\r
- return close(fd_spi);\r
-}\r
-\r
-// 送信データ長セット\r
-void spi_tx_len_set(int len)\r
-{\r
- spi_ioctl(CMD_TX_LEN, &len);\r
-}\r
-// 同期シリアル受信データ取得\r
-void spi_rx_get(char *p)\r
-{\r
- spi_ioctl(CMD_RX_GET, p);\r
-}\r
-\r
-// 同期シリアル送信データセット\r
-void spi_tx_set(char *p)\r
-{\r
- spi_ioctl(CMD_TX_SET, p);\r
-}\r
-// リングバッファクリア\r
-void spi_buf_clear(void)\r
-{\r
- spi_ioctl(CMD_BUF_CLEAR, NULL);\r
-}\r
-\r
-// リングバッファのデータ数get\r
-int spi_dnum_get(void)\r
-{\r
- int i = 0;\r
- spi_ioctl(CMD_DNUM_GET, &i);\r
- return i;\r
-}\r
-// SPIで実際に受信したデータ長を取得\r
-int spi_rcvd_len_get(void)\r
-{\r
- int i = 0;\r
- spi_ioctl(CMD_RECEIVED_LEN_GET, &i);\r
- return i;\r
-}\r
-// コマンド送信\r
-void spi_cmd_send(char cmd_code, char *cmd_data, int data_len)\r
-{\r
- char cmd[SPI_CMD_LEN];\r
- \r
- memset(cmd, 0, SPI_CMD_LEN);\r
-\r
- cmd[SPI_CMD_OFS_HEAD] = SPI_HEAD_CHAR;\r
- cmd[SPI_CMD_OFS_CMDCODE] = cmd_code;\r
-\r
- // data\r
- if (data_len > (SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA)) {\r
- data_len = SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA;\r
- }\r
- memcpy(&cmd[SPI_CMD_OFS_CMDDATA], cmd_data, data_len);\r
-\r
- spi_tx_set(cmd);\r
-}\r
-// Gainコマンド送信\r
-void spi_cmd_send_gain(int gain)\r
-{\r
- char cmd[SPI_CMD_LEN];\r
- int i;\r
- \r
- memset(cmd, 0, SPI_CMD_LEN);\r
-\r
- for(i = 0; i < AD_CHNUM; i++)\r
- cmd[i] = gain;\r
- spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM);\r
-}\r
-\r
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+//#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include "spi.h"
+#include "ad_ring.h"
+
+static int fd_spi = -1;
+
+int spi_get_fd(void)
+{
+ return fd_spi;
+}
+int spi_open(char *dev)
+{
+ fd_spi = open(dev, O_RDWR);
+ return fd_spi;
+}
+/*
+ デバイスへのioctl
+*/
+int spi_ioctl(int cmd, void *p)
+{
+ return ioctl(fd_spi, cmd, p);
+}
+
+int spi_close(void)
+{
+ return close(fd_spi);
+}
+
+// 送信データ長セット
+void spi_tx_len_set(int len)
+{
+ spi_ioctl(CMD_TX_LEN, &len);
+}
+// 同期シリアル受信データ取得
+void spi_rx_get(char *p)
+{
+ spi_ioctl(CMD_RX_GET, p);
+}
+
+// 同期シリアル送信データセット
+void spi_tx_set(char *p)
+{
+ spi_ioctl(CMD_TX_SET, p);
+}
+// リングバッファクリア
+void spi_buf_clear(void)
+{
+ spi_ioctl(CMD_BUF_CLEAR, NULL);
+}
+
+// リングバッファのデータ数get
+int spi_dnum_get(void)
+{
+ int i = 0;
+ spi_ioctl(CMD_DNUM_GET, &i);
+ return i;
+}
+// SPIで実際に受信したデータ長を取得
+int spi_rcvd_len_get(void)
+{
+ int i = 0;
+ spi_ioctl(CMD_RECEIVED_LEN_GET, &i);
+ return i;
+}
+// コマンド送信
+void spi_cmd_send(char cmd_code, char *cmd_data, int data_len)
+{
+ char cmd[SPI_CMD_LEN];
+
+ memset(cmd, 0, SPI_CMD_LEN);
+
+ cmd[SPI_CMD_OFS_HEAD] = SPI_HEAD_CHAR;
+ cmd[SPI_CMD_OFS_CMDCODE] = cmd_code;
+
+ // data
+ if (data_len > (SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA)) {
+ data_len = SPI_CMD_LEN - SPI_CMD_OFS_CMDDATA;
+ }
+ memcpy(&cmd[SPI_CMD_OFS_CMDDATA], cmd_data, data_len);
+
+ spi_tx_set(cmd);
+}
+// Gainコマンド送信
+void spi_cmd_send_gain(int gain)
+{
+ char cmd[SPI_CMD_LEN];
+ int i;
+
+ memset(cmd, 0, SPI_CMD_LEN);
+
+ for(i = 0; i < AD_CHNUM; i++)
+ cmd[i] = gain;
+ spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM);
+}
+
-#if !defined(__SPI_H__)\r
-#define __SPI_H__\r
-\r
-// ioctlコマンド\r
-// SPI受信データ取得\r
-#define CMD_RX_GET 11\r
-// SPI送信データ長セット\r
-#define CMD_TX_LEN 12\r
-// SPI送信データセット PICへのコマンド\r
-#define CMD_TX_SET 13\r
-// リングバッファにあるデータ数を返す\r
-#define CMD_DNUM_GET 14\r
-// リングバッファクリア\r
-#define CMD_BUF_CLEAR 15\r
-// SPIで実際に受信したデータ長を取得\r
-#define CMD_RECEIVED_LEN_GET 20\r
-\r
-// SPIで受信するデータ長 固定長 $含む\r
-#define SPI_DATA_LEN (965)\r
-// SPI受信データの先頭文字 送信コマンドでも使用\r
-#define SPI_HEAD_CHAR '$'\r
-\r
-// SPI受信データ 応答データ 長さ\r
-#define SPI_RESDATA_LEN 17\r
-\r
-// SPI受信データ Checksum 長さ\r
-#define SPI_CHECKSUM_LEN 2\r
-\r
-// SPI受信データの先頭からのオフセット\r
-#define SPI_OFS_RESCODE 1\r
-#define SPI_OFS_RESDATA 2\r
-#define SPI_OFS_GPS (19)\r
-#define SPI_OFS_1SEC (39)\r
-#define SPI_OFS_DATA (63)\r
-#define SPI_OFS_SUM 963\r
-\r
-// SPI送信コマンド長 固定長 $含む\r
-#define SPI_CMD_LEN 20\r
-\r
-// SPI送信コマンド フィールドのオフセット位置\r
-#define SPI_CMD_OFS_HEAD 0\r
-#define SPI_CMD_OFS_CMDCODE 1\r
-#define SPI_CMD_OFS_CMDDATA 2\r
-\r
-// SPIコマンドコード\r
-#define SPI_CMDCODE_GAIN 1\r
-\r
-// SPIコマンドGainの設定値\r
-#define SPI_CMD_GAIN_1P8 0 // 1/8\r
-#define SPI_CMD_GAIN_1P4 1 // 1/4\r
-#define SPI_CMD_GAIN_1P2 2 // 1/2\r
-#define SPI_CMD_GAIN_1 3\r
-#define SPI_CMD_GAIN_2 4\r
-#define SPI_CMD_GAIN_4 5\r
-#define SPI_CMD_GAIN_8 6\r
-#define SPI_CMD_GAIN_16 7\r
-#define SPI_CMD_GAIN_32 8\r
-#define SPI_CMD_GAIN_64 9\r
-#define SPI_CMD_GAIN_128 10\r
-\r
-\r
-int spi_get_fd(void);\r
-int spi_open(char *dev);\r
-int spi_ioctl(int cmd, void *p);\r
-int spi_close(void);\r
-\r
-void spi_tx_len_set(int len);\r
-void spi_rx_get(char *p);\r
-void spi_tx_set(char *p);\r
-void spi_buf_clear(void);\r
-int spi_dnum_get(void);\r
-int spi_rcvd_len_get(void);\r
-\r
-void spi_cmd_send(char cmd_code, char *cmd_data, int data_len);\r
-void spi_cmd_send_gain(int gain);\r
-\r
-#endif\r
+#if !defined(__SPI_H__)
+#define __SPI_H__
+
+// ioctlコマンド
+// SPI受信データ取得
+#define CMD_RX_GET 11
+// SPI送信データ長セット
+#define CMD_TX_LEN 12
+// SPI送信データセット PICへのコマンド
+#define CMD_TX_SET 13
+// リングバッファにあるデータ数を返す
+#define CMD_DNUM_GET 14
+// リングバッファクリア
+#define CMD_BUF_CLEAR 15
+// SPIで実際に受信したデータ長を取得
+#define CMD_RECEIVED_LEN_GET 20
+
+// SPIで受信するデータ長 固定長 $含む
+#define SPI_DATA_LEN (965)
+// SPI受信データの先頭文字 送信コマンドでも使用
+#define SPI_HEAD_CHAR '$'
+
+// SPI受信データ 応答データ 長さ
+#define SPI_RESDATA_LEN 17
+
+// SPI受信データ Checksum 長さ
+#define SPI_CHECKSUM_LEN 2
+
+// SPI受信データの先頭からのオフセット
+#define SPI_OFS_RESCODE 1
+#define SPI_OFS_RESDATA 2
+#define SPI_OFS_GPS (19)
+#define SPI_OFS_1SEC (39)
+#define SPI_OFS_DATA (63)
+#define SPI_OFS_SUM 963
+
+// SPI送信コマンド長 固定長 $含む
+#define SPI_CMD_LEN 20
+
+// SPI送信コマンド フィールドのオフセット位置
+#define SPI_CMD_OFS_HEAD 0
+#define SPI_CMD_OFS_CMDCODE 1
+#define SPI_CMD_OFS_CMDDATA 2
+
+// SPIコマンドコード
+#define SPI_CMDCODE_GAIN 1
+
+// SPIコマンドGainの設定値
+#define SPI_CMD_GAIN_1P8 0 // 1/8
+#define SPI_CMD_GAIN_1P4 1 // 1/4
+#define SPI_CMD_GAIN_1P2 2 // 1/2
+#define SPI_CMD_GAIN_1 3
+#define SPI_CMD_GAIN_2 4
+#define SPI_CMD_GAIN_4 5
+#define SPI_CMD_GAIN_8 6
+#define SPI_CMD_GAIN_16 7
+#define SPI_CMD_GAIN_32 8
+#define SPI_CMD_GAIN_64 9
+#define SPI_CMD_GAIN_128 10
+
+
+int spi_get_fd(void);
+int spi_open(char *dev);
+int spi_ioctl(int cmd, void *p);
+int spi_close(void);
+
+void spi_tx_len_set(int len);
+void spi_rx_get(char *p);
+void spi_tx_set(char *p);
+void spi_buf_clear(void);
+int spi_dnum_get(void);
+int spi_rcvd_len_get(void);
+
+void spi_cmd_send(char cmd_code, char *cmd_data, int data_len);
+void spi_cmd_send_gain(int gain);
+
+#endif
-#include <stdio.h>\r
-//#include <unistd.h>\r
-//#include <sys/ioctl.h>\r
-//#include <termios.h>\r
-#include <fcntl.h>\r
-#include <syslog.h>\r
-#include <sys/types.h>\r
-#include <string.h>\r
-\r
-#include "debug_print.h"\r
-#include "spi.h"\r
-#include "ad_ring.h"\r
-#include "conf.h"\r
-\r
-/*\r
- Little Endian\r
- 3byte -> signed long(4byte)\r
-*/\r
-static long b3_to_long32(unsigned char *ptr)\r
-{\r
- char buf[4];\r
- \r
- buf[0] = *ptr++;\r
- buf[1] = *ptr++;\r
- buf[2] = *ptr;\r
- if (*ptr & 0x80) {\r
- buf[3] = 0xFF;\r
- } else {\r
- buf[3] = 0;\r
- }\r
- return *((long*)buf);\r
-}\r
-static int decode(char *buf, AdData *ad)\r
-{\r
- UbloxNavTimeUtc *gps;\r
- u_int8_t *ptr;\r
- int ch;\r
- int i;\r
- \r
- ptr = (u_int8_t*)buf;\r
- gps = &(ad->gps);\r
-\r
- if (*ptr++ != SPI_HEAD_CHAR) return -1;\r
- // Response Code\r
- ad->rescode = *ptr++;\r
- // Response Data\r
- memcpy(ad->resdata, ptr, SPI_RESDATA_LEN);\r
- ptr += SPI_RESDATA_LEN;\r
- // GPS\r
- gps->tow = *((int32_t*)ptr); ptr += 4;\r
- gps->tacc = *((int32_t*)ptr); ptr += 4;\r
- gps->nano = *((int32_t*)ptr); ptr += 4;\r
- gps->year = *((int16_t*)ptr); ptr += 2;\r
- gps->month = *((u_int8_t*)ptr); ptr++;\r
- gps->day = *((u_int8_t*)ptr); ptr++;\r
- gps->hour = *((u_int8_t*)ptr); ptr++;\r
- gps->min = *((u_int8_t*)ptr); ptr++;\r
- gps->sec = *((u_int8_t*)ptr); ptr++;\r
- gps->valid = *((u_int8_t*)ptr); ptr++;\r
- // 1sec平均\r
- for(ch = 0; ch < AD_CHNUM; ch++) {\r
- ad->data1sec[ch] = *((int32_t*)ptr);\r
- ptr += 4;\r
- }\r
- // 50Hz data\r
- for(ch = 0; ch < AD_CHNUM; ch++) {\r
- for(i = 0; i < AD_SAMPLE; i++) {\r
- ad->data[ch][i] = b3_to_long32(ptr);\r
- ptr += 3;\r
- }\r
- }\r
- // Checksum\r
- ad->checksum = *((u_int16_t*)ptr);\r
- \r
- // gps->struct tm\r
- ad->t.tm_year = gps->year - 1900;\r
- ad->t.tm_mon = gps->month - 1; // struct tmの月は(0〜11)なので注意\r
- ad->t.tm_mday = gps->day;\r
- ad->t.tm_hour = gps->hour;\r
- ad->t.tm_min = gps->min;\r
- ad->t.tm_sec = gps->sec;\r
- \r
- if (gps->year == 0) return -1;\r
-\r
- return 0;\r
-}\r
-/*\r
- int freq\r
- 平均化後の周波数 Hz\r
-*/\r
-static void do_avg(int freq, AdData *ad)\r
-{\r
- int ch, i, j;\r
- long add;\r
- int avg_freq;\r
- int avg_num;\r
- \r
- avg_freq = conf_freq_get();\r
- avg_num = AD_SAMPLE/avg_freq;\r
- for(ch = 0; ch < AD_CHNUM; ch++) {\r
- for(i = 0; i < avg_freq; i++) {\r
- add = 0;\r
- for(j = 0; j < avg_num; j++) {\r
- add += ad->data[ch][i*avg_num + j];\r
- }\r
- ad->avg[ch][i] = add / avg_num;\r
- }\r
- }\r
-}\r
-static unsigned int sum_calc(char *buf)\r
-{\r
- int i;\r
- u_int8_t suma = 0;\r
- u_int8_t sumb = 0;\r
- u_int16_t uint_sum;\r
-\r
- for(i = SPI_OFS_RESCODE; i < SPI_OFS_SUM; i++) {\r
- suma += (u_int8_t)buf[i];\r
- sumb += suma;\r
- }\r
- uint_sum = ((sumb << 8) & 0xFF00U) | suma;\r
- return uint_sum;\r
-}\r
-\r
-void* thread_rcv(void* pParam)\r
-{\r
-\r
- fd_set fds;\r
- char buf[SPI_DATA_LEN+256];\r
- int i;\r
- int fd_spi;\r
- AdData ad, *ad_ptr;\r
- unsigned int sum;\r
-#if 0\r
-char cmd[SPI_CMD_LEN];\r
-char c;\r
-c=0;\r
-#endif\r
- // spiドライバのリングバッファクリア\r
- spi_buf_clear();\r
- // spi送信コマンド長セット\r
- spi_tx_len_set(SPI_CMD_LEN);\r
-\r
- while(1) {\r
- fd_spi = spi_get_fd();\r
- // select の準備\r
- FD_ZERO(&fds);\r
- // FDセット\r
- FD_SET(fd_spi, &fds);\r
- // 受信を待つ タイムアウト無し\r
- i = select(fd_spi + 1, &fds, NULL, NULL, NULL); // 読みselect\r
- if(i <= 0) syslog(LOG_ERR, "%s: select returned with signal or error. ret=%d\n", __FUNCTION__, i);\r
- if(FD_ISSET(fd_spi, &fds)) {\r
-#if 0\r
-// SPI送信データセット\r
-if (c++ % 5 == 0) {\r
- memset(cmd, 0, SPI_CMD_LEN);\r
- for(i = 0; i < AD_CHNUM; i++)\r
- cmd[i] = SPI_CMD_GAIN_128;\r
- spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM);\r
-}\r
-#endif\r
- // 受信した\r
- i = spi_dnum_get();\r
-//PDEBUG("thread_rcv(): wakeup dnum=%d rcv_len=%d\n", i, spi_rcvd_len_get());\r
- while(i-- > 0) {\r
- memset(&ad, 0, sizeof(ad));\r
- // データ取得\r
- spi_rx_get(buf);\r
- // デコード\r
- if (decode(buf, &ad)) continue;\r
- // chekcsum check\r
- sum = sum_calc(buf);\r
- if (sum != ad.checksum) {\r
- PDEBUG("thread_rcv(): SUM ERR! CALC=%04X RCV=%04X\r\n", sum, ad.checksum);\r
- }\r
- // 平均\r
- ad.freq = conf_freq_get();\r
- do_avg(AD_SAMPLE, &ad);\r
-#if 1\r
-int ch;\r
-PDEBUG("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X,%d",\r
- ad.gps.year, ad.gps.month, ad.gps.day, ad.gps.hour, ad.gps.min, ad.gps.sec, ad.gps.nano,\r
- ad.gps.tow, ad.gps.tacc, ad.gps.valid, ad.rescode);\r
-for(ch = 0; ch < AD_CHNUM; ch++) {\r
- PDEBUG(",%+7ld", ad.data1sec[ch]);\r
-}\r
-PDEBUG("\r\n");\r
-#endif\r
-#if 0\r
-for(ch = 0; ch < AD_CHNUM; ch++) {\r
- PDEBUG(",%+7ld", ad.data[ch][0]);\r
-}\r
-PDEBUG("\r\n");\r
-for(ch = 0; ch < AD_CHNUM; ch++) {\r
- PDEBUG(",%+7ld", ad.data[ch][49]);\r
-}\r
-PDEBUG("\r\n");\r
-#endif\r
- // ADリングバッファに書き込み\r
- ad_ptr = ad_ring_get(ad_ring_write_get());\r
- *ad_ptr = ad;\r
- // 最新データ位置\r
- ad_ring_latest_set(ad_ring_write_get());\r
- // 書き込み位置+1\r
- ad_ring_write_plus();\r
-\r
- } // while((i = sub_dnum_get()) > 0) {\r
- } // if(FD_ISSET(fd_sub, &fds)) {\r
- } // while(1) {\r
- return NULL;\r
-}\r
-\r
-//\r
-/**** CUnit test\r
-*/\r
-#ifdef CUNIT\r
-#include <CUnit/CUnit.h>\r
-\r
-static void test_b3_to_long32(void)\r
-{\r
- long l;\r
- char buf[SPI_DATA_LEN+256];\r
- AdData ad;\r
- int i;\r
-\r
- buf[0] = 0x56;\r
- buf[1] = 0x34;\r
- buf[2] = 0x12;\r
- l = b3_to_long32(buf);\r
- CU_ASSERT(l == 0x00123456);\r
-\r
- buf[0] = 0x56;\r
- buf[1] = 0x34;\r
- buf[2] = 0x82;\r
- l = b3_to_long32(buf);\r
- CU_ASSERT(l == (long)0xFF823456);\r
-\r
- buf[0] = 0xFF;\r
- buf[1] = 0xFF;\r
- buf[2] = 0xFF;\r
- l = b3_to_long32(buf);\r
- CU_ASSERT(l == -1);\r
-\r
- buf[0] = '$';\r
- i = SPI_OFS_DATA;\r
- buf[i++] = 0x56;\r
- buf[i++] = 0x34;\r
- buf[i++] = 0x12;\r
- decode(buf, &ad);\r
- CU_ASSERT(ad.data[0][0] == 0x123456);\r
-\r
- buf[0] = '$';\r
- i = SPI_OFS_DATA + AD_CHNUM * AD_SAMPLE * 3;\r
- i -= 3;\r
- buf[i++] = 0x56;\r
- buf[i++] = 0x34;\r
- buf[i++] = 0x12;\r
- decode(buf, &ad);\r
- CU_ASSERT(ad.data[AD_CHNUM-1][AD_SAMPLE-1] == 0x123456);\r
-}\r
-\r
-void thread_rcv_test(CU_pSuite test_suite)\r
-{\r
-\r
- CU_add_test(test_suite, "test_b3_to_long32", test_b3_to_long32);\r
-\r
-}\r
-\r
-#endif\r
+#include <stdio.h>
+//#include <unistd.h>
+//#include <sys/ioctl.h>
+//#include <termios.h>
+#include <fcntl.h>
+#include <syslog.h>
+#include <sys/types.h>
+#include <string.h>
+
+#include "debug_print.h"
+#include "spi.h"
+#include "ad_ring.h"
+#include "conf.h"
+
+/*
+ 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->gps);
+
+ if (*ptr++ != SPI_HEAD_CHAR) return -1;
+ // Response Code
+ ad->rescode = *ptr++;
+ // Response Data
+ memcpy(ad->resdata, ptr, SPI_RESDATA_LEN);
+ ptr += SPI_RESDATA_LEN;
+ // GPS
+ gps->tow = *((int32_t*)ptr); ptr += 4;
+ gps->tacc = *((int32_t*)ptr); ptr += 4;
+ gps->nano = *((int32_t*)ptr); ptr += 4;
+ gps->year = *((int16_t*)ptr); ptr += 2;
+ gps->month = *((u_int8_t*)ptr); ptr++;
+ gps->day = *((u_int8_t*)ptr); ptr++;
+ gps->hour = *((u_int8_t*)ptr); ptr++;
+ gps->min = *((u_int8_t*)ptr); ptr++;
+ gps->sec = *((u_int8_t*)ptr); ptr++;
+ gps->valid = *((u_int8_t*)ptr); ptr++;
+ // 1sec平均
+ for(ch = 0; ch < AD_CHNUM; ch++) {
+ ad->data1sec[ch] = *((int32_t*)ptr);
+ 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
+ ad->checksum = *((u_int16_t*)ptr);
+
+ // 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;
+
+ if (gps->year == 0) return -1;
+
+ return 0;
+}
+/*
+ int freq
+ 平均化後の周波数 Hz
+*/
+static void do_avg(int freq, AdData *ad)
+{
+ int ch, i, j;
+ long add;
+ int avg_freq;
+ int avg_num;
+
+ avg_freq = conf_freq_get();
+ avg_num = AD_SAMPLE/avg_freq;
+ for(ch = 0; ch < AD_CHNUM; ch++) {
+ for(i = 0; i < avg_freq; i++) {
+ add = 0;
+ for(j = 0; j < avg_num; j++) {
+ add += ad->data[ch][i*avg_num + j];
+ }
+ ad->avg[ch][i] = add / avg_num;
+ }
+ }
+}
+static unsigned int sum_calc(char *buf)
+{
+ int i;
+ u_int8_t suma = 0;
+ u_int8_t sumb = 0;
+ u_int16_t uint_sum;
+
+ for(i = SPI_OFS_RESCODE; i < SPI_OFS_SUM; i++) {
+ suma += (u_int8_t)buf[i];
+ sumb += suma;
+ }
+ uint_sum = ((sumb << 8) & 0xFF00U) | suma;
+ return uint_sum;
+}
+
+void* thread_rcv(void* pParam)
+{
+
+ fd_set fds;
+ char buf[SPI_DATA_LEN+256];
+ int i;
+ int fd_spi;
+ AdData ad, *ad_ptr;
+ unsigned int sum;
+#if 0
+char cmd[SPI_CMD_LEN];
+char c;
+c=0;
+#endif
+ // spiドライバのリングバッファクリア
+ spi_buf_clear();
+ // spi送信コマンド長セット
+ spi_tx_len_set(SPI_CMD_LEN);
+
+ while(1) {
+ fd_spi = spi_get_fd();
+ // select の準備
+ FD_ZERO(&fds);
+ // FDセット
+ FD_SET(fd_spi, &fds);
+ // 受信を待つ タイムアウト無し
+ i = select(fd_spi + 1, &fds, NULL, NULL, NULL); // 読みselect
+ if(i <= 0) syslog(LOG_ERR, "%s: select returned with signal or error. ret=%d\n", __FUNCTION__, i);
+ if(FD_ISSET(fd_spi, &fds)) {
+#if 0
+// SPI送信データセット
+if (c++ % 5 == 0) {
+ memset(cmd, 0, SPI_CMD_LEN);
+ for(i = 0; i < AD_CHNUM; i++)
+ cmd[i] = SPI_CMD_GAIN_128;
+ spi_cmd_send(SPI_CMDCODE_GAIN, cmd, AD_CHNUM);
+}
+#endif
+ // 受信した
+ i = spi_dnum_get();
+PDEBUG("thread_rcv(): wakeup dnum=%d rcv_len=%d\n", i, spi_rcvd_len_get());
+ while(i-- > 0) {
+ memset(&ad, 0, sizeof(ad));
+ // データ取得
+ spi_rx_get(buf);
+ // デコード
+ if (decode(buf, &ad)) continue;
+ // chekcsum check
+ sum = sum_calc(buf);
+ if (sum != ad.checksum) {
+ PDEBUG("thread_rcv(): SUM ERR! CALC=%04X RCV=%04X\r\n", sum, ad.checksum);
+ }
+ // 平均
+ ad.freq = conf_freq_get();
+ do_avg(AD_SAMPLE, &ad);
+#if 1
+int ch;
+PDEBUG("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X,%d",
+ ad.gps.year, ad.gps.month, ad.gps.day, ad.gps.hour, ad.gps.min, ad.gps.sec, ad.gps.nano,
+ ad.gps.tow, ad.gps.tacc, ad.gps.valid, ad.rescode);
+for(ch = 0; ch < AD_CHNUM; ch++) {
+ PDEBUG(",%+7ld", ad.data1sec[ch]);
+}
+PDEBUG("\r\n");
+#endif
+#if 0
+for(ch = 0; ch < AD_CHNUM; ch++) {
+ PDEBUG(",%+7ld", ad.data[ch][0]);
+}
+PDEBUG("\r\n");
+for(ch = 0; ch < AD_CHNUM; ch++) {
+ PDEBUG(",%+7ld", ad.data[ch][49]);
+}
+PDEBUG("\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)) {
+ } // while(1) {
+ return NULL;
+}
+
+//
+/**** CUnit test
+*/
+#ifdef CUNIT
+#include <CUnit/CUnit.h>
+
+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