Diff: main.cpp

Revision:

6:8441a6864784

Parent:

5:bc45ed158abf

Child:

7:fc55813f823e

--- a/main.cpp	Tue Apr 15 20:46:22 2014 +0000
+++ b/main.cpp	Thu Apr 24 15:39:06 2014 +0000
@@ -2,20 +2,27 @@
 #include "FIR_f32.h"
 #include "arm_math.h"
 #include "display.h"
+#include "st7735.h"
 #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 6 //the number of callibration steps or points
+#define numGainStages 2
 Serial pc(USBTX, USBRX); //USB serial for PC, to be removed later
 AnalogOut waveOut(p18); //for debugging
 
 //--------------------  SPI communication
-SPI spi(p5, p6, p7);
+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(32);
+char* strength = newString(32);
+char* dist = newString(32);
 
 //--------------------  signal-related stuff
 AnalogIn input(p15); //pin 15 for analog reading
@@ -25,14 +32,48 @@
 bool waitForNext;
 int index_g; //tracks the index for the waveform array
 
-//-------------------the tuning constants for distance calculation
+//-------------------for distance calculation and calibration
+bool adjusting = true; //whether the user is still adjusting the beacon's distance
 float minThreshold;
 float maxThreshold;
+int callibrationStep;
+int state;
+int gainStage;
+float gainMultiplier;
+float gainCutoffs[numGainStages] = {20.0, 100.0};
+//gainCutoffs = {20.0, 100.0, 1200.0, 10000.0};
 float gain1;
-float gain2;
-float c1;
-float c2;
-float c3;
+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};
+//callibrationPoints = {6, 10, 14, 14, 20, 24, 26, 36, 50, 50, 62, 78};
+
+float mLower[numGainStages]; //m (slope)  lower portion
+float bLower[numGainStages]; //b (y-offset) lower portion
+float mid[numGainStages]; //the middle x-value for the piecewise
+float mUpper[numGainStages]; //m (slope) upper portion
+float bUpper[numGainStages]; //b (y-offset) upper portion
+
+/*
+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] = 
@@ -89,6 +130,13 @@
 
                     
 
+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;
+}
+
 
 /*
     This is a helper function for precision timing of Tickers
@@ -141,7 +189,7 @@
     @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)
+    @return: the integer command actually sent (for debugging)
 */
 int setPot(int wiperNo, float kOhms) 
 {
@@ -159,6 +207,30 @@
     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
@@ -175,89 +247,227 @@
     return sqrt(rms/BlockSize);
 }
 
-void calibrate()
+
+/**
+    This function will wait for a button press.  It will work 250ms after being called (to reduce double reads)
+*/
+void waitForButton()
 {
-    gain1 = 1.0;
-    gain2 = 25.0;
-    minThreshold = .0275;
-    maxThreshold = .1850;
+    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.print("Button pressed.");
+}
+
+
+/**
+    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 > maxThreshold*.97) return -1;
+    
+    switch (gainStage)
+    {
+    case 0:
+        if (value > mid[0]) return mLower[0]*value + bLower[0];
+        else return mUpper[0]*value + bUpper[0];
+    case 1:
+        if (value > mid[1]) return mLower[1]*value + bLower[1];
+        else return mUpper[1]*value + bUpper[1];
+    /*
+    case 2:
+        if (value > mid[2]) return mLower[2]*value + bLower[2];
+        else return mUpper[2]*value + bUpper[2];
+    case 3:
+        if (value > mid[3]) return mLower[3]*value + bLower[3];
+        else return mUpper[3]*value + bUpper[3];
+        */
+    default:
+        return 0;
+    }
 }
 
-int adjustGains(float estimate)
+/**
+    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)
 {
-    if (estimate < minThreshold)
+    pc.printf("GainStage = %d  Distance=%f\n\r", gainStage, distance);
+    if (distance == -1) //the special case for clipping
     {
-        if (gain2 < 100) //post amp not yet maxed
+        pc.printf(" Too close\n\r");
+        if (gainStage <= 0) lcd.print(" Too close.  Please back up.");
+        else 
         {
-            
+            gainStage--;
         }
-        else;
     }
-    else if (estimate > maxThreshold)
+    if (distance == 999) //the special case for being too far
     {
-        
+        pc.printf("  Too far.\n\r");
+        if (gainStage >= numGainStages - 1) lcd.print("Beacon not detected.");
+        else gainStage++;
     }
-    
+    setGain( gainCutoffs[gainStage] );
+    //return gainStage; 
 }
 
-float estimateDistance(float estimate)
+void enforceGainStage()
+{
+    setGain( gainCutoffs[gainStage] * gainMultiplier);
+}
+
+/**
+    This function takes one point of callibration data.
+*/
+void callibratePoint(float value)
 {
-    return estimate;
+    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; //history of RMS voltages.
-    int index_h = 0;
-    int state = 0; //which state of the state machine to be in, change to enum if desired
+    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; //string to be printed to the LCD display (or other output)
+    //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;
+    float average = 0;
     while(1) 
     {
-        //pc.printf("While loop\n\r");
         switch(state)
         {
         case 0: //initialization
-            calibrate();
-            
             for (int i = 0; i < NumTaps; i++)
             {
-                pCoeffs[i] *= 1.3;
+                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);
-            setPot(1, gain1);
-            setPot(0, gain2);
             state = 1;
+            callibrationStep = 0;
+            gainStage = 0;
+            gainMultiplier = 1.0;
             pc.printf("Done with init.\n\r");
             break;
             
-        case 1: //read data, take samples
+        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;
+                }
+                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 == 1) //at 6in, adjust gain scaling and take one datapoint
+            {       
+                /*
+                gain1 = 20.0;
+                gain0 = 1.0;
+                setPot(1, gain1);
+                setPot(0, gain0);
+                */
+                setGain( 20 );
+                callibratePoint(average);
+            }
+            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\n\r", mLower[i], mUpper[i], bLower[i], bUpper[i], mid[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 2: //printout to pc connection or other output debugging
-            
-            for (int i = 0; i < 10; i++) outputWaveform(postFilterData);
-            wait_ms(500);
-            state = 1;
-            break;
         case 3: //filter?
-            pc.printf("RMS of waveform = %f\n\r", rms(waveform));
-            pc.printf("Filtering?\n\r");
+            //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);
-            state = 6;
+            RMS = rms(postFilterData);
+            estimate = estimateDistance(RMS);
+            if (callibrationStep == -1) state = 6; //done with callibration
+            else state = 7; //still callibrating
             break;
         case 4: //FFT?
             break;
@@ -269,46 +479,61 @@
             //*/
             break;
         case 6: //calculate the average voltage 
-            
-            estimate = rms(postFilterData);
-            pc.printf("post filter RMS = %f\n\n\r", estimate);
-            state = 1;
+            //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);
             /*
-            array[index_h] = estimate;
-            index_h++;
+            snprintf(strength, 32, " %f", RMS);
+            if (estimate == -1) dist = " Unknown (clipping)";
+            else if (estimate == 999) dist = " Unknown (no sig)";
+            else snprintf(dist, 32, " %f in.", estimate);
             */
-            
+            pc.printf( strength);
+            pc.printf( dist);
+            lcd.displayStr(strength);
+            lcd.displayDist(dist);
+            //lcd.print(outputString);
+            if (button == 1) state = 9;
+            else state = 2;
+            pc.printf(" end of display\n\r");
             break;
-        case 7: //estimate amplitude using simple peak detection (consider discarding)
-            
-            break;
-            
-        case 8: //digital pot interfacing and calibration
-            pc.printf("kOhms?\n\r");
+        case 9: //digital pot interfacing and calibration
+            pc.printf("Gain?\n\r");
             pc.scanf("%s", buffer);
             float value = atof(buffer);
-            pc.printf("Command: %x Scanned:%f\n\r", setPot(1, value), value);
-            wait_ms(250);
-            state = 8;
+            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 9:
-            
-        case 10: //purely for testing that the digital potentiometer is working.
-            pc.printf("Start of 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);
+        case 10: 
             state = 10;
             break;
         default:
@@ -339,8 +564,7 @@
             
             
             
-            /*
----------------peak detection
+/*---------------peak detection
             pc.printf("Into estimation\n\r");
             int peaks = 0;
             float sum = 0.0;
@@ -360,3 +584,45 @@
             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);
+            */
+            
+             /*               
+                m00 = -15.221;
+                b00 = 10.836;
+                mid0 = .088;
+                m01 = -142.2;
+                b01 = 22.101;
+
+                m10 = -48.639;
+                b10 = 22.128;
+                mid1 = .068;
+                m11 = -363.74;
+                b11 = 22.352;
+
+                m20 = -45.513;
+                b20 = 39.895;
+                mid2 = .115;
+                m21 = -314.87;
+                b21 = 70.387;
+
+                m30 = -81.809;
+                b30 = 76.868;
+                mid3 = .194;
+                m31 = -201.48;
+                b31 = 99.556;
+*/ 
\ No newline at end of file