// 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;
}
}
}
