File content as of revision 11:8c3b4995b05d:

#include "mbed.h"
#include "FIR_f32.h"
#include "arm_math.h"
#include "display.h"
#include "st7735.h"
//#include <string>
//#include <sstream>
#define f_sampling 2000 //the sampling frequency
#define NumTaps 27      //the number of filter coefficients
#define BlockSize 512  //the size of the buffer
#define numCallibrationSteps 12 //the number of callibration steps or points
#define numGainStages 4
Serial pc(USBTX, USBRX); //USB serial for PC, to be removed later
AnalogOut waveOut(p18); //for debugging





//--------------------  SPI communication
SPI spi(p5, p6, p7); //MOSI, MISO, SCLK
DigitalOut cs(p8);
DigitalIn button(p21);

//--------------------  LCD display
ST7735_LCD disp( p14, p13, p12, p10, p11); //for digital display
display lcd(&disp);
char* newString(int length); //prototype for newString
char* outputString = newString(64);
char* strength = newString(64);
char* dist = newString(64);

//--------------------  signal-related stuff
AnalogIn input(p15); //pin 15 for analog reading
float32_t waveform[BlockSize]; //array of input data
float32_t postFilterData[BlockSize]; //array of filtered data
bool fullRead; //whether the MBED has finish
bool waitForNext;
int index_g; //tracks the index for the waveform array

//-------------------for distance calculation and calibration
bool adjusting = true; //whether the user is still adjusting the beacon's distance
float minThreshold = .042;
float maxThresholds[numGainStages] = {.400, .400, .400, .400};
float average = 0;
float pastAverage1 = 0;
float pastAverage2 = 0;
int callibrationStep;
int state;
int gainStage;
float gainMultiplier;
float gainCutoffs[numGainStages] = {20.639122, 71.663605, 1486.016724, 8599.633789};
//gainCutoffs = {20.0, 100.0, 1200.0, 10000.0};
float gain1;
float gain0;
//These constants are for linear interpolation for the varius gain stage.  Two linear equations per stage (piecewise)
float linearSamples[numCallibrationSteps];
int callibrationPoints[numCallibrationSteps] = {6, 10, 14, 14, 20, 24, 26, 36, 50, 50, 62, 78};
//callibrationPoints = {6, 10, 14, 14, 20, 24, 26, 36, 50, 50, 62, 78};

//the slopes and y-intercepts and mid voltages.
float mLower[numGainStages] = {-15.322362, -108.246826, -43.797886, 90.550484};
float mUpper[numGainStages] = {-100.010368, -485.365387, -186.721283, -223.606857};
float bLower[numGainStages] = {11.394491, 25.797676, 42.176929, 77.429733};
float bUpper[numGainStages] = {19.101967, 45.996063, 62.333790, 100.10244};
float mid[numGainStages] = {0.091010, 0.053560, 0.141033, 0.170399};
float fix = -0.75;

/*
float m10;
float b10;
float mid1;
float m11;
float b11;

float m20;
float b20;
float mid2;
float m21;
float b21;

float m30;
float b30;
float mid3;
float m31;
float b31;
*/

//------------------------the filter coefficients for FIR filter
float32_t pCoeffs[NumTaps] = 
                {    0.012000000000000,   0.012462263166161,  -0.019562318415964,  -0.026175892863747,
                     0.031654803781611,   0.050648026372209,  -0.032547136829180,  -0.070997780956819,
                     0.032992306874347,   0.094643188024724,  -0.020568171368385,  -0.106071176200193,
                     0.009515198320277,   0.114090808482376,   0.009515198320275,  -0.106071176200193,
                    -0.020568171368382,   0.094643188024728,   0.032992306874351,  -0.070997780956815,
                    -0.032547136829177,   0.050648026372211,   0.031654803781612,  -0.026175892863746,
                    -0.019562318415964,   0.012462263166161,   0.012000000000000                        };
float32_t pState[NumTaps + BlockSize - 1];
//                    */


//-----------------------IIR stuff (if needed)
/*
float32_t pkCoeffs[NumTaps] = 
    {
        1,-2.496708288,3.17779085,-2.022333713,0.6561,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
    };
    
float32_t pvCoeffs[NumTaps] =
    {
        0.0000556000,0.0002167120,0.0004326320,0.0005056930,0.0002111890,-0.0004911030,-0.0013071920,-0.0017060250,-0.0012444070,0.0000684000,0.0016603140,0.0026622100,0.0024306750,0.0009787140,-0.0009787140,-0.0024306750,-0.0026622100,-0.0016603140,-0.0000684000,0.0012444070,0.0017060250,0.0013071920,0.0004911030,-0.0002111890,-0.0005056930,-0.0004326320,-0.0002167120,-0.0000556000
    };
float32_t pState[NumTaps + BlockSize];
//*/


//--------------------------------if needed, the 4kHz FIR filter
/*
float32_t pCoeffs[NumTaps] = 
{
    -0.00130297171184699,    -0.00456436168827987,    -0.00757978930408609,    -0.00696944302000657,
        -0.00100059082174453,    0.00812867271498616, 0.0148953048520266,  0.0137935053264369,
          0.00350484996910501, -0.0112195199182290, -0.0216305356563913, -0.0202538386423356,
           -0.00609419278464673,    0.0137348990478646,  0.0275645559768492,  0.0261107576153156,
             0.00866220574766616, -0.0156131009924596, -0.0324957126350438, -0.0311514181527343,
              -0.0110879396617141, 0.0168179120126559,  0.0362758644669149,  0.0352058948414930,
               0.0132978095684398,  -0.0172706692984796, -0.0386711719606551, -0.0379507530937637,
                -0.0149419841919419, 0.0172996706397712,  0.0400000000000000,  0.0397279151377323,
                  0.0164353142069562,  -0.0164055618588934, -0.0396949785867063, -0.0399629114640568,
                   -0.0172605211576678, 0.0149790280104299,  0.0379815311949588,  0.0386933807609119,
                     0.0172844840085185,  -0.0132904115318555, -0.0352024033389307, -0.0362742608690452,
                      -0.0168170401765007, 0.0110885383139611,  0.0311518509994083,  0.0324959946809230,
                        0.0156132578212073,  -0.00866213238945794,    -0.0261107291487171, -0.0275645472357883,
                         -0.0137348973043660, 0.00609419268963993, 0.0202538383407381,  0.0216305354798053,
                           0.0112195198475825,  -0.00350484999121515,    -0.0137935053321021, -0.0148953048532365,
                            -0.00812867271519995,    0.00100059082171422, 0.00696944302000319, 0.00757978930408577,
                             0.00456436168827984, 0.00130297171184699
    };
    //*/


                    

char* newString(int length) //creates an initialized string of given length 
{
    char* temp = new char[length+1];
    for (int i = 0; i <= length; i++) temp[i] = '\0';
    return temp;
}

/*
std::string convert(float number)
{
    std::ostringstream buffer;
    buffer<<number;
    return buffer.str();
}
*/


/*
    This is a helper function for precision timing of Tickers
*/
void readPoint()
{
    waitForNext = false;    
}


/**
* This function reads one full set of analog data into the uC 
*/
void readSamples()
{
    Ticker sampler; //allows for precision data reading
    waitForNext = true;
    sampler.attach_us(&readPoint, (int) (1000000/f_sampling) ); //read in data according the sampling freq
    for (int i = 0; i < BlockSize; i++)
    {
        while (waitForNext); //wait until the ticker calls for the next sample
        waveform[i] = input.read();
        waitForNext = true;
    }
    sampler.detach();
}

/**
    This function spits out the waveform on the analogOut pin (p18)
    This function will be unused in the final version, but is still usefull for debugging.
    @param array (float32_t *): (array of data) pointer to the data to output over the analogOut pin
    @return none
*/
void outputWaveform(float32_t* array)
{
    Ticker outputter;
    waitForNext = true;
    outputter.attach_us(&readPoint, (int) (1000000/f_sampling) ); //output data according the sampling freq
    for (int i = 0; i < BlockSize; i++)
    {
        while (waitForNext); //wait until the ticker calls for the next data point
        waveOut.write(array[i]);
        waitForNext = true;
    }
    outputter.detach();
}
    
/*
    This method writes to the digital potentiometer (MCP4251)
    @param wiperNo (int): this is the wiper number to write to (either 0 or 1)
    @param kOhms (float): this is the value to set the resistance (in kilo Ohms) between the wiper and terminal B 
        note 
    @return: the integer command actually sent (for debugging)
*/
int setPot(int wiperNo, float kOhms) 
{
    //257 steps (8 bits + 1), see section 7.0 for SPI instructions
    float Rmax = 100000;
    spi.frequency(2000000);
    spi.format(16, 0); //16 bits, mode b00
    float ratio = kOhms * 1000.0 / Rmax;
    if (ratio > 1) ratio = 1;
    if (ratio < 0) ratio = 0;
    int dataBits = (int) (ratio * 0x100);
    int command = wiperNo << 12; //setting the Address and Command bits
    command += dataBits; //add in the data bits (digital settings)
    spi.write(command);    
    return command;
}

/**
    This function uses both sides of the digital pot to produce an overall gain for the circuit.  It uses side1 (post filter) before side0 (prefilter)
    @param gain (float): the overall gain wanted (bound by [1, 10000] inclusive)
*/
void setGain(float gain)
{
    if (gain < 0) return;
    if (gain <= 100.0) //only side1 is used 
    {
        setPot(0, 1.0);
        setPot(1, gain);
    }
    else if (gain <= 10000)
    {
        setPot(1, 100.0);
        setPot(0, gain / 100.0);
    }
    else
    {
        setPot(1, 100.0);
        setPot(0, gain / 100.0);        
    }
}

/*
    This function calculates the RMS (root mean squared) of an array of float data.
    @param array (float32_t *): the array to calculate RMS from
    @return float_32: the resulting RMS value of the given array
*/
float32_t rms(float32_t* array)
{
    float32_t rms = 0;
    for(int i = 0; i < BlockSize; i++)
    {
        rms += array[i]*array[i];
    }
    //pc.printf("Sum of squares %f\n\r", rms);
    return sqrt(rms/BlockSize);
}


/**
    This function will wait for a button press.  It will work 250ms after being called (to reduce double reads)
*/
void waitForButton()
{
    wait_ms(250); //to ward off double reads or sticky buttons
    while(button.read() == 0) wait_ms(10); //poll button press every 10ms
    //char* outputString = newString(32);
    //outputString = "Button pressed.";
    lcd.clearscreen();
    lcd.print("Button pressed.");
    wait_ms(250);
    lcd.clearscreen();
}


/**
    This function takes RMS voltage and estimates the distance using linear interpolations.
    Each gain stage is split into a 2-piece-wise linear funtion for estimation
    @param value (float): the post-filter RMS value
    @return (float): the distance estimate in inches (6 to 84) assuming perfect alignment, or special:
        Special cases: 
            -1: clipping likely, too close adjust to a lower gain stage 
            999: cannot detect signal (too far), adjust to higher gain stage 
*/
float estimateDistance(float value)
{
    //if outside range, then alert to try to adjust the gain settings
    if (value < minThreshold*1.10) return 999;
    if (value > maxThresholds[gainStage]*.97) return -1;
    
    switch (gainStage)
    {
    case 0:
        if (value > mid[0]) return mLower[0]*value + bLower[0] + fix;
        else return mUpper[0]*value + bUpper[0] + fix;
    case 1:
        if (value > mid[1]) return mLower[1]*value + bLower[1] + fix;
        else return mUpper[1]*value + bUpper[1] + fix;
    case 2:
        if (value > mid[2]) return mLower[2]*value + bLower[2] + fix;
        else return mUpper[2]*value + bUpper[2] + fix;
    case 3:
        if (value > mid[3]) return mLower[3]*value + bLower[3] + fix;
        else return mUpper[3]*value + bUpper[3] + fix;
        //*/
    default:
        return 0;
    }
}

/**
    This function takes in a distance estimate and tries to change the gain stage
    @param distance (float): the RMS from estimateDistance    
*/
void adjustGains(float distance)
{
    //pc.printf("GainStage = %d  Distance=%f\n\r", gainStage, distance);
    if (distance == -1) //the special case for clipping
    {
        //pc.printf(" Too close\n\r");
        if (gainStage > 0) gainStage--;
        else lcd.print("Clipping. Back up.");
    }
    else if (distance == 999) //the special case for being too far
    {
        //pc.printf("  Too far.\n\r");
        if (gainStage < numGainStages-1) gainStage++;
        else lcd.print("No beacon found.");
    }
    else lcd.print("");
    setGain( gainCutoffs[gainStage] );
    //return gainStage; 
}

void enforceGainStage()
{
    setGain( gainCutoffs[gainStage] );
}

/**
    This function takes one point of callibration data.
*/
void callibratePoint(float value)
{
    if (callibrationStep%3 == 1) //this looks for the gain stage thresholds
    {
       if (adjusting)
        {
            //outputString = "Turn off the beacon.  Press the button when done.";
            gainStage = callibrationStep/3;
            snprintf(outputString, 32, "%i in.", callibrationPoints[ callibrationStep-1 ]); 
            lcd.calibrationdist(outputString);
            waitForButton();
            adjusting = false;
            state = 2;
            gainMultiplier = .2;
            gainCutoffs[ gainStage ] *= .2;
            enforceGainStage();
        }
        else
        {
            //pc.printf("Av=%f gainStage=%d\n\r", gainMultiplier, gainStage);
            if (pastAverage1*1.15 < average || pastAverage1 < minThreshold*3) //not yet maxed, so bump up gainMultiplier
            {
                gainMultiplier *= 1.2;
                gainCutoffs[ gainStage ] *= 1.2;
                enforceGainStage();
                pastAverage2 = pastAverage1;
                pastAverage1 = average;
            }
            else //move onto next callibration step
            {
                adjusting = true;
                linearSamples[ callibrationStep-1 ] = pastAverage2; //record the intensity from 2 stages ago
                maxThresholds[ gainStage ] = pastAverage1;
                gainCutoffs[ gainStage ] /= 1.44;
                enforceGainStage();
                pastAverage1 = minThreshold;
                pastAverage2 = minThreshold;
                callibrationStep++;
            }
            state = 1;
        }        
    }    
    else //----------regular point-------------------------------
    {
        if (adjusting)
        {
            gainStage = (callibrationStep) / 3;
            snprintf(outputString, 32, "%i", callibrationPoints[ callibrationStep-1 ]); 
            lcd.calibrationdist(outputString);
            waitForButton();
            adjusting = false;
            state = 2;
        }
        else
        {
            enforceGainStage();
            linearSamples[ callibrationStep - 1] = value;
            callibrationStep++; //move to next callibration step
            //get ready for next callibration step
            adjusting = true;
            state = 1;
        }   
    }
}

int main() {
    //arm_iir_lattice_instance_f32* filter1 = new arm_iir_lattice_instance_f32();
    arm_fir_instance_f32* filter = new arm_fir_instance_f32();
    float history[10]; //history of RMS voltages.
    
    state = 0; //which state of the state machine to be in, change to enum if desired
    
    uint16_t numTaps = NumTaps;
    uint32_t blockSize = BlockSize;
    char buffer[32]; //for debugging scanf things
    //char* outputString = newString(30); //string to be printed to the LCD display (or other output)
    //char* strength = newString(32);
    //char* dist = newString(32);
    float32_t estimate = 0;
    float RMS = 0;
    int index_h = 0;
    while(1) 
    {
        switch(state)
        {
        case 0: //initialization
            for (int i = 0; i < NumTaps; i++)
            {
                pCoeffs[i] *= 1.70;
            }
            
            arm_fir_init_f32(filter, numTaps, pCoeffs, pState, blockSize);
            //arm_iir_lattice_init_f32(filter1, numTaps, pkCoeffs, pvCoeffs, pState, blockSize);
            //pc.printf("Pre-attachment");
            spi.frequency(1000000);
            state = 1;
            callibrationStep = 0;
            gainStage = 0;
            gainMultiplier = 1.0;
            //pc.printf("Done with init.\n\r");
            break;
            
        case 1: //callibration
            //pc.printf(" Callibration step: %i\n\r", callibrationStep);
            if (callibrationStep == 0) //calculate the offset (beacon is off, or at infinity)
            {
                setGain( gainCutoffs[0]);
                if (adjusting)
                {
                    //outputString = "Turn off the beacon.  Press the button when done.";
                    lcd.calibrationunl();
                    waitForButton();
                    adjusting = false;
                    state = 2;
                    if (button == 1) //skip callibration and use preset stuff;
                    {
                        callibrationStep = -1;
                    }
                    else fix = 0;
                }
                else
                {

                    minThreshold = average; //the average RMS of background noise
                    //maxThreshold = .400;
                    callibrationStep = 1; //move to next callibration step
                    //get ready for next callibration step
                    adjusting = true;
                    state = 1;
                }
            }            
            else if (callibrationStep <= numCallibrationSteps)
            {
                callibratePoint(average);
            }
            else //now all the points are captured, so create the coeffs
            {
                //pc.printf("calculating coeffs\n\r");
                for (int i = 0; i < numGainStages; i++)
                {
                    mid[i] = linearSamples[i*3+1];
                    mLower[i] = (callibrationPoints[i*3+1] - callibrationPoints[i*3+0]) / (linearSamples[i*3+1] - linearSamples[i*3+0]) ;
                    mUpper[i] = (callibrationPoints[i*3+2] - callibrationPoints[i*3+1]) / (linearSamples[i*3+2] - linearSamples[i*3+1]) ;
                    bLower[i] = callibrationPoints[i*3+0] - mLower[i]*linearSamples[i*3+0];
                    bUpper[i] = callibrationPoints[i*3+1] - mUpper[i]*linearSamples[i*3+1];
                    pc.printf("mL=%f mU=%f bL=%f, bU=%f, mid=%f, cutoff=%f\n\r", mLower[i], mUpper[i], bLower[i], bUpper[i], mid[i], gainCutoffs[i]);
                }                
                callibrationStep = -1;
                state = 2;
                gainStage = 0;
                
                for (int i = 0; i < numCallibrationSteps; i++)
                {
                    pc.printf("linear(x)=%f callibration(y)=%d \n\r", linearSamples[i], callibrationPoints[i]);
                }
                //pc.printf("End of callibration.\n\r");
            }
            
        case 2: //read data, take samples
            //pc.printf("Reading data.\n\r");
            readSamples();
            state = 3;
            break;
        case 3: //filter?
            //pc.printf("RMS of waveform = %f\n\r", rms(waveform));
            //pc.printf("Filtering?\n\r");
            arm_fir_f32(filter, waveform, postFilterData, blockSize);
            //arm_iir_lattice_f32(filter1, waveform, postFilterData, blockSize);
            RMS = rms(postFilterData);
            estimate = estimateDistance(RMS);
            if (callibrationStep == -1) state = 6; //done with callibration
            else state = 7; //still callibrating
            break;
        case 4: //FFT?
            break;
        case 5: //output, write to display and PWM tone
            /*
            sprintf(outputString, "RMS = %f", estimate);
            lcd.print(outputString);
            state = 1;
            //*/
            break;
        case 6: //calculate the average voltage 
            //pc.printf("post filter RMS = %f\n\n\r", estimate);
            adjustGains(estimate);
            state = 8;
            break;
        case 7: //callibration-related, take 10pt average and record it
            history[index_h] = RMS;
            index_h++;
            state = 2;
            if (index_h >= 10) //ten-pt average done
            {
                average = 0;
                for (int i = 0; i < 10; i++) average+= history[i];
                average /= 10;
                //pc.printf("10-pt average of RMS = %f\n\r", average);
                float t = (float) average;
                int n = snprintf(strength, 32," %f", t);
                lcd.displayStr(strength);
                index_h = 0;
                state = 1; //go back to callibration
            }
            //state is 2, unless 10pts are collected, then state is 1
            //continue taking and filtering data until full of 10pts
            break;
        case 8: //output
            //int n = sprintf(outputString, "RMS = %f, distance = %fin", RMS, estimate);
            //pc.printf("   RMS=%f, Dist=%f GainStage=%d\n\r", RMS, estimate, gainStage);
            //strcpy( strength, (convert(RMS) + "\0").c_str() );
            
            
            //snprintf(strength, 32, " %f\0", RMS);
            /*
            if (estimate == -1) dist = " Unknown (clipping)\0";
            else if (estimate == 999) dist = " Unknown (no sig)\0";
            else */
            //strcpy( dist, (convert(estimate) + "in\0").c_str() );
            snprintf(strength, 32, " %f\0", RMS);
            snprintf(dist, 32, " %.1f in.\0", estimate);
            //*/
            //pc.printf( strength);
            //pc.printf( dist);
            lcd.displayStr(strength);
            lcd.displayDist(dist);
            
            /*
            if (button == 1) state = 9;
            else state = 2;
            //*/
            state = 2;
            //pc.printf(" end of display\n\r");
            break;
        case 9: //digital pot interfacing and calibration
            pc.printf("Gain?\n\r");
            pc.scanf("%s", buffer);
            float value = atof(buffer);
            setGain(value);
            //int side = (int) value;
            //float k = (value - side) * 100;
            //pc.printf("Command: %x Scanned:%d %f\n\r", setPot(side, k), side, k);
            pc.printf("Scanned:%f\n\r", value);
            //lcd.print("Press button to continue.");
            //waitForButton();
            state = 2;
            break;
        case 10: 
            state = 10;
            break;
        default:
            break;
        }
    } //end of (infinite) while loop
}


//-----------------------------Unused code, but potentially useful

            /*
            double sum = 0;
            for (int i = 0; i < BlockSize; i++) sum += postFilterData[i];
            double average = sum/BlockSize*3.3;  //*3.3 V_ref (array stored as fractions of V_ref)
            pc.printf("Average = %f\n\r", average);
            wait_ms(500);
            state = 2;
            */
            
            //pc.printf("into print\n\r");
            /*
            for (int i = 0; i < BlockSize; i++)
            {
                pc.printf("Waveform contents:%f\n\r", waveform[i]);
            }
            */
            
            
            
/*---------------peak detection
            pc.printf("Into estimation\n\r");
            int peaks = 0;
            float sum = 0.0;
            float prev, current, next;
            for (int i = 0+1; i < BlockSize-1; i++)
            {
                prev = postFilterData[i-1];
                current = postFilterData[i];
                next = postFilterData[i+1];
                if (prev < current && next < current) //local max
                {
                    sum += current;
                    peaks++;
                }
            }
            float average = sum/peaks;
            pc.printf("Average of peaks (scalar) = %f\n\r", average);
            state = 1;
            //*/

/*---------------------------//purely for testing that the digital potentiometer is working.
            pc.printf("Start of digital pot loop.\n\r");
            setPot(1,0);
            wait_ms(1000);
            setPot(1,20);
            wait_ms(1000);
            setPot(1,40);
            wait_ms(1000);
            setPot(1,50);
            wait_ms(1000);
            setPot(1, 80);
            wait_ms(1000);
            setPot(1, 100);
            wait_ms(1000);
            */
            
             /*
             mLower = {-15.322362, -108.246826, -43.797886, 90.550484};
             mUpper = {-100.010368, -485.365387, -186.721283, -223.606857};
             bLower = {11.394491, 25.797676, 42.176929, 77.429733};
             bUpper = {19.101967, 45.996063, 62.333790, 100.10244};
             mid = {0.091010, 0.053560, 0.141033, 0.170399};
             gainCutoffs = {20.639122, 71.663605, 1486.016724, 8599.633789};
mL=-15.322362 mU=-100.010368 bL=11.394491, bU=19.101967, mid=0.091010, cutoff=20.639122
mL=-108.246826 mU=-485.365387 bL=25.797676, bU=45.996063, mid=0.053560, cutoff=71.663605
mL=-43.797886 mU=-186.721283 bL=42.176929, bU=62.333790, mid=0.141033, cutoff=1486.016724
mL=-90.550484 mU=-223.606857 bL=77.429733, bU=100.102448, mid=0.170399, cutoff=8599.633789
linear(x)=0.352067 callibration(y)=6
linear(x)=0.091010 callibration(y)=10
linear(x)=0.051014 callibration(y)=14
linear(x)=0.108989 callibration(y)=14
linear(x)=0.053560 callibration(y)=20
linear(x)=0.045319 callibration(y)=24
linear(x)=0.369354 callibration(y)=26
linear(x)=0.141033 callibration(y)=36
linear(x)=0.066055 callibration(y)=50
linear(x)=0.302922 callibration(y)=50
linear(x)=0.170399 callibration(y)=62
linear(x)=0.098845 callibration(y)=78
                
                
                
                
                
/*
std::string convert(float number)
{
    std::ostringstream buffer;
    buffer<<number;
    return buffer.str();
}





*/