void sensors_init(){
enableObstSens();
enableGroundSens();
initCompass();
sensor_sensorReadings.last_position_index = -1;
internal_values.analog_sensors_updated = NO;
internal_values.num_readings = 0;
internal_values.initial_compas_offset = 0.0;
internal_values.initilisation_done = false;
internal_values.sensor_readings_offset.beaconSensor.isVisible = false;
internal_values.sensor_readings_offset.beaconSensor.apsolute_direction = 0;
internal_values.sensor_readings_offset.beaconSensor.relative_direction = 0;
internal_values.sensor_readings_offset.groundSensor = 0;
internal_values.sensor_readings_offset.obstacleSensor.front = 0;
internal_values.sensor_readings_offset.obstacleSensor.left = 0;
internal_values.sensor_readings_offset.obstacleSensor.right = 0;
internal_values.sensor_readings_offset.positionSensor.x = 0;
internal_values.sensor_readings_offset.positionSensor.y = 0;
internal_values.sensor_readings_offset.positionSensor.t = 0;
refresh_buffer();
refresh_buffer();
refresh_buffer();
refresh_buffer();
internal_values.initial_compas_offset = -sensor_filteredSensorReadings.positionSensor.compass_direction;
refresh_buffer();
internal_values.initilisation_done = true;
}
/////////////////////////////////////////////////////////
//
// pix_rtx
//
/////////////////////////////////////////////////////////
// Constructor
//
/////////////////////////////////////////////////////////
pix_rtx :: pix_rtx()
{
imageStruct image;
image.xsize = image.ysize = 64;
image.setCsizeByFormat(GL_RGBA_GEM);
refresh_buffer(image, buffer);
bufcount = 0;
mode = true;
set_buffer=true;
}
SensorReadings get_accurate_sensor_reading(){
refresh_buffer();
return get_filteredSensorReadings();
}
/////////////////////////////////////////////////////////
// processImage
//
/////////////////////////////////////////////////////////
void pix_rtx :: processImage(imageStruct &image)
{
if (!refresh_buffer(image, buffer)) {
// ouch, couldn't allocate memory!
return;
}
size_t pixsize = image.ysize * image.xsize;
int cols=image.xsize, c=0, c1=0;
int rows=image.ysize, r=0;
unsigned char *pixels = image.data;
unsigned char *wp; // write pointer
unsigned char *rp; // read pointer
// first copy the pixels into our buffer
if (!set_buffer) {
wp = buffer.data + pixsize * buffer.csize * bufcount;
memcpy(wp, pixels, pixsize * buffer.csize * sizeof(unsigned char));
} else {
// fill the buffer with the current frame
// this might be useful to prevent the black screen in the beginning.
// "set" message
c=cols;
size_t imagesize=pixsize * buffer.csize * sizeof(unsigned char);
while (c--) {
wp = buffer.data + imagesize * c;
memcpy(wp, pixels, imagesize);
}
c = 0;
set_buffer=false;
}
// then copy the buffer rtx-transformed back to the pixels
switch(image.csize){
case 1: // Grey
while (c < cols) {
c1 = mode?((c+cols-bufcount)%cols):(c+1)%cols;
while (r < rows) {
rp = buffer.data + buffer.csize * (buffer.xsize * buffer.ysize * c + buffer.xsize * r + (bufcount - c + cols) % cols );
pixels = image.data + image.csize * (image.xsize * r + cols - c1);
*pixels = *rp;
r++;
}
r=0;
c++;
}
break;
case 2: // YUV
while (c < cols) {
c1 = mode?((c+cols-bufcount)%cols):(c+1)%cols;
while (r < rows) {
rp = buffer.data + buffer.csize * (buffer.xsize * buffer.ysize * c + buffer.xsize * r + (bufcount - c + cols) % cols );
pixels = image.data + image.csize * (image.xsize * r + cols - c1);
pixels[0] = rp[0];
pixels[1] = rp[1];
r++;
}
r=0;
c++;
}
break;
case 4: // RGBA
while (c < cols) {
c1 = mode?((c+cols-bufcount)%cols):(c+1)%cols;
while (r < rows) {
rp = buffer.data + buffer.csize * (buffer.xsize * buffer.ysize * c + buffer.xsize * r + (bufcount - c + cols) % cols );
pixels = image.data + image.csize * (image.xsize * r + cols - c1);
pixels[chRed] = rp[chRed];
pixels[chBlue] = rp[chBlue];
pixels[chGreen] = rp[chGreen];
pixels[chAlpha] = rp[chAlpha];
r++;
}
r=0;
c++;
}
break;
}
bufcount++;
bufcount%=image.xsize;
}
