 }
 // End of SRF command functions
 
+/** setup - Regular Arduino setup function. 
+ * We setup the I/O pins, read jumpers and start the sensor
+ */
 void setup() {
-  // initialise serial interface for debugging/SRF communication:
+  // initialise serial interface for SRF communication:
   Serial.begin(115200);
 
   // Get device ID

  }
}

// The main loop, we basically wake up the SRF, take a reading, transmit reading then go back to sleep
void loop() {
  float temperature = 0.0;
  int rawDistance = 0;
  int distance = 0;

  // Determine if we need to send a battery voltage reading yet
  if( --batteryCountDown <= 0 ) {
    int mV = readVcc();
    LLAP.sendIntWithDP("B", mV, 3 );
    batteryCountDown = BATTERY_READ_INTERVAL;
  } 

    // Distance reading
    boolean rangeValid = getRange( &rawDistance, &distance, &temperature);
    // Send temperature reading
    if( tempSensorFound ) {
      int latestTemp = (int)(temperature * 100);
      LLAP.sendIntWithDP( "T", latestTemp, 2);
    }
    // Uncompensated distance
    LLAP.sendInt( "U", rawDistance);
    // Send reading 3 times to make sure it gets through
    //    for(int n = 0; n<1; n++ ) {

    if( rangeValid ) {
      //if( rawDistance > 23 )     // 23cm seems to be the minimum value
      LLAP.sendInt( "D", distance );
    } else {
      if( rawDistance == 0 ) {
        LLAP.sendMessage( "UMax" );
      } else {
        LLAP.sendMessage( "UErr" );
      }
    }
  

  // Determine if we are still in startup sequence, if so, do we need to
  // adjust polling interval.

  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)
    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 
  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.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;
