-#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
- memcpy(&(ad->checksum), ptr, SPI_CHECKSUM_LEN);\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
- 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
-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
-#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
-memset(cmd, 0, SPI_CMD_LEN);\r
-cmd[0] = SPI_HEAD_CHAR;\r
-cmd[1] = 1;\r
-cmd[2] = 2;\r
-cmd[3] = c++;\r
-spi_tx_set(cmd);\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
- decode(buf, &ad);\r
- ad.freq = conf_freq_get();\r
- // 平均 仮コード 要変更\r
- do_avg(AD_SAMPLE, &ad);\r
-#if 1\r
-int ch;\r
-PDEBUG("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X",\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);\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 <stdint.h>
+
+#include "debug_print.h"
+#include "spi.h"
+#include "ad_ring.h"
+#include "conf.h"
+
+/*
+ Little Endian
+ 3byte -> signed long(4byte)
+*/
+static int32_t 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 *((int32_t*)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;
+ }
+/*
+u_int8_t *ptrsave = ptr;
+for(i = 0; i < AD_SAMPLE; i++) {
+ *ptr++ = i;
+ *ptr++ = 0;
+ *ptr++ = 0;
+}
+ptr = ptrsave;
+*/
+ // 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;
+ }
+ }
+// DEBUG!!!!!!!!!!!!!!!!!!!!!!!
+//for(i = 0; i < AD_SAMPLE; i++) {
+// ad->data[0][i] = i;
+//}
+
+ // 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
OSDN Git Service
