// Receive data from the brain void brain_rx_thread(void) { uint8_t input; while(1) { // wait for input while(!rx_ready(BRAIN)) yeild(); input = rx_byte(BRAIN); switch(input) { case 'M': // receive motor command brain_pack.reset(); do { while(!rx_ready(BRAIN)) yeild(); input = rx_byte(BRAIN); brain_pack.input((char)input); } while( input != '\r'); if( !bt_control ) { target_speed = brain_pack.reads8(); steer = brain_pack.readu8(); input = steer + STEER_OFFSET; // steering zero set servo_set(0, input); } break; case 'G': passthrough(BRAIN, BT, 'G'); break; default: passthrough(BRAIN, BT, input); break; } } }
/* GPS listen thread */ void gps_spinOnce(void) { if(rx_ready(gps_port)) { gps_input = rx_byte(gps_port); if(gps.encode(gps_input)) { gps.get_position(&lat, &lon); if( gps_pub.reset() ) { gps_pub.append(lat); gps_pub.append(lon); // TODO: fill in rest of GPS message gps_pub.append(gps.altitude()); gps_pub.append(gps.hdop()); gps_pub.finish(); } if( gps_pub2.reset() ) { gps_pub2.append(lat); gps_pub2.append(lon); // TODO: fill in rest of GPS message gps_pub2.finish(); } } } }
static int do_rx_work(struct c8250_serial *device) { uint8_t data; struct stream_interface *si = &device->rx; struct stream_pkt *packet = stream_get_head(si); while (rx_ready()) { data = rbr_get_data(); /* 11 or 0xb is Ctrl-k */ if ((data & 0xff) == 0xb){ L4_KDB_Enter("breakin"); // Dont want to pass the C-k to the client continue; } if(!packet) continue; packet->data[packet->xferred] = (data & 0xFF); packet->xferred++; while(rx_ready()) rbr_get_data(); if (packet->xferred == packet->length) { if ((packet = stream_switch_head(si)) == NULL) { break; } } } if (packet){ if (packet->xferred) packet = stream_switch_head(si); } return 0; }
// eat the data on a uart until a carriage return is found void finish(int src) { do { while(!rx_ready(src)) yeild(); } while( rx_byte(src) != '\r' ); }
// Receive data from bluetooth void bt_rx_thread(void) { uint8_t input; uint16_t i; volatile uint16_t sz = 0; while(1) { while(!rx_ready(BT)) yeild(); input = rx_byte(BT); switch(input) { case 'Z': input = 1; for( i=0; i<8; i++ ) { while(!rx_ready(BT)) yeild(); if( rx_byte(BT) != 'Z' ) input = 0; } if( input ) { //tx_bytes(BRAIN, (uint8_t*)"ZZZZZZZZ\r", 9); while(sz != 0) yeild(); sz = 9; brain_tx_buffer((uint8_t*)"ZZZZZZZZ\r", (uint16_t*)&sz); shutdown_count = 4*60; } finish(BT); break; case 'M': input = rx_byte(BT); if( bt_control ) { int8_t speed = (int8_t)input; speed = speed>100?100:speed; speed = speed<-100?-100:speed; //motor_speed(speed); target_speed = speed; } finish(BT); break; case 'S': input = rx_byte(BT); if( bt_control ) { servo_set(0, input); steer = input - STEER_OFFSET; } finish(BT); break; case 'C': bt_control = rx_byte(BT); finish(BT); break; case 'L': // at 8 bytes/pair, we can hold about 127 points buffer[0] = 'L'; for( i=1, input=rx_byte(BT); i<1023 && input != '\r'; i++, input=rx_byte(BT) ) { buffer[i] = input; } buffer[i] = '\r'; while( sz != 0 ) yeild(); sz = i+1; brain_tx_buffer(buffer, (uint16_t*)&sz); //passthrough(BT, BRAIN, 'L'); //finish(BT); break; } } }