thread_rcv.c

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