コード例 #1
0
void sendAllMapData()
{
	for(uint8_t i = 0; i < 16; ++i)
	{
		for(uint8_t j = 0; j < 16; ++j)
		{
			if(glob_map[j][i] != UNKNOWN)
			{
				cbWrite(&glob_mapDataToSend, i);
				cbWrite(&glob_mapDataToSend, j);
			}
		}
	}
}
コード例 #2
0
ファイル: fifo3.c プロジェクト: spo0lsh/c_learn
int main() {
	
	CircularBuffer cb;
	int *value;
	value = (int *)0; // cast must be
	
	printf("[ ] %d %0x\n", (int)value,(int)value); // cast must be
	
	cbInit(&cb);

	int i;
	for(i=0;i<20;++i)
	{
		value = (int *)i;
		printf("[T] %d %0x\n", (int)value,(int)value); // cast must be
		if(! cbIsFull(&cb)) { /* if not full */
			cbWrite(&cb, (int)value);
		} 
	}

	while(!cbIsEmpty(&cb)) 
	{
		cbRead(&cb, (int*)&value);
		printf("[P] %d %0x\n", (int)value,(int)value); // cast must be
	}
	
	return(0);
}
コード例 #3
0
ファイル: fifo2.c プロジェクト: spo0lsh/c_learn
int main() {
	CircularBuffer cb; /* */
	ElemType elem = {0};
    
    int testBufferSize = 10; /* arbitrary size */
    cbInit(&cb, testBufferSize);
	
    /* Fill buffer with test elements 3 times */
    for (elem.value = 0; elem.value < 3 * testBufferSize; ++ elem.value)
    {
		if(! cbIsFull(&cb)) { /* if not full */
			cbWrite(&cb, &elem);
		} 
	}

    /* Remove and print all elements */
    while (!cbIsEmpty(&cb)) {
        cbRead(&cb, &elem);
        printf("%d\n", elem.value);
    }
 
    cbFree(&cb);
	
	return(0);
}
コード例 #4
0
ファイル: ring_buffer.c プロジェクト: ulyssesrr/carmen_lcad
/* Write an element, overwriting oldest element if buffer is full. App can
   choose to avoid the overwrite by checking cbIsFull(). */
void cbWriteBlock(CircularBuffer *cb, short *elem, int length)
{
	int i;

	for(i = 0; i < length; i++)
		cbWrite(cb, elem[i]);
}
コード例 #5
0
ファイル: interrupt.c プロジェクト: frankicia/zeOS
void RSI_KEYBOARD(){
	unsigned char key = inb(0x60);
	unsigned char mask = 0b10000000;
	unsigned char make = key & mask;
	char c = '0';
	if(make == 0) {
		mask = 0b01111111;
		c = char_map[key & mask];
		if(!(c>=0x20 && c<=0x7E))
			c = 'C';

		cbWrite(&keybuffer, &c);

		//MIRAR
		if (!(cbIsFull(&keybuffer) && list_empty(&keyboardqueue))) {
		
			if(!list_empty(&keyboardqueue)) {
				struct list_head * to_unblock;
				struct task_struct * pcb_to_unlock;
				
				to_unblock = list_first(&keyboardqueue);
				pcb_to_unlock = list_head_to_task_struct(to_unblock);
				
				if ((cbIsFull(&keybuffer) || pcb_to_unlock->characters_left <= cbCount(&keybuffer))) {
						list_del(to_unblock);
						list_add_tail(to_unblock, &readyqueue);
				}
			}
		}
	}
	else
		c = ' ';
		
	printc_xy(0x3f, 0x15, c);
}
コード例 #6
0
uint8_t Api_add_cmd_to_cb(void* api_ptr) {
	struct ApiHandlerVars* handler = global_api_handler(0);
	ElemType elem;
    elem.value = api_ptr;

    cbWrite(&(handler->tx_buffer), &elem);
    return 1;
}
コード例 #7
0
ファイル: sc_io.cpp プロジェクト: DowerChest/codeblocks
 bool WriteFileContents(const wxString& filename, const wxString& contents)
 {
     wxFileName fname(Manager::Get()->GetMacrosManager()->ReplaceMacros(filename));
     NormalizePath(fname, wxEmptyString);
     if (!SecurityAllows(_T("CreateFile"), fname.GetFullPath()))
         return false;
     wxFile f(fname.GetFullPath(), wxFile::write);
     return cbWrite(f, contents);
 }
コード例 #8
0
ファイル: cbuffer.c プロジェクト: Spacerat/PrimerWare
__attribute__((section(".rodata"))) int main( void ) {
	CircularBuffer buff;
	cbInit(&buff, 10);
	cbWrite(&buff, 3);
	cbWrite(&buff, 23);
	cbWrite(&buff, 43);	
	buff_t dat;
	cbRead(&buff, &dat);
	printf("%d\n", dat);

	cbRead(&buff, &dat);
	printf("%d\n", dat);

	cbRead(&buff, &dat);
	printf("%d\n", dat);

	printf("%d\n", cbIsEmpty(&buff));
	return 0;
}
コード例 #9
0
void printp(const char *fmt, ...) {
	int count = 0;
	int i;
	va_list args;
	spin_lock(&cb_spinlock);
	va_start(args, fmt);
	count += vsnprintf(printproc_buf, 1024, fmt, args);
	for (i = 0; i<count; i++) {
		cbWrite(&cb, &printproc_buf[i]);
	}
	va_end(args);
	spin_unlock(&cb_spinlock);
}
コード例 #10
0
ファイル: circular.c プロジェクト: jpierre03/libs
int main(int argc, char **argv) {
	CircularBuffer cb;
	ElemType elem = {0};

	int testBufferSize = 50; /* arbitrary size */
	cbInit(&cb, testBufferSize);

	/* Fill buffer with test elements 3 times */
	for (elem.value = 0; elem.value < 3 * testBufferSize; ++ elem.value){
		cbWrite(&cb, &elem);
	}

	/* Remove and print all elements */
	while (!cbIsEmpty(&cb)) {
		cbRead(&cb, &elem);
		printf("%d\n", elem.value);
	}

	cbFree(&cb);
	return 0;
}
コード例 #11
0
void * stdio_thread_func(void * ptr) {
	char command [10];
	int delaytime;
	
	while (run) {
		printf("> ");
		delaytime = 0;
		scanf("%s %d", command, &delaytime);
		
		if (strcmp("l", command) == 0) {
			cbWrite(&commands, goleft);
			printf("Go left\n");
		}
		else if (strcmp("r", command) == 0) {
			cbWrite(&commands, goright);
			printf("Go right\n");
		}
		else if (strcmp("f", command) == 0) {
			cbWrite(&commands, goforward);
			printf("Go forward\n");
		}
		else if (strcmp("b", command) == 0) {
			cbWrite(&commands, goback);
			printf("Go backward\n");
		}
		else if (strcmp("q", command) == 0) {
			cbWrite(&commands, goquit);
		}
		
		if (delaytime > 0) {
			cbWrite(&commands, gowait);
			cbWrite(&commands, delaytime);
		}
		
		printf("\n");
	}
}
コード例 #12
0
ファイル: uarts.c プロジェクト: zzattack/radiocontroller
void U2puts(const char* msg) {
    if (uart2BlockMs > 0) return;
    while (*msg != '\0')
        cbWrite(&cbUart2Tx, *msg++);
}
コード例 #13
0
ファイル: kestrel2ivy.c プロジェクト: 2seasuav/paparuzzi
/**
 * Send an 'S' command (read sensor data) to the station and get the packet back.
 */
BOOL send_S() {

  char cmd[CMD_LENGTH] = "S\r";

  int bytes = write(fd, cmd, strlen(cmd));
  if (bytes == -1) {
    perror("kestrel2ivy: Error writing command bytes");
    fprintf(stderr,"You may need to reset the connection.\n");
  }

  bzero(tempBuf, BUF_LENGTH);

  // repeat a few times within time limit, just return if done
  int result = 0;
  int j = 0;
  for(j = 0; j < 3; j++) {
    // delay to allow kestrel to give some data
    usleep(250000); // 0.25sec
    // read in one chunk, skip reads for the period if there is a read error
    if (result >= 0) {
      result = read(fd, tempBuf, cbAvailable(&cb));
      if (result < 0) {
        //perror("kestrel2ivy: Got a read error on the port");
      } else {
        int i = 0;
        for (i = 0; i < result; i++) {
          ElemType elemBuf;
          elemBuf.value = tempBuf[i];
          cbWrite(&cb, &elemBuf);
        }
      }
    }

    while (!cbIsEmpty(&cb)) {
      // First get the (approximate) start indicator
      // Then copy in the rest of the data
      // Stop copying in data at the (approximate) end indicator
      // Return if it looks like the last packet in the buffer, otherwise keep processing buffer
      ElemType tempElem;
      cbRead(&cb,&tempElem);
      char tempChar = tempElem.value;

      switch (msgState) {
        case UNINIT:
          if (tempChar == '>') {
            bzero(packet, PACKET_LENGTH); // clear the packet after getting values
            packetidx = 0;
            packet[packetidx++] = tempChar;
            msgState = GOT_START;
          }
          break;
        case GOT_START:
          if (tempChar == '>') {
            if (packetidx < MIN_PACKET_LENGTH) {
              // False alarm, not actually the start
              bzero(packet, PACKET_LENGTH);
              packetidx = 0;
              packet[packetidx++] = tempChar;
            } else {
              msgState = UNINIT;
              // if we think we have another full packet available, don't return
              if (cbAvailable(&cb) > (BUF_LENGTH - (MIN_PACKET_LENGTH + 80)))
                return TRUE;
            }
          }
          else {
            packet[packetidx++] = tempChar;
          }
          break;
        default:
          fprintf(stderr, "kestrel2ivy: Unknown parser state!\n");
      }
    }
  }
  return FALSE;
}
コード例 #14
0
ファイル: network.c プロジェクト: jhosen/elevator
//#include <termios.h>
void *com_handler(void * peer){
	struct peer* pinf = (struct peer*) peer;
	struct peer p;
	p.socket = pinf->socket; // creating a local copy of the peer object
	p.ip = pinf->ip;
	struct peer * pp = nw_get(p);

	printf("Peer connected (ip %lu socket %i).\n", pinf->ip, pinf->socket);

	struct msg newpeermsg = {.msgtype = OPCODE_NEWPEER,.from	 = p.ip,};
	handle_msg(newpeermsg, 0);

	char recv_msg[MAXRECVSIZE];
	int read_size;
	struct timeval ctime, ptime, ttime;
	gettimeofday(&ttime,0);

	/* Set non-blocking state */
	int flags;
	if (-1 == (flags = fcntl(pinf->socket, F_GETFL, 0))){
		flags = 0;
	}
	fcntl(p.socket, F_SETFL, flags | O_NONBLOCK);

	char * string 	= calloc(MAXRECVSIZE,1);
	char * cjsonstr = calloc(MAXRECVSIZE,1);
	/** Normal operating mode **/
	while(1){
		/** Maintain connection by passing and receiving I'm alive **/
		gettimeofday(&ctime, 0);
		if((ctime.tv_usec - ptime.tv_usec) >= IMALIVE_UPPERIOD || (ctime.tv_usec<ptime.tv_usec)){
			struct msg packet = { .msgtype = OPCODE_IMALIVE,};
			char * cout  = pack(packet);
			send(p.socket, cout, strlen(cout), 0);
			gettimeofday(&ptime, 0);
		}

		/** Receive data **/
		read_size = recv(pinf->socket, recv_msg, MAXRECVSIZE, 0);
		if(read_size <= 0){ // ERROR/RECOVERY mode
				if(read_size == 0){
						printf("Peer disconnected (ip %lu socket %i).\n", pinf->ip, pinf->socket);
						break;
				}
				else if((read_size==-1) ){
					if( (errno != EAGAIN) && (errno != EWOULDBLOCK) ) {
						printf("Peer disconnected (ip %lu socket %i).\n", pinf->ip, pinf->socket);
						break;
					}
				}
				else{
					0;// Did not receive anything, but no error
				}
		}
		else { // Received something
			strcpy(string, recv_msg);
			string[read_size]='\0';

			int start_i = 0;
			int end_i = cjsonendindex(string, start_i);
			/* Separate packets arriving together */
			while(end_i<(strlen(string)-1)){
				strncpy(cjsonstr, (string+start_i), (end_i-start_i+1));
				cjsonstr[end_i-start_i+1] = '\0';
				struct msg packetin = unpack(cjsonstr);
				packetin.from = p.ip;
				handle_msg(packetin, &ttime);
				start_i = end_i+1;
				end_i = cjsonendindex(string, start_i);
			}
			if(end_i!=-1){
				strncpy(cjsonstr, (string+start_i), (end_i-start_i+1));
				cjsonstr[end_i-start_i+1] = '\0';
				struct msg packetin = unpack(cjsonstr);
				packetin.from = p.ip;
				handle_msg(packetin, &ttime);
			}
		}
//		tcflush(p.socket, TCIOFLUSH);
		/** Check for timeout **/
		gettimeofday(&ctime, 0);
		if((ctime.tv_sec-ttime.tv_sec) > IMALIVE_TIMEOUT){
			printf("Timeout on I'm alive, socket: %i\n", pinf->socket);
			break;
		}
		/** Send data **/
		struct msg elem;
		while(!cbIsEmpty(&pp->bufout)){  // Send data from buffer
			 cbRead(&pp->bufout, &elem);
			 if (elem.msgtype!=OPCODE_CORRUPT){
				 char * cout  = pack(elem);
				 send(pinf->socket, cout, strlen(cout), 0);
				 free(cout);
			 }
		}
	}

	/** Recovery mode: **/
	free(cjsonstr);
	free(string);
	nw_rm(p);
    nw_setevent(DISCONNECTION);
	struct msg recovermsg = { .from = p.ip, .to	= highest_ip()};
	if(recovermsg.to == root->p.ip){ // I have the highest ip on the network and should take over orders
		recovermsg.msgtype = OPCODE_PEERLOSTTAKEOVER;
	}
	else{
		recovermsg.msgtype = OPCODE_PEERLOST;
	}
	handle_msg(recovermsg, 0);
	close(p.socket);
   	pthread_exit(0);
}


int sendtoallpeer(struct msg package){
	struct peer p = {
			.ip = TOALLIP
	};
	return sendtopeer(package, p);

}

int sendtopeer(struct msg package, struct peer p){
	struct nw_node * iter;
	iter = root;
	iter = iter->next;

	while(iter!=0){
		if((iter->p.ip) == p.ip || p.ip == TOALLIP){
			struct peer * pp = nw_get(iter->p);
			cbWrite(&pp->bufout, &package);
		}
		iter = iter->next;
	}
	return 1;
}

int connect_to_peer(in_addr_t peer_ip){
	int peer_socket = socket(AF_INET, SOCK_STREAM, 0);

	struct sockaddr_in peer;
	peer.sin_family			= AF_INET;
	peer.sin_addr.s_addr	= peer_ip;
	peer.sin_port			= htons(LISTEN_PORT); // Connect to listen port.
	if (connect(peer_socket, (struct sockaddr *)&peer , sizeof(peer)) < 0){
		perror("err: connect. Connecting to peer failed\n");
		return -1;
	}
	struct peer p = peer_object(peer_socket, peer_ip);
	nw_add(p);
	assign_com_thread(p);
	return 1;
}


void assign_com_thread(struct peer p){
	// We have a connection! Now assign a communication handler thread.
	struct peer *pinf = malloc(sizeof(struct peer));
	pinf->socket 	= p.socket;
	pinf->ip		= p.ip;
	if( pthread_create( &pinf->com_thread , NULL ,  com_handler , pinf)<0){
		perror("err: pthread_create\n");
		exit(1);
	}
}



/* \!brief Listen for TCP connections and accept incoming.
 *
 */
void *listen_tcp(){
	int opt = TRUE;
	struct sockaddr_in listen_addr;

	listen_addr.sin_family = AF_INET;
	listen_addr.sin_port = htons(LISTEN_PORT);
	listen_addr.sin_addr.s_addr = htons(INADDR_ANY); // Binding local IP-address

	listen_socket = socket(AF_INET, SOCK_STREAM, 0);
	if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt)) == -1){
		perror("err: setsockopt\n");
		exit(1);
	}
	if ( bind(listen_socket, (struct sockaddr *)&listen_addr, sizeof listen_addr) == -1){
		perror("err: bind\n");
		exit(1);
	}

	struct sockaddr_in peer;
	int new_peer_socket, structsize;
	structsize = sizeof(struct sockaddr_in);

	while(1){
		listen(listen_socket, LISTEN_BACKLOG);
		if ( (new_peer_socket = accept(listen_socket, (struct sockaddr *)&peer, (socklen_t*)&structsize)) == -1 ){
			perror("err: accept\n");
			exit(1);
		}
		struct peer newpeer = peer_object(new_peer_socket, peer.sin_addr.s_addr);
		if(!nw_find(newpeer)){
			nw_add(newpeer);
			assign_com_thread(newpeer);
            nw_setevent(CONNECTION);
		}
		else{ // Already in connected list
			close(new_peer_socket);
		}
	}
}
コード例 #15
0
int main(int argc, char *argv[]){
	write_uart_error_ptr = &write_uart_error;  //initialize the function pointer to write error
	write_udp_error_ptr = &write_udp_error; 
	write_decode_error_ptr = &write_decode_error;  
	write_log_error_ptr = &write_log_error;  
 
	//parse arguments
	if(argc == 4){
		//first argument is always name of program or empty string
		connection.server_ip=argv[1];
		connection.port_number_lisa_to_pc=atoi(argv[2]);
		connection.port_number_pc_to_lisa=atoi(argv[3]);
	}else{
			printf("wrong parameters: server ip - send port number - receive port number\n");
			exit(EXIT_FAILURE);
	}

	//init log (mount sd card if necessary)

	int err = init_log();
	LOG_err_handler(err,write_log_error_ptr);

	if(err != LOG_ERR_NONE){
		exit(EXIT_FAILURE);		//mounting SD card failed
	}
	
	enable_ptp();

	#if LOGGING > 0
	//init circular data log buffers
	 cbInit(cb_read_lisa, CBSIZE);
	 cbInit(cb_write_lisa, CBSIZE);
	 cbInit(cb_read_ground, CBSIZE);
	 cbInit(cb_write_ground, CBSIZE);
	 #endif

	err = serial_port_setup();
	UART_err_handler(err,write_uart_error_ptr);
	if(err != UART_ERR_NONE){
		exit(EXIT_FAILURE);
	}

	//thread variables
	pthread_t thread_lisa_to_pc,thread_data_logging_lisa,thread_data_logging_ground;

	//create a second thread which executes lisa_to_pc
	if(pthread_create(&thread_lisa_to_pc, NULL, lisa_to_pc,NULL)) {
		error_write(FILENAME,"error creating lisa thread");
		exit(EXIT_FAILURE);
	}

	#if LOGGING > 0

	//create a third thread which executes data_logging_lisa
	if(pthread_create(&thread_data_logging_lisa, NULL, data_logging_lisa,NULL)) {
		error_write(FILENAME,"error creating lisa logging thread");
		exit(EXIT_FAILURE);
	}

	//create a fourth thread which executes data_logging_groundstation
	if(pthread_create(&thread_data_logging_ground, NULL, data_logging_groundstation,NULL)) {
		error_write(FILENAME,"error creating groundstation logging thread");
		exit(EXIT_FAILURE);
	}

	#endif
	/*-------------------------START OF FIRST THREAD: PC TO LISA------------------------*/
	static UDP udp_server;
	uint8_t input_stream[OUTPUT_BUFFER];

	ElemType cb_elem = {0};

	//init the data decode pointers
	init_decoding();

	UDP_err_handler(openUDPServerSocket(&udp_server,connection.port_number_pc_to_lisa,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);

	while(1){

		//1. retreive UDP data form PC from ethernet port.
		err=receiveUDPServerData(&udp_server,(void *)&input_stream,sizeof(input_stream)); //blocking !!!
		UDP_err_handler(err,write_udp_error_ptr);

		if(err==UDP_ERR_NONE){

			#if LOGGING > 0
			
			if(!cbIsFull(cb_write_ground)){
				 memcpy (&cb_elem.value, &input_stream, sizeof(input_stream));
				 cbWrite(cb_write_ground, &cb_elem);
			 }else{
				if(reading_flag_ground==0){
					switch_cb_ground_pointers();
				}else{
					printf("GROUND WRITE WAS NOT READY \n");
				}
			 }
			
			#endif

			int new_length = strip_timestamp(input_stream); //lisa expects a package without a timestamp

			UART_err_handler(serial_port_write(input_stream,new_length),write_uart_error_ptr);
		}

	}
	UART_err_handler(serial_port_close(),write_uart_error_ptr);
	UDP_err_handler(closeUDPServerSocket(&udp_server),write_udp_error_ptr);
	/*------------------------END OF FIRST THREAD------------------------*/


	//wait for the second thread to finish
	if(pthread_join(thread_lisa_to_pc, NULL)) {
		error_write(FILENAME,"error joining thread_lisa_to_pc");
	}

	#if LOGGING > 0

	//wait for the third thread to finish
	if(pthread_join(thread_data_logging_lisa, NULL)) {
		error_write(FILENAME,"error joining thread_data_logging_lisa");
	}


	//wait for the fourth thread to finish
	if(pthread_join(thread_data_logging_ground, NULL)) {
		error_write(FILENAME,"error joining thread_data_logging_ground");
	}

	//free circular buffers
	cbFree(cb_read_lisa);
	cbFree(cb_write_lisa);
	cbFree(cb_read_ground);
	cbFree(cb_write_ground);

	#endif

	return 0;
}
コード例 #16
0
/************************************
 * FUNCTIONS
 * **********************************/
static void *lisa_to_pc(void *arg){
/*-------------------------START OF SECOND THREAD: LISA TO PC------------------------*/

	static UDP udp_client;
	int message_length;
	ElemType cb_elem = {0};
	uint8_t input_buffer[INPUT_BUFFER_SIZE];

	UDP_err_handler(openUDPClientSocket(&udp_client,connection.server_ip,connection.port_number_lisa_to_pc,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);

	while(1)
	{
		message_length = serial_input_get_lisa_data(input_buffer); //blocking !!!
		if(message_length > 0){
			
			//to test latency from lisa pull pin high when airspeed package arrives
			/*if(serial_input.buffer[3]==AIRSPEED_ETS){
				if (test==0){
					system("echo 1 > /sys/class/gpio/gpio60/value");
					test=1;
				}else{
					system("echo 0 > /sys/class/gpio/gpio60/value");
					test=0;
				}
			}*/

			//add timestamp
			message_length=add_timestamp(input_buffer);

			//send data to eth port using UDP
			UDP_err_handler(sendUDPClientData(&udp_client,input_buffer,message_length),write_udp_error_ptr);

			#if LOGGING > 0

			//write the data to circual buffer for log thread
			 if(!cbIsFull(cb_write_lisa)){
				 memcpy (&cb_elem.value, input_buffer, message_length);
				 cbWrite(cb_write_lisa, &cb_elem);
			 }else{
				if(reading_flag_lisa==0){
					switch_cb_lisa_pointers();
					//printf("switching lisa pointers\n");
				}else{
					printf("LISA WRITE WAS NOT READY \n");
					exit(1); //FOR DEBUGGING
				}
			 }

			#endif
		}else{
		//send error message to server: not receiving data on uart port
			printf("error on uart, see log...\n"); //FOR DEBUGGING
			UART_err_handler(message_length,write_uart_error_ptr);
		}

	}

	UART_err_handler(serial_port_close(),write_uart_error_ptr);

	UDP_err_handler(closeUDPClientSocket(&udp_client),write_udp_error_ptr);

	return NULL;
/*------------------------END OF SECOND THREAD------------------------*/

}
コード例 #17
0
//#define SENSOR_OFF
//#define KOM_OFF
void timedInterupt(void)
{
	//Toggla 
	if(temp==0){
		PORTB &= 0b11111110;
		temp = 1;
	}else{
		PORTB |= 0b00000001;
		temp = 0;
	}
	uint8_t msgRecieve[32];
	uint8_t type;
	uint8_t len;
	uint8_t msgSend[32];
	//skicka request till sensorenehten att skicka data
#ifndef SENSOR_OFF
	SPI_set_sensor(START);
	SPI_MASTER_write(msgSend, TYPE_REQUEST_SENSOR_DATA, 0);
	
	//ta emot data från sensorenheten
	uint8_t answerSensor=0;
	uint8_t answerCounterSensor=0;
	while((!answerSensor)&&(answerCounterSensor<254))//räknare så vi inte fastnar här om vi hamnar aout of sync med kom.
	{
		answerSensor = SPI_MASTER_read(msgRecieve, &type, &len);
		answerCounterSensor++;
	}
	SPI_set_sensor(END);
	
	if(answerCounterSensor == 255)
		return;
	
	glob_longFront_old = glob_longFront; // cm
	glob_longRight_old = glob_longRight; // cm
	glob_longRear_old = glob_longRear;// cm
	glob_longLeft_old = glob_longLeft;// cm
	glob_shortFrontRight_old = glob_shortFrontRight; // cm/2
	glob_shortFrontLeft_old = glob_shortFrontLeft; // cm/2
	glob_shortRearRight_old = glob_shortRearRight; // cm/2
	glob_shortRearLeft_old = glob_shortRearLeft;
		
	//tolka/spara sensordata
	if(type==TYPE_SENSOR_DATA && len!=0)
	{
		for(uint8_t i = 0; i < len; ++i)
		{
			uint8_t id = msgRecieve[i];
			switch(id)
			{
				case LONGFRONT:
					glob_longFront = msgRecieve[i+1];
					++i;
					break;
				case LONGRIGHT:
					glob_longRight = msgRecieve[i+1];
					++i;
					break;
				case LONGREAR:
					glob_longRear = msgRecieve[i+1];
					++i;
					break;
				case LONGLEFT:
					glob_longLeft = msgRecieve[i+1];
					++i;
					break;
				case SHORTFRONTRIGHT:
					glob_shortFrontRight = msgRecieve[i+1];
					++i;
					break;
				case SHORTFRONTLEFT:
					glob_shortFrontLeft = msgRecieve[i+1];
					++i;
					break;
				case SHORTREARRIGHT:
					glob_shortRearRight = msgRecieve[i+1];
					++i;
					break;
				case SHORTREARLEFT:
					glob_shortRearLeft = msgRecieve[i+1];
					++i;
					break;
				case IDGYROSENSOR:
					glob_gyro = (msgRecieve[i+1]<<8)|msgRecieve[i+2];
					i = i+2;
					break;
				case IDSPEEDRIGHT:
					glob_vRight = msgRecieve[i+1];
					++i;
					break;
				case IDSPEEDLEFT:
					glob_vLeft = msgRecieve[i+1];
					++i;
					break;
			}	
		}
	
		//uppdatera tillstånd
		updateState();
	}
#endif
#ifndef KOM_OFF
	sendcounter++;
	if(sendcounter==8)
	{
		sendcounter=0;
		//skicka vidare till PC
		SPI_set_kom(START);
		//väldigt ful kod gör om, gör rätt
		msgRecieve[len] = 12;
		msgRecieve[len+1] = glob_x;
		msgRecieve[len+2] = 13;
		msgRecieve[len+3] = glob_y;
		msgRecieve[len+4] = 14;
		msgRecieve[len+5] = glob_theta;
		SPI_MASTER_write(msgRecieve, TYPE_DEBUG_DATA, len+6);
		
		
		/*int8_t vinkelHastHjul = (glob_vRight-glob_vLeft)>>4; // 17 ca 16
		cbWrite(&glob_debugMesssageBuffer, 21);
		cbWrite(&glob_debugMesssageBuffer, vinkelHastHjul);*/
		cbWrite(&glob_debugMesssageBuffer, 25);
		cbWrite(&glob_debugMesssageBuffer, (glob_sum_y&0xFF00)>>8);
		cbWrite(&glob_debugMesssageBuffer, (glob_sum_y&0x00FF));
		//send debug data
		uint8_t bytesToSend = 0;
		while(cbBytesUsed(&glob_debugMesssageBuffer) != 0)
		{
			msgSend[bytesToSend] = cbRead(&glob_debugMesssageBuffer);
			++bytesToSend;
		}
		if(bytesToSend != 0)
		{
			SPI_MASTER_write(msgSend, TYPE_DEBUG_DATA, bytesToSend);
		}
			//end send debug data
		//uint8_t msg[4]={12, glob_x, 13, glob_y};
		//SPI_MASTER_write(msg, TYPE_DEBUG_DATA, 4);
	
		volatile uint8_t answer = 0;
		uint8_t answerCounter=0;
		do
		{
			SPI_MASTER_write(msgSend, TYPE_REQUEST_PC_MESSAGE, 0);

		
			answer = 0;
			answerCounter=0;
			while((!answer)&&(answerCounter<254))//räknare så vi inte fastnar här om vi hamnar aout of sync med kom.
			{
				answer = SPI_MASTER_read(msgRecieve, &type, &len);
				answerCounter++;
			}
				

			if(type == TYPE_MANUAL_COMMAND)
			{
				//lägg till msg[0] först i route
				for(uint8_t i = glob_routeLength; i > 0; --i)
				{
					glob_route[glob_routeLength] = glob_route[glob_routeLength-1];
				}
				glob_route[0] = msgRecieve[0];
				glob_routeLength = glob_routeLength+1;
				break;
			}
			else if(type == TYPE_CHANGE_PARM)
			{
				uint8_t ID = msgRecieve[0];
				uint8_t val = msgRecieve[1];
				
				if (ID==PARAMLEFTCUSTOM)
				{
					glob_paramCustomLeft = val;
				}
				else if (ID==PARAMRIGHTCUSTOM)
				{
					glob_paramCustomRight = val;
				}
				else if (ID==L1_STRAIGHTX)
				{
					glob_L1_straightX = val;
				}
				else if (ID==L2_STRAIGHTTHETA)
				{
					glob_L2_straightTheta = val;
				}
				else if (ID==L3_STRAIGHTOMEGA)
				{
					glob_L3_straightOmega = val;
				}
				else if (ID==L1_TURNTHETA)
				{
					glob_L1_turnTheta = val;
				}
				else if (ID==L2_TURNOMEGA)
				{
					glob_L2_turnOmega = val;
				}

			}		
		}while((type != TYPE_NO_PC_MESSAGES) && (type != TYPE_REQUEST_PC_MESSAGE) && (answerCounter<254));//type != TYPE_REQUEST_PC_MESSAGE betyder att vi ej laggt in ny data i kom. dvs ej handskakat.


		//skicka all kartdata till komm
		while(cbBytesUsed(&glob_mapDataToSend) > 1)
		{
			uint8_t x = cbRead(&glob_mapDataToSend);
			uint8_t y = cbRead(&glob_mapDataToSend);
			msgSend[0] = x;
			msgSend[1] = y;
			msgSend[2] = glob_map[y][x];
			SPI_MASTER_write(msgSend, TYPE_MAP_DATA, 3);
		}
		SPI_set_kom(END);
	}	
コード例 #18
0
ファイル: term.c プロジェクト: joaoneves1/alerta_raios
int main(int argc, char *argv[]) 
{
	int print;
	printf("%d %s\n",argc, argv[0]);
	if(argc >1 && strcmp("-p", argv[1]) == 0) print=1;
	
	time_t nosso_tempo;
	
//************************************************/
	CircularBuffer cb;
    ElemType elem = {0};
    int testBufferSize = 11264; // 176 blocos de 64 palavras
    cbInit(&cb, testBufferSize);
//************************************************/

	FILE *arquivo;
	
	int fd;
	
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"	
	int res;
#pragma GCC diagnostic pop
	
	unsigned short crc16;
	int i,j=0;
	int detected= 0, pronto = 0;
	char *ptr;
	uint16_t crc;
	uint16_t n_mesg, msg_num = 0;
	int offset;
	struct termios oldtio,newtio;
	char buf[BUFFER_SIZE];
	fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY );
	if (fd < 0) {
		perror(MODEMDEVICE); 
		cbFree(&cb);
		exit(-1); 
	}
	tcgetattr(fd,&oldtio); /* save current port settings */
	bzero(&newtio, sizeof(newtio));
	newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
	newtio.c_iflag = IGNPAR;
	newtio.c_oflag = 0;

	/* set input mode (non-canonical, no echo,...) */
	newtio.c_lflag = 0;

	newtio.c_cc[VTIME]    = 0;   /* inter-character timer unused */
	newtio.c_cc[VMIN]     = 132;   /* blocking read until 132 chars received */

	tcflush(fd, TCIFLUSH);
	tcsetattr(fd,TCSANOW,&newtio);

	arquivo = fopen("raios_detetados.bin", "wb");
	if (arquivo == NULL) {
		perror("Nao foi possível abrir o arquivo!");
		cbFree(&cb);
		close(fd);
		exit(30);
	}
	
	while (1) {     /* loop until we have a terminating condition */
		/* read blocks program execution until a line terminating character is 
			input, even if more than 255 chars are input. If the number
			of characters read is smaller than the number of chars available,
			subsequent reads will return the remaining chars. res will be set
			to the actual number of characters actually read */
		res=read(fd,buf,BUFFER_SIZE);
// 		printf("%d chars - ",res);

	crc=((uint16_t) (0xFF00 & (buf[MSG_SIZE+1]<<8)) + (uint16_t) (0x00FF & buf[MSG_SIZE]));
	n_mesg=((uint16_t) (0xFF00 & (buf[1]<<8)) + (uint16_t) (0x00FF & buf[0]));
// 		char test[]="CRC16 stream simulation.";
		ptr=buf;
		crc16= 0;
		for (i = 0; i < MSG_SIZE; i++) {
			crc16 = update_crc_16(crc16, *ptr);
			ptr++;
		}
		if(crc==crc16){
// 			printf(" CRC OK! -  %4X %4X %d\n", crc, crc16, n_mesg);

			for(i=1;i<65;i++){
				elem.value=((int16_t) (0xFF00 & (buf[2*(i)+1]<<8)) + (int16_t) (0x00FF & buf[2*i]));
				if (detected == 1) {
//					if (n_mesg != msg_num) {
					if (j < 13632) { 
						vetor_disco[j] = elem.value;
						j++;
// 						printf("j = %d\n", j);
					}
					else {
						pronto = 1;
						detected = 0;
						cbWrite(&cb, &elem);
					}
				}
				else {
					cbWrite(&cb, &elem);
				}
				if(print==1){
					printf("%4d ",elem.value);
					if (((i+2) % 32) ==1)
					printf("\n");
				}
			}
		} else {
			clearScreen();
// 			printf("not OK!  %X %X\n", crc, crc16);
			tcflush(fd, TCIFLUSH);
		}
		if (pronto == 1) {
			fwrite(vetor_disco, sizeof(vetor_disco), 1, arquivo);
			printf("Write!!!!!");
			j = 0;
			pronto = 0;
		}
// 		if(j>=testBufferSize){
// 			while (!cbIsEmpty(&cb)) {
	// 					elem = cbRead(&cb);i
// 				for (offset=0;offset<(cb.end-cb.start);offset++){
// 					elem = cbPeek(&cb,offset);
// 					printf("%d\t%d\t%d\t%d\n", elem.value, cb.start, cb.end, cb.size);
// 				}
		if (detected == 0) {
			offset = r_change(&cb,n_mesg); // TODO: o que acontece quando n_mesg retorna a zero?
			if(offset){
				nosso_tempo = time(&nosso_tempo);
				fwrite(&nosso_tempo, sizeof(time_t),1, arquivo);
//				for (i = offset - 2263, j = 0; i < cb.end; i++, j++){ // TODO :: fazer a conta do offset levando em consideração o wrap around.
				for (i = (offset%64 - 37)*64, j = 0; i < (offset%64 +1)*64; i++, j++){
					ElemType temp_elem;
 					temp_elem = cbPeek(&cb, i);
					vetor_disco[j] = temp_elem.value;
				}
				cb.end = cb.start; // Esvazia o buffer circular	
				detected = 1;
				msg_num = n_mesg + 176;
				printf("Detected: %ld %d %d\n",nosso_tempo, n_mesg,msg_num);
			}	
	
		}
// 			}
// 		break;
// 		}
// 			for(i=0;i<64;i++){
// 				printf("%4d ",((uint16_t) (0xFF00 & (buf[2*(i)+1]<<8)) + (uint16_t) (0x00FF & buf[2*i])));
// 				if (((i+2) % 32) ==1)           printf("\n");
// 				}
		}
		tcsetattr(fd,TCSANOW,&oldtio);
		return(0);
}
コード例 #19
0
ファイル: input_sdr.c プロジェクト: ashh87/dabtools
int sdr_demod(struct demapped_transmission_frame_t *tf, struct sdr_state_t *sdr){
  int i,j;

  tf->has_fic = 0;

  /* resetting coarse freqshift */
  sdr->coarse_freq_shift = 0;
  
  /* write input data into fifo */
  for (i=0;i<sdr->input_buffer_len;i++) {
    cbWrite(&(sdr->fifo),&sdr->input_buffer[i]);
  }

  /* Check for data in fifo */
  if (sdr->fifo.count < 196608*3) {
    return 0;
  }
  
  /* read fifo */
  sdr_read_fifo(&(sdr->fifo),196608*2,sdr->coarse_timeshift+sdr->fine_timeshift,sdr->buffer);


  
  /* give the AGC some time to settle */
  if (sdr->startup_delay<=GAIN_SETTLE_TIME) {
    sdr->startup_delay+=1;
    fprintf(stderr,"startup_delay=%i\n",sdr->startup_delay);
    return 0;
  }
  


  /* complex data conversion */
  for (j=0;j<196608*2;j+=2){
    sdr->real[j/2]=sdr->buffer[j]-127;
    sdr->imag[j/2]=sdr->buffer[j+1]-127;
  }

  /* resetting coarse timeshift */
  sdr->coarse_timeshift = 0;

  /* coarse time sync */
  /* performance bottleneck atm */
  sdr->coarse_timeshift = dab_coarse_time_sync(sdr->real,sdr->filt,sdr->force_timesync);
  // we are not in sync so -> next frame
  sdr->force_timesync=0;
  if (sdr->coarse_timeshift) {
    //printf("coarse time shift\n");
    return 0;
  }

  /* create complex frame */
  for (j=0;j<196608;j++){
    sdr->dab_frame[j][0] = sdr->real[j];
    sdr->dab_frame[j][1] = sdr->imag[j];
  }

  /* fine time sync */
  sdr->fine_timeshift = dab_fine_time_sync(sdr->dab_frame);
  if (sdr->coarse_freq_shift) {
    sdr->fine_timeshift = 0;
    }
  /* coarse_frequency shift */
  fftw_plan p;
  p = fftw_plan_dft_1d(2048, &sdr->dab_frame[2656+505+sdr->fine_timeshift], sdr->symbols[0], FFTW_FORWARD, FFTW_ESTIMATE);
  fftw_execute(p);
  fftw_destroy_plan(p);
  
  fftw_complex tmp;
    for (i = 0; i < 2048/2; i++)
    {
      tmp[0]     = sdr->symbols[0][i][0];
      tmp[1]     = sdr->symbols[0][i][1];
      sdr->symbols[0][i][0]    = sdr->symbols[0][i+2048/2][0];
      sdr->symbols[0][i][1]    = sdr->symbols[0][i+2048/2][1];
      sdr->symbols[0][i+2048/2][0] = tmp[0];
      sdr->symbols[0][i+2048/2][1] = tmp[1];
    }
  sdr->coarse_freq_shift = dab_coarse_freq_sync_2(sdr->symbols[0]);
  if (abs(sdr->coarse_freq_shift)>1) {
    sdr->force_timesync = 1;
    return 0;
  }

  /* fine freq correction */
  sdr->fine_freq_shift = dab_fine_freq_corr(sdr->dab_frame,sdr->fine_timeshift);

  /* d-qpsk */
  for (i=0;i<76;i++) {
    p = fftw_plan_dft_1d(2048, &sdr->dab_frame[2656+(2552*i)+504],
			 sdr->symbols[i], FFTW_FORWARD, FFTW_ESTIMATE);
    fftw_execute(p);
    fftw_destroy_plan(p);
    for (j = 0; j < 2048/2; j++)
      {
	tmp[0]     = sdr->symbols[i][j][0];
	tmp[1]     = sdr->symbols[i][j][1];
	sdr->symbols[i][j][0]    = sdr->symbols[i][j+2048/2][0];
	sdr->symbols[i][j][1]    = sdr->symbols[i][j+2048/2][1];
	sdr->symbols[i][j+2048/2][0] = tmp[0];
	sdr->symbols[i][j+2048/2][1] = tmp[1];
      }
    
  }
  //
  for (j=1;j<76;j++) {
    for (i=0;i<2048;i++)
      {
	sdr->symbols_d[j*2048+i][0] =
	  ((sdr->symbols[j][i][0]*sdr->symbols[j-1][i][0])
	   +(sdr->symbols[j][i][1]*sdr->symbols[j-1][i][1]))
	  /(sdr->symbols[j-1][i][0]*sdr->symbols[j-1][i][0]+sdr->symbols[j-1][i][1]*sdr->symbols[j-1][i][1]);
	sdr->symbols_d[j*2048+i][1] = 
	  ((sdr->symbols[j][i][0]*sdr->symbols[j-1][i][1])
	   -(sdr->symbols[j][i][1]*sdr->symbols[j-1][i][0]))
	  /(sdr->symbols[j-1][i][0]*sdr->symbols[j-1][i][0]+sdr->symbols[j-1][i][1]*sdr->symbols[j-1][i][1]);
      }
  }
  
  uint8_t* dst = tf->fic_symbols_demapped[0];
  tf->has_fic = 1;  /* Always true for SDR input */

  int k,kk;
  for (j=1;j<76;j++) {
    if (j == 4) { dst = tf->msc_symbols_demapped[0]; }
    k = 0;
    for (i=0;i<2048;i++){
      if ((i>255) && i!=1024 && i < 1793) {
        /* Frequency deinterleaving and QPSK demapping combined */  
        kk = rev_freq_deint_tab[k++];
        dst[kk] = (sdr->symbols_d[j*2048+i][0]>0)?0:1;
        dst[1536+kk] = (sdr->symbols_d[j*2048+i][1]>0)?1:0;
      }
    }
    dst += 3072;
  }
  
  return 1;
}
コード例 #20
0
int main(void)
{
	// Initiera SPI som slav
	SPI_SLAVE_init();
	
	// Initiera UART
	UART_init();
	
	// Sätt på interrupts
	sei();
	
	// Skapa meddelandebuffert för meddelanden som ska skickas till styrenheten
	CircularBuffer messageQueue;
	cbInit(&messageQueue, 32);
	
	uint8_t spiMsg[32];
	uint8_t spiType;
	uint8_t spiLen;
	
	uint8_t spiMsgR[32];
	uint8_t spiTypeR;
	uint8_t spiLenR;
	
	uint8_t uartMsg[16];
	uint8_t uartType;
	uint8_t uartLen;
	
	uint8_t stopped = 0;

	while(1){
		
		// Kolla om det har kommit ett helt meddelande från PCn
		if(UART_readMessage(uartMsg,&uartType,&uartLen)){
			// Kolla om det är ett nödstoppsanrop
			if(uartType == TYPE_EMERGENCY_STOP){
				SETBIT(PORTB, PORTB3);
				stopped=1;
			}else{
				// Annars, lägg hela meddelandet i meddelandebuffern
				cbWrite(&messageQueue, (uartType<<5)|uartLen);
				for(uint8_t i=0; i < uartLen; i++)
				{
					cbWrite(&messageQueue, uartMsg[i]);
				}
			}
			if(uartType == TYPE_CHANGE_PARM && stopped==1)
			{
				CLEARBIT(PORTB, PORTB3);
				stopped=0;
			}
		}
		
		// Kolla om vi har tagit emot ett helt meddelande från styrenheten
		if(SPI_SLAVE_read(spiMsg, &spiType, &spiLen))
		{	
			if(spiType == TYPE_REQUEST_PC_MESSAGE)
			{
				// Om det är en efterfrågan av PC-meddelanden, skicka det första meddelandet i buffern, om det finns något
				if(cbBytesUsed(&messageQueue)!=0)				
				{				
					uint8_t head = cbPeek(&messageQueue);
					uint8_t len = head&0b00011111;
					uint8_t type = 0b11100000&head;
					type = type>>5;
					if(len < cbBytesUsed(&messageQueue) && cbBytesUsed(&messageQueue) != 0)
					{
						head = cbRead(&messageQueue); // Läs headern igen
						for(uint8_t i = 0; i < len; i++)
						{
							spiMsgR[i]=cbRead(&messageQueue);
						}
						SPI_SLAVE_write(spiMsgR, type, len);	
					}
					else
					{
						SPI_SLAVE_write(spiMsgR, TYPE_NO_PC_MESSAGES, 0);
					}
				}
				else
				{
					SPI_SLAVE_write(spiMsgR, TYPE_NO_PC_MESSAGES, 0);
				}			
			}else if(spiType == TYPE_MAP_DATA)