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