     if( rangevalue[i] < 645 && rangevalue[i] >= 15 ) i++;  // ensure no values out of range
     delay(10);                      // wait between samples
   }
-  
+
+  // Turn off sensor as no longer needed
   digitalWrite(SENSOR_ENABLE, LOW);
 
   isort(rangevalue,arraysize);        // sort samples
   uint16_t distance = mode(rangevalue,arraysize);  // get median 
 
-  lastUncompDistance = distance;
-  // Use temperature compensation if temp sensor found
+  *outRawDistance = distance;
+  // Use temperature comprensation if temp sensor found
   if( tempSensorFound ) {
-    //temperature = sensors.getTempC(temperatureSensor);
     sensors.requestTemperatures();
     temperature = sensors.getTempCByIndex(0);
-    latestTemperature = temperature;
-//  Serial.print("Uncomp Dist: ");
-//  Serial.println(distance,DEC);
-//  Serial.print("temperature: ");
-//  Serial.println(temperature);
+    *outTemperature = temperature;
     float tof = distance * 0.0058;
     uint16_t newDist = tof * (( 20.05 * sqrt( temperature + 273.15))/2);
-    distance = newDist;
-  }
-  return distance; 
+    *outDistance = newDist;
+  }
+  // Add check for validity of reading
+  if( *outRawDistance <= 23 ) {    // 23cm seems to be the minimum value
+    return false;
+  }
+
+  return true; 
 }
 
 // End of Maxbotix sensor code

// arrays to hold device address
DeviceAddress temperatureSensor;

boolean tempSensorFound = false;
float latestTemperature = 0.0;
uint16_t lastUncompDistance = 0;

// Define pins that are not used and are to be made inputs
uint8_t inPin[] = { 9, 10, 11, 12, 13 };

int batteryCountDown = BATTERY_READ_INTERVAL;
int pollingInterval = FIFTEENMINS;

uint8_t enterCommandMode();
// Node ID, default 00, set by input pins
// Stricly speaking this should only be digits, according to the Ciseco LLAP spec.
char nodeId[2] = { '0', '0' };

// Some functions to get the configured node address and polling
void readJumpers() {
  // Set analog input pins to read digital values, set internal pullup
  // TODO: ugly - refactor in loop.
  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);
  pinMode(9,INPUT);
  pinMode(10,INPUT);
  pinMode(12,INPUT);
  pinMode(13,INPUT);
  digitalWrite(A0, HIGH);
  digitalWrite(A1, HIGH);
  digitalWrite(A2, HIGH);
  digitalWrite(A3, HIGH);
  digitalWrite(A4, HIGH);
  digitalWrite(A5, HIGH);
  digitalWrite(9, HIGH);
  digitalWrite(10, HIGH);
  digitalWrite(12, HIGH);
  digitalWrite(13, HIGH);
  
  // Read input and parse value
  int digit2 = ((PINB & 0x30) >> 4) | ((PINC & 0x03) << 2);
  digit2 ^= 0x0f;
  int digit1 = (PINC & 0x3c) >> 2;
  digit1 ^= 0x0f;
  nodeId[0] = '0' + min(digit1,9);
  nodeId[1] = '0' + min(digit2,9);
  pollingInterval = ((PINB & 0x06) >> 1) ^ 0x03;

  Serial.println("Debug: ");
  Serial.print("Port B: ");
  Serial.println(PINB,HEX);
  Serial.print("Port C: ");
  Serial.println(PINC,HEX);
  
  Serial.print("Polling Interval: ");
  Serial.println(pollingInterval,DEC);

  Serial.print("Node ID Digits: ");
  Serial.print(digit1,DEC);
  Serial.print(" ");
  Serial.println(digit2,DEC);
  // A0, A1 are first part, R, S, T, W
  // A2 - A5 are 0-9, A-F
/*
  int a = PINC & 0x3F;
  a ^= 0x3F;
  int id1 = a & 0x03;
  switch( id1 ) {
    case 0: nodeId[0] = 'R'; break;
    case 1: nodeId[0] = 'S'; break;
    case 2: nodeId[0] = 'T'; break;
    case 3: nodeId[0] = 'W'; break;
  }
  int id2 = a >> 2;
  if( id2 <10 ) 
    nodeId[1] = '0' + id2;
  else
    nodeId[1] = 'A' + (id2-10);
*/

  // Reset analog inputs to set internal pullup off
  digitalWrite(A0, LOW);
  digitalWrite(A1, LOW);
  digitalWrite(A2, LOW);
  digitalWrite(A3, LOW);
  digitalWrite(A4, LOW);
  digitalWrite(A5, LOW);
  digitalWrite(9, LOW);
  digitalWrite(10, LOW);
  digitalWrite(12, LOW);
  digitalWrite(13, LOW);
}

// **************************************

// Get the range in cm. Need to enable sensor first then wait briefly for it to power up
// Also request temperture sensor, if previously detected, to take a reading.
// Disable sensor after use.
// Returns compensated, uncompensated and temperature values as reference params
// boolean true returned form function for valid reading. false for <23cm or out of range
// Calling function can still do checks to determine whether to use reading or not.
boolean getRange( int *outRawDistance, int *outDistance, float *outTemperature ) {
  int16_t pulse;  // number of pulses from sensor
  int i=0;
  // These values are for calculating a mathematical median for a number of samples as
