void sensor_notifier() {
// Find the sensor server.
int sensor_server_tid = -2;
do {
sensor_server_tid = WhoIs("SensorServer");
dlog("Sensor Server Tid %d\n", sensor_server_tid);
} while (sensor_server_tid < 0);
// Set up sensors.
enable_sensor_reset();
// Delay a bit to read old data and then clear it.
Delay(10);
ReadClear(COM1);
// Poll sensor data and forward it.
SensorServerMessage msg;
SensorServerMessage rply;
msg.type = SENSOR_EVENT_REQUEST;
char old_data[SENSOR_DATA_SIZE] = {0,0,0,0,0,0,0,0,0,0};
for(;;) {
dump_sensors();
int i = 0;
for (; i < SENSOR_DATA_SIZE; ++i) {
char new_data = (char)Getc(COM1);
msg.data[i] = new_data & ~old_data[i];
old_data[i] = new_data;
}
Send(sensor_server_tid, (char *)&msg, sizeof(msg), (char *)&rply, sizeof(rply));
dassert(rply.type == SENSOR_EVENT_RESPONSE, "Invalid Response from SensorServer");
}
}
void roomba_t::update()
{
uint8_t data;
while(serial_m->available()>0 && serial_m->
#ifdef __AVR
readBytes((char *)&data,1)==1
#else
read(&data,1)==1
#endif
)
{
if(serial_state_m==HEADER&&data==ROOMBA_PACKET_HEADER)
{
serial_state_m=SIZE;
}
else if(serial_state_m==SIZE)
{
serial_size_m=data;
serial_buffer_m=(uint8_t*)malloc(serial_size_m);
serial_state_m=DATA;
}
else if(serial_state_m==DATA)
{
serial_buffer_m[serial_pointer_m++]=data;
if(serial_pointer_m>=serial_size_m)
serial_state_m=CHECKSUM;
}
else if(serial_state_m==CHECKSUM)
{
if(checksum(ROOMBA_PACKET_HEADER,serial_size_m,serial_buffer_m,data))
{
parse_sensor_packet_m();
dump_sensors();
}
serial_size_m=0;
free(serial_buffer_m);
serial_pointer_m=0;
serial_state_m=HEADER;
}
}
}
