File content as of revision 4:fe63b514d4ef:

/* Copyright (c) 2012 Nick Ryder, University of Oxford
 * nick.ryder@physics.ox.ac.uk
 *
 *  MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
 * and associated documentation files (the "Software"), to deal in the Software without restriction, 
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or 
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
 
 
 #include "RangeFinder.h"

// Init with 1 Sensor:
RangeFinder::RangeFinder(PinName pin0, int pulsetime, float scale, float offset, int time):
    pio0(pin0), pio1(NC), pio2(NC), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
    n_sensors = 1;
    this->old_val[0] = 0.0;
    current_sensor = 0;
}

// Init with 2 Sensors:
RangeFinder::RangeFinder(PinName pin0, PinName pin1,int pulsetime, float scale, float offset, int time):
    pio0(pin0), pio1(pin1), pio2(NC), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
    n_sensors = 2;
    for(uint8_t k=0;k<n_sensors;k++)
        this->old_val[k] = 0.0;
    current_sensor = 0;
    
}
// Init with 3 Sensors:
RangeFinder::RangeFinder(PinName pin0,PinName pin1, PinName pin2, int pulsetime, float scale, float offset, int time):
    pio0(pin0), pio1(pin1), pio2(pin2), pio3(NC), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
    n_sensors = 3;
    for(uint8_t k=0;k<n_sensors;k++)
        this->old_val[k] = 0.0;
    current_sensor = 0;
}
// Init with 4 Sensors:
RangeFinder::RangeFinder(PinName pin0, PinName pin1, PinName pin2,PinName pin3, int pulsetime, float scale, float offset, int time):
    pio0(pin0), pio1(pin1), pio2(pin2), pio3(pin3), scale(scale), offset(offset), pulsetime(pulsetime), timeout(time)  {
    n_sensors = 4;
    for(uint8_t k=0;k<n_sensors;k++)
        this->old_val[k] = 0.0;
    current_sensor = 0;
}

RangeFinder::~RangeFinder() {
}
void RangeFinder::read_and_filter(uint8_t i)  {
val[i] = this->read_m(i);
if(val[i] < 0)
    val[i] = old_val[i];
else{
    old_val[i] = val[i];
    }
}
// -----------------------------------------------------------------------------
float RangeFinder::read_m(uint8_t i)  {
    float t;
    switch(i){
        case 0:
            pio0.write_us(1, pulsetime);
            t = (float) pio0.read_high_us(timeout);
            break;
        case 1:
            pio1.write_us(1, pulsetime);
            t = (float) pio1.read_high_us(timeout);
            break;
        case 2:
            pio2.write_us(1, pulsetime);
            t = (float) pio2.read_high_us(timeout);
            break;
        case 3:
            pio3.write_us(1, pulsetime);
            t = (float) pio3.read_high_us(timeout);
            break;
        }
    if (t == -1.0)   {
        return -1.0;
    }
    return t / scale + offset;
}