   while( i < arraysize ) {								    
     pulse = pulseIn(SENSOR_PIN, HIGH);  // read in time for pin to transition
     if( pulse == 0 ) return 0;
-    rangevalue[i]=pulse/58;         // pulses to centimeters (use 147 for inches)
-    if( rangevalue[i] < 645 && rangevalue[i] >= 15 ) i++;  // ensure no values out of range
+    //    rangevalue[i]=pulse/58;         // pulses to centimeters (use 147 for inches)
+    pulsevalue[i]=pulse;              // raw pulses to centimeters (use 147 for inches)
+    //    if( rangevalue[i] < 645 && rangevalue[i] >= 15 ) i++;  // ensure no values out of range
+    if( pulsevalue[i] < 37410 && pulsevalue[i] >= 870 ) 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 
-
-  *outRawDistance = distance;
+  //  isort(rangevalue,arraysize);        // sort samples
+  //  uint16_t distance = mode(rangevalue,arraysize);  // get median 
+  isort(pulsevalue,arraysize);        // sort samples
+  uint16_t rawtof = mode(pulsevalue,arraysize);  // get median 
+
+  //  *outRawDistance = distance;
+  *outRawDistance = rawtof/58;      // time of flight / 58uS for both there and back
   // Use temperature compensation if temp sensor found
   if( tempSensorFound ) {
     sensors.requestTemperatures();
     temperature = sensors.getTempCByIndex(0);
     *outTemperature = temperature;
-    float tof = distance * 0.0058;
-    uint16_t newDist = tof * (( 20.05 * sqrt( temperature + 273.15))/2);
+    //    float tof = distance * 0.58;
+    //    uint16_t newDist = tof * (( 20.05 * sqrt( temperature + 273.15))/200);
+    uint16_t newDist = (rawtof * (331.3 + 0.606 * temperature)) / 20000;
+
     *outDistance = newDist;
-  }
+  } else {
+    // No temperature sensor, just return same distance for both
+    *outDistance = rawtof/58;  
+  }
+  
   // Add check for validity of reading
   if( *outRawDistance <= 23 ) {    // 23cm seems to be the minimum value
     return false;

}
// End of SRF command functions

void setup() {
  // initialise serial interface for debugging/SRF communication:
  Serial.begin(115200);

  // Get device ID

int startupCounter = FAST_POLL_COUNT;

// Node ID, default 00, set by input pins
// Strictly speaking this should only be digits, according to the Ciseco LLAP spec.
// but since the spec is inefficient we've ignored the format.
char nodeId[2] = {  '0', '0' };


// Some functions to get the configured node address and polling
/** readJumpers - determine node ID and polling interval for this sensor
 */
void readJumpers() {
  // Set analog input pins to read digital values, set internal pullup
  uint8_t jumperPins[] = { 
    A0, A1, A2, A3, A4, A5, 9, 10, 12, 13   };
  for( int n = 0; n < 10; n++ ) {
    pinMode( jumperPins[n], INPUT );
    digitalWrite( jumperPins[n], HIGH);
