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