示例#1
0
static void recordPulse( unsigned n, unsigned rise, unsigned drop, unsigned end,
			 unsigned cw, unsigned ccw, unsigned crazy, unsigned terminal)
{
    static struct ook_burst *burst = 0;
    unsigned hiLen = drop-rise;
    unsigned lowLen = end-drop;
    
    // The frequency calculation could be a lot better. There is a lot of noise
    // in there which leads to misinterpretations of cw and ccw. There is a significant
    // variance in the pulse to pulse results of the same transmitter.
    float cycles = ((int)cw-(int)ccw)/4.0;
    if ( cycles > 0) cycles += crazy/2.0;  // figure we are going fast enough to sometimes skip
    if ( cycles < 0) cycles -= crazy/2.0;  // .. might ought to check that.
    float frequency = cycles/(hiLen/(float)sampleRate);

    if ( !burst) {
	burst = ook_allocate_burst(512*8);     // first pulse of new burst
	if ( !burst) {
	    fprintf(stderr,"Failed to allocate burst\n");
	    exit(-1);
	} else {
	    burst->positionNanoseconds = samplesToNs( sampleCounter + rise);
	}
    }

    if ( ook_add_pulse(burst, samplesToNs(hiLen), samplesToNs(lowLen), lrint(frequency))) {
	fprintf(stderr,"Failed to add pulse to burst! Too long?\n");
    }

    if ( terminal) {
	if ( burst) {
	    if ( burst->pulses > minPacket) {
		void *data=0;
		size_t len;
		if ( ook_encode( burst, &data, &len) != 0 || data == 0) {
		    fprintf(stderr, "Failed to encode a pulse burst.\n");
		} else {
		    int e = sendto( multicastSocket, data, len, 0, multicastSockaddr, multicastSockaddrLen);
		    if ( e < 0) {
			fprintf(stderr, "Failed to multicast pulse (%zu bytes): %s\n", len, strerror(errno));
		    }
		    if ( verbose) fprintf(stderr,"Multicast %u pulse, %zu bytes\n", burst->pulses, len);
		    
		    free(data);
		}
	    } else {
		if ( verbose) fprintf(stderr,"Skipped run burst of %d pulses\n", burst->pulses);
	    }

	    free(burst);
	    burst = 0;
	}
    }
}
示例#2
0
文件: ook.c 项目: dbrooke/ook-decoder
int ook_open( const char *address, const char *port, const char *interface)
{
    int sock = -1;
    struct addrinfo *multicast_ai = 0;
    struct addrinfo *interface_ai = 0;
    struct addrinfo hints = { .ai_family = AF_UNSPEC,
			      .ai_socktype = SOCK_DGRAM,
    };

    int err = getaddrinfo( address, port, &hints, &multicast_ai);
    if (err){
	fprintf(stderr,"Illegal multicast address (addr=%s port=%s):%s\n", address, port, gai_strerror(err));
	goto Fail;
    }

    err = getaddrinfo( interface, port, &hints, &interface_ai);
    if (err){
	fprintf(stderr,"Illegal interface address (addr=%s port=%s):%s\n", interface, port, gai_strerror(err));
	goto Fail;
    }

    // add a verbose print here
	
    sock = socket( interface_ai->ai_family, SOCK_DGRAM, 0);
    if ( sock < 0) {
	fprintf(stderr,"Failed to create socket: %s\n", strerror(errno));
	goto Fail;
    }

    int reuse=1;
    if ( setsockopt( sock, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)) < 0) {
	fprintf(stderr, "Failed to reuse socket: %s\n", strerror(errno));
	goto Fail;
    }

    switch( multicast_ai->ai_family) {
      case AF_INET:
	  {
	      struct sockaddr_in anySock = { .sin_family = AF_INET,
					     .sin_port = ((struct sockaddr_in *)multicast_ai->ai_addr)->sin_port,
					     .sin_addr.s_addr = INADDR_ANY };
	      if ( bind( sock, (struct sockaddr *)&anySock, sizeof(anySock)) < 0) {
		  fprintf(stderr,"Failed to bind to multicast interface: %s\n", strerror(errno));
		  goto Fail;
	      }

	      struct ip_mreq group;
	      group.imr_multiaddr.s_addr = ((struct sockaddr_in *)multicast_ai->ai_addr)->sin_addr.s_addr;
	      group.imr_interface.s_addr = ((struct sockaddr_in *)interface_ai->ai_addr)->sin_addr.s_addr;

	      int r = setsockopt( sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const void *)&group, sizeof(group));
	      if ( r < 0) {
		  fprintf(stderr,"Failed to join multicast group: %s\n", strerror(errno));
		  goto Fail;
	      }
	  }
	  break;
      case AF_INET6:
	  {
	      struct sockaddr_in6 anySock = { .sin6_family = AF_INET6,
					      .sin6_port = ((struct sockaddr_in6 *)multicast_ai->ai_addr)->sin6_port,
					      .sin6_addr = in6addr_any };
	      if ( bind( sock, (struct sockaddr *)&anySock, sizeof(anySock)) < 0) {
		  fprintf(stderr,"Failed to bind to multicast interface: %s\n", strerror(errno));
		  goto Fail;
	      }

	      struct ipv6_mreq group;
	      memcpy( &group.ipv6mr_multiaddr, &((struct sockaddr_in6 *)multicast_ai->ai_addr)->sin6_addr, sizeof(struct in6_addr));
	      group.ipv6mr_interface = 0;
		  
	      int r = setsockopt( sock, IPPROTO_IPV6, IPV6_JOIN_GROUP, (const void *)&group, sizeof(group));
	      if ( r < 0) {
		  fprintf(stderr,"Failed to join multicast group: %s\n", strerror(errno));
		  goto Fail;
	      }
	  }
	  break;
      default:
	fprintf(stderr, "Unsupported family for multicast groups: %d\n", multicast_ai->ai_family);
	goto Fail;
	break;
    }

    freeaddrinfo( multicast_ai);
    freeaddrinfo( interface_ai);
    
    return sock;

  Fail:
    if ( multicast_ai) freeaddrinfo( multicast_ai);
    if ( interface_ai) freeaddrinfo( interface_ai);
    if ( sock >= 0) close(sock);

    return -1;
}


int ook_decode_from_socket( int sock, struct ook_burst **burstReturn, struct sockaddr *from, socklen_t *fromLen, int verbose)
{
    struct ook_burst *burst = 0;
    unsigned char buf[65536];
    int e=0;

    do {
	e = recvfrom( sock, buf, sizeof(buf), 0, from, fromLen);
	if ( e == -1 && (errno == EAGAIN || errno == EINTR)) continue;
	if ( e == -1) return -1;
    } while(0);

    if ( verbose) fprintf(stderr,"Received %u bytes\n", e);

    uint32_t left = e;
    unsigned char *thumb = buf;

#define OGET_U32() ({ uint32_t v; if ( left<4) goto Fail; memcpy(&v,thumb,4); thumb+=4; left -= 4; v; })
#define OGET_I32() ({ int32_t v; if ( left<4) goto Fail; memcpy(&v,thumb,4); thumb+=4; left -= 4; v; })
#define OGET_U64() ({ uint64_t v; if ( left<8) goto Fail; memcpy(&v,thumb,8); thumb+=8; left -= 8; v; })

    uint32_t vers = OGET_U32();
    if ( vers != 0x36360001) goto Fail;

    uint64_t pos = OGET_U64();
    uint32_t pulses = OGET_U32();

    burst = ook_allocate_burst( pulses);
    if ( !burst) goto Fail;

    burst->positionNanoseconds = pos;
    for ( int i = 0; i < pulses; i++) {
	uint32_t hi = OGET_U32();
	uint32_t low = OGET_U32();
	int32_t freq = OGET_I32();

	if ( ook_add_pulse( burst, hi, low, freq) < 0) goto Fail;
    }

    if ( left > 0) goto Fail;

    *burstReturn = burst;

    return 1;
    
  Fail:
    *burstReturn = 0;
    if (burst) free(burst);

    return 0;
}

int ook_decode_pulse_width( struct ook_burst *burst, 
			    uint32_t minZeroHi, uint32_t maxZeroHi, 
			    uint32_t minOneHi, uint32_t maxOneHi, 
			    uint32_t minLow, uint32_t maxLow, 
			    unsigned char **dataReturn, size_t *dataLenReturn,
			    int verbose)
{
    size_t dataLen = (burst->pulses + 7)/8;
    unsigned char *data = (unsigned char *)malloc( dataLen);
    if ( data == 0) goto Fail;

    unsigned char accum = 0;
    unsigned char *thumb = data;
    unsigned char bitsInAccum = 0;
    unsigned bits = 0;
    for ( int i = 0; i < burst->pulses; i++) {
	uint32_t hi = burst->pulse[i].hiNanoseconds;
	uint32_t low = burst->pulse[i].lowNanoseconds;

	if ( low < minLow || low > maxLow) {
	    if ( verbose) fprintf(stderr,"low of %u was %u, not between %u and %u\n", i, low, minLow, maxLow);
	    goto Fail;
	}

	if ( hi >= minZeroHi && hi <= maxZeroHi) {
	    accum = (accum<<1);
	    bitsInAccum++;
	    bits++;
	} else if ( hi >= minOneHi && hi <= maxOneHi) {
	    accum = ((accum<<1)|1);
	    bitsInAccum++;
	    bits++;
	} else {
	    if ( verbose) fprintf(stderr,"high of %u was %u, not between %u and %u or %u and %u\n", 
				  i, hi, minZeroHi, maxZeroHi, minOneHi, maxOneHi);
	    goto Fail;
	}

	if ( bitsInAccum == 8) {
	    *thumb++ = accum;
	    bitsInAccum = 0;
	}
    }
    if ( bitsInAccum != 0) {
	*thumb++ = accum;
	bitsInAccum = 0;
    }

    *dataReturn = data;
    *dataLenReturn = dataLen;
    return bits;

  Fail:
    if ( data) free(data);
    return -1;
}