87-108 Mhz (70-108 possible) a spectrum analyzer with TEA5767 and OLED display

/* * * * * * * * *
  RF Electronics
  Spectrum Scanner
  
  v.0.0.5
 * * * * * * * * */

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

//#include "rfe.h" // rfe logo


#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#if (SSD1306_LCDHEIGHT != 64)
#error("Height incorrect, please fix Adafruit_SSD1306.h!");
#endif

/****
***/

int dw;
const float pi = 6.047619047619048;

byte buffer[5];
byte signal;

byte firstRun = 1;
byte prev[2];

bool disp = false;

/***
****/

void setup()
{ 
  // Display init
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); //0x3D too
  
  // Clear the buffer.
  display.clearDisplay();

  // draw welcome logo
  //display.invertDisplay(true);
  //display.drawBitmap(0, 0,  rfe, 128, 64, 1);
  //display.display();
  //delay(2000);
  //display.invertDisplay(false);
  //display.clearDisplay();

  //loading_animation();
  
  display.setTextSize(1);
  display.setTextColor(WHITE);
  
  dw = display.width();
  
  TEA_init();
}

void loop()
{
  display.setCursor(0,0);
  display.println("87M ____________ 108M");
  
  for(int i=0; i<dw; i++)
  {
    
    //radio.selectFrequency( round((float)i/pi * 10) / 10.0 + 87.00 ); // pi = (dw-1)/(108-87)
    //radio.selectFrequency( round_to_dp((float)i/pi, 2) + 87.00 ); // pi = (dw-1)/(108-87)
    //radio.selectFrequency( i/6 + 87 );
    //setFrequency( (float)(i/6 + 87) );
    //setFrequency( round_to_dp((float)i/pi, 2) + 87.00 );
    setFrequency( round((float)i/pi * 10) / 10.0 + 87.00 );
    
    
    Wire.requestFrom(0x60,5); //reading TEA5767
    while(!Wire.available()) ;
    for (int i=0; i<5; i++)
      buffer[i]= Wire.read();
    signal = buffer[3];
    
    
    //display.drawPixel(i, 62-radio.getPowerLevel()/5, WHITE);
    if(firstRun)
    {
      display.drawPixel(i, 63-signal/13, WHITE);
      firstRun = 0;
    }
    else
      display.drawLine(prev[0], prev[1], i, 63-signal/13, WHITE);
    prev[0] = i;
    prev[1] = 63-signal/13;
    
    display.display();
    
  }
  
  prev[0] = 0;
  prev[1] = 63;
  firstRun = 1;
  display.invertDisplay(disp);
    if(disp) disp=false;
    else disp=true;
  
  display.clearDisplay(); 
}

void setFrequency(float frequency)
{
  unsigned int frequencyB;
  unsigned char frequencyH=0;
  unsigned char frequencyL=0;
  
  frequencyB = 4 * (frequency * 1000000 + 225000) / 32768;
  frequencyH = frequencyB >> 8;
  frequencyL = frequencyB & 0XFF;
  //delay(100);
  Wire.beginTransmission(0x60);
  Wire.write(frequencyH);
  Wire.write(frequencyL);
  Wire.write(0xB0);
  Wire.write(0x10);
  Wire.write((byte)0x00);
  Wire.endTransmission();
  //delay(100); 
}

void TEA_init()
{
  unsigned int frequencyB;
  unsigned char frequencyH=0;
  unsigned char frequencyL=0;
  float frequency=87.5;

  frequencyB=4*(frequency*1000000+225000)/32768; //calculating PLL word
  frequencyH=frequencyB>>8;
  frequencyL=frequencyB&0XFF;

  delay(100);

  TWBR = (F_CPU/400000L - 16)/2;
  Wire.beginTransmission(0x60);   //writing TEA5767

  Wire.write(frequencyH);
  Wire.write(frequencyL);
  Wire.write(0xB0);
  Wire.write(0x10);
  Wire.write(0x00);
  
  Wire.endTransmission();

  delay(100);
}

float round_to_dp( float in_value, int decimal_place )
{
	float multiplier = (float)pow( 10.0f, decimal_place );
	in_value = ((float)round( in_value * multiplier )) / multiplier;
	return in_value;
}