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