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