thread_rcv.c

   1 #include <stdio.h>\r
   2 //#include <unistd.h>\r
   3 //#include <sys/ioctl.h>\r
   4 //#include <termios.h>\r
   5 #include <fcntl.h>\r
   6 #include <syslog.h>\r
   7 #include <sys/types.h>\r
   8 #include <string.h>\r
   9 \r
  10 #include "spi.h"\r
  11 #include "ad_ring.h"\r
  12 \r
  13 /*\r
  14         Little Endian\r
  15         3byte -> signed long(4byte)\r
  16 */\r
  17 static long b3_to_long32(unsigned char *ptr)\r
  18 {\r
  19         char    buf[4];\r
  20         \r
  21         buf[0] = *ptr++;\r
  22         buf[1] = *ptr++;\r
  23         buf[2] = *ptr;\r
  24         if (*ptr & 0x80) {\r
  25                 buf[3] = 0xFF;\r
  26         } else {\r
  27                 buf[3] = 0;\r
  28         }\r
  29         return *((long*)buf);\r
  30 }\r
  31 static int decode(char *buf, AdData *ad)\r
  32 {\r
  33         UbloxNavTimeUtc *gps;\r
  34         u_int8_t        *ptr;\r
  35         int     ch;\r
  36         int     i;\r
  37         \r
  38         ptr = (u_int8_t*)buf;\r
  39         gps = &(ad->gps);\r
  40 \r
  41         if (*ptr++ != SPI_HEAD_CHAR) return -1;\r
  42         // GPS\r
  43         gps->tow = *((int32_t*)ptr); ptr += 4;\r
  44         gps->tacc = *((int32_t*)ptr); ptr += 4;\r
  45         gps->nano = *((int32_t*)ptr); ptr += 4;\r
  46         gps->year = *((int16_t*)ptr); ptr += 2;\r
  47         gps->month = *((u_int8_t*)ptr); ptr++;\r
  48         gps->day = *((u_int8_t*)ptr); ptr++;\r
  49         gps->hour = *((u_int8_t*)ptr); ptr++;\r
  50         gps->min = *((u_int8_t*)ptr); ptr++;\r
  51         gps->sec = *((u_int8_t*)ptr); ptr++;\r
  52         gps->valid = *((u_int8_t*)ptr); ptr++;\r
  53         // 1sec平均\r
  54         for(ch = 0; ch < AD_CHNUM; ch++) {\r
  55                 ad->data1sec[ch] = *((int32_t*)ptr);\r
  56                 ptr += 4;\r
  57         }\r
  58         // 50Hz data\r
  59         for(ch = 0; ch < AD_CHNUM; ch++) {\r
  60                 for(i = 0; i < AD_SAMPLE; i++) {\r
  61                         ad->data[ch][i] = b3_to_long32(ptr);\r
  62                         ptr += 3;\r
  63                 }\r
  64         }\r
  65         // Checksum\r
  66         \r
  67         // gps->struct tm\r
  68         ad->t.tm_year = gps->year - 1900;\r
  69         ad->t.tm_mon = gps->month - 1;  // struct tmの月は(0〜11)なので注意\r
  70         ad->t.tm_mday = gps->day;\r
  71         ad->t.tm_hour = gps->hour;\r
  72         ad->t.tm_min = gps->min;\r
  73         ad->t.tm_sec = gps->sec;\r
  74 \r
  75         return 0;\r
  76 }\r
  77 /*\r
  78         int freq\r
  79                 平均化後の周波数 Hz\r
  80 */\r
  81 static void do_avg(int freq, AdData *ad)\r
  82 {\r
  83 // 仮コード\r
  84         memcpy(ad->avg, ad->data, AD_CHNUM*AD_SAMPLE*AD_BYTES);\r
  85 }\r
  86 void* thread_rcv(void* pParam)\r
  87 {\r
  88         while(1) {\r
  89                 fd_set  fds;\r
  90                 char    buf[SPI_DATA_LEN+256];\r
  91                 int     i;\r
  92 //              int     ch;\r
  93                 int     fd_spi;\r
  94                 AdData  ad, *ad_ptr;\r
  95 \r
  96                 fd_spi = spi_get_fd();\r
  97                 // select の準備\r
  98                 FD_ZERO(&fds);\r
  99                 // FDセット\r
 100                 FD_SET(fd_spi, &fds);\r
 101                 // 受信を待つ　タイムアウト無し\r
 102                 i = select(fd_spi + 1, &fds, NULL, NULL, NULL);    // 読みselect\r
 103                 if(i <= 0) syslog(LOG_ERR, "%s: select returned with signal or error. ret=%d\n", __FUNCTION__, i);\r
 104                 if(FD_ISSET(fd_spi, &fds)) {\r
 105                         // 受信した\r
 106                         i = spi_dnum_get();\r
 107 //printf("thread_rcv(): wakeup dnum=%d rcv_len=%d\n", i, spi_rcvd_len_get());\r
 108                         while(i-- > 0) {\r
 109                                 // バッファ書き込み位置get\r
 110 //                              d = ring_get(ring_write_get());\r
 111 //                              ring_zero();\r
 112                                 // データ取得\r
 113                                 spi_rx_get(buf);\r
 114                                 // デコード\r
 115                                 decode(buf, &ad);\r
 116                                 // 平均　仮コード　要変更\r
 117                                 do_avg(AD_SAMPLE, &ad);\r
 118 #if 0\r
 119 printf("%04d/%02d/%02d %02d:%02d:%02d.%09ld,%6lums,%6luns,%02X",\r
 120         ad.t.year, ad.t.month, ad.t.day, ad.t.hour, ad.t.min, ad.t.sec, ad.t.nano,\r
 121         ad.t.tow, ad.t.tacc, ad.t.valid);\r
 122 for(ch = 0; ch < AD_CHNUM; ch++) {\r
 123         printf(",%+7ld", ad.data1sec[ch]);\r
 124 }\r
 125 printf("\r\n");\r
 126 for(ch = 0; ch < AD_CHNUM; ch++) {\r
 127         printf(",%+7ld", ad.data[ch][0]);\r
 128 }\r
 129 printf("\r\n");\r
 130 for(ch = 0; ch < AD_CHNUM; ch++) {\r
 131         printf(",%+7ld", ad.data[ch][49]);\r
 132 }\r
 133 printf("\r\n");\r
 134 #endif\r
 135                                 // ADリングバッファに書き込み\r
 136                                 ad_ptr = ad_ring_get(ad_ring_write_get());\r
 137                                 *ad_ptr = ad;\r
 138                                 // 最新データ位置\r
 139                                 ad_ring_latest_set(ad_ring_write_get());\r
 140                                 // 書き込み位置+1\r
 141                                 ad_ring_write_plus();\r
 142 \r
 143                         } // while((i = sub_dnum_get()) > 0) {\r
 144                 } // if(FD_ISSET(fd_sub, &fds)) {\r
 145         } //    while(1) {\r
 146         return NULL;\r
 147 }\r
 148 \r
 149 //\r
 150 /**** CUnit test\r
 151 */\r
 152 #ifdef CUNIT\r
 153 #include <CUnit/CUnit.h>\r
 154 \r
 155 static void test_b3_to_long32(void)\r
 156 {\r
 157         long    l;\r
 158         char    buf[SPI_DATA_LEN+256];\r
 159         AdData  ad;\r
 160         int     i;\r
 161 \r
 162         buf[0] = 0x56;\r
 163         buf[1] = 0x34;\r
 164         buf[2] = 0x12;\r
 165         l = b3_to_long32(buf);\r
 166         CU_ASSERT(l == 0x00123456);\r
 167 \r
 168         buf[0] = 0x56;\r
 169         buf[1] = 0x34;\r
 170         buf[2] = 0x82;\r
 171         l = b3_to_long32(buf);\r
 172         CU_ASSERT(l == (long)0xFF823456);\r
 173 \r
 174         buf[0] = 0xFF;\r
 175         buf[1] = 0xFF;\r
 176         buf[2] = 0xFF;\r
 177         l = b3_to_long32(buf);\r
 178         CU_ASSERT(l == -1);\r
 179 \r
 180         buf[0] = '$';\r
 181         i = SPI_OFS_DATA;\r
 182         buf[i++] = 0x56;\r
 183         buf[i++] = 0x34;\r
 184         buf[i++] = 0x12;\r
 185         decode(buf, &ad);\r
 186         CU_ASSERT(ad.data[0][0] == 0x123456);\r
 187 \r
 188         buf[0] = '$';\r
 189         i = SPI_OFS_DATA + AD_CHNUM * AD_SAMPLE * 3;\r
 190         i -= 3;\r
 191         buf[i++] = 0x56;\r
 192         buf[i++] = 0x34;\r
 193         buf[i++] = 0x12;\r
 194         decode(buf, &ad);\r
 195         CU_ASSERT(ad.data[AD_CHNUM-1][AD_SAMPLE-1] == 0x123456);\r
 196 }\r
 197 \r
 198 void thread_rcv_test(CU_pSuite test_suite)\r
 199 {\r
 200 \r
 201         CU_add_test(test_suite, "test_b3_to_long32", test_b3_to_long32);\r
 202 \r
 203 }\r
 204 \r
 205 #endif\r