Skip to content

Instantly share code, notes, and snippets.

/gist:1a9c58c6035d442a6a3ad7d73f27d6c0

Created December 13, 2016 19:13
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save anonymous/1a9c58c6035d442a6a3ad7d73f27d6c0 to your computer and use it in GitHub Desktop.
Save anonymous/1a9c58c6035d442a6a3ad7d73f27d6c0 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
gistfile1.txt
//
// Simple Raspberry Pi2 Audio Spectrum Analyser
// The analog to digital sampling is done on a
// coprocessor board using an MSP430 microcontroller
// connected to the RP2 with the i2c bus.
//
// Written By: CFLanger
// 10jun15
//
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <unistd.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include <wiringPi.h>
#include <wiringPiSPI.h>
#include <wiringPiI2C.h>
#include <pthread.h>
#include "lcd_fonts.h"
#include "lcd_Fonts.c"
#include "mailbox.h"
#include "gpu_fft.h"
#define TOTAL_ADC_SAMPS 4096
#define MAX_I2C_READ_SIZE 8192
#define FFT_N 4096
#define SAMPLING_RATE 44500.0 // Samples/second
#define HZ_PER_BIN SAMPLING_RATE / FFT_N
#define LCD_CS_HI digitalWrite(5, HIGH);
#define LCD_CS_LO digitalWrite(5, LOW);
#define LCD_RS_HI digitalWrite(6, HIGH);
#define LCD_RS_LO digitalWrite(6, LOW);
#define LCD_DISP_OFF 0xAE // turn LCD panel OFF
#define LCD_DISP_ON 0xAF // turn LCD panel ON
#define LCD_SET_LINE 0x40 // set line for COM0 (6 lsbs = ST5:ST4:ST3:ST2:ST1:ST0)
#define LCD_SET_PAGE 0xB0 // set page address (4 lsbs = P3:P2:P1:P0)
#define LCD_SET_COL_HI 0x10 // set column address (4 lsbs = Y7:Y6:Y5:Y4)
#define LCD_SET_COL_LO 0x00 // set column address (4 lsbs = Y3:Y2:Y1:Y0)
#define LCD_SET_ADC_NOR 0xA0 // ADC set for normal direction
#define LCD_SET_ADC_REV 0xA1 // ADC set for reverse direction
#define LCD_DISP_NOR 0xA6 // normal pixel display
#define LCD_DISP_REV 0xA7 // reverse pixel display
#define LCD_EON_OFF 0xA4 // normal display mode
#define LCD_EON_ON 0xA5 // entire dsplay on
#define LCD_SET_BIAS_0 0xA2 // set LCD bias select to 0
#define LCD_SET_BIAS_1 0xA3 // set LCD boas select to 1
#define LCD_SET_MODIFY 0xE0 // set modfy read mode
#define LCD_CLR_MODIFY 0xEE // clear modify read mode
#define LCD_RESET 0xE2 // soft reset command
#define LCD_SET_SHL_NOR 0xC0 // set COM direction normal
#define LCD_SET_SHL_REV 0xC8 // set COM direction reverse
#define LCD_PWR_CTL 0x28 // set power control (3 lsbs = VC:VR:VF)
#define LCD_REG_RESISTOR 0x20 // set regulator resistor (3 lsbs = R2:R1:R0)
#define LCD_REF_VOLT_MODE 0x81 // set reference voltage mode (first of dual command)
#define LCD_REF_VOLT_REG 0x00 // set reference voltage register (6 lsbs = SV5:SV4:SV3:SV2:SV1:SV0)
#define LCD_ST_IND_MODE_0 0xAC // set static indicator mode to 0
#define LCD_ST_IND_MODE_1 0xAD // set static indicator mode to 1
#define LCD_ST_IND_REG 0x00 // set the static indicator register (2 lsbs = S1:S0)
#define LCD_NOP 0xE3 // LCD controller NOP command
#define LCD_TEST_CMD_1 0xF0 // LCD Test Instruction 1.. do not use
#define LCD_TEST_CMD_2 0x90 // LCD Test Instruction 2.. do not use
#define X_BYTES 128
#define Y_BYTES 8
#define SCRN_LEFT 0
#define SCRN_TOP 0
#define SCRN_RIGHT 127
#define SCRN_BOTTOM 63
const unsigned char img_rplogo_bmp_char_table[] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* [ ] */
0x03, 0xFF, 0xE0, 0x07, 0xFF, 0x80, /* [ ************* ************ ] */
0x07, 0xFF, 0xF8, 0x1F, 0xFF, 0xE0, /* [ **************** **************** ] */
0x18, 0x00, 0x14, 0x38, 0x00, 0x18, /* [ ** * * *** ** ] */
0x18, 0x00, 0x06, 0x60, 0x00, 0x18, /* [ ** ** ** ** ] */
0x3C, 0x00, 0x06, 0xE0, 0x00, 0x38, /* [ **** ** *** *** ] */
0x3C, 0x40, 0x06, 0xE0, 0x02, 0x38, /* [ **** * ** *** * *** ] */
0x1C, 0x0C, 0x03, 0xC0, 0x30, 0x38, /* [ *** ** **** ** *** ] */
0x1C, 0x04, 0x03, 0xC0, 0x60, 0x78, /* [ *** * **** ** **** ] */
0x1C, 0x03, 0x03, 0xC0, 0x80, 0x38, /* [ *** ** **** * *** ] */
0x1C, 0x00, 0xE3, 0xC7, 0x00, 0x70, /* [ *** *** **** *** *** ] */
0x0E, 0x00, 0x77, 0xEC, 0x00, 0x70, /* [ *** *** ****** ** *** ] */
0x07, 0x00, 0x1F, 0xF8, 0x00, 0xE0, /* [ *** ********** *** ] */
0x03, 0x80, 0x1F, 0xF0, 0x01, 0x80, /* [ *** ********* ** ] */
0x03, 0x80, 0x1F, 0xF8, 0x03, 0x80, /* [ *** ********** *** ] */
0x01, 0xE0, 0x3F, 0xF8, 0x07, 0x80, /* [ **** *********** **** ] */
0x00, 0xF0, 0x7F, 0xFE, 0x0E, 0x00, /* [ **** ************** *** ] */
0x00, 0x7F, 0xFE, 0xFF, 0xFC, 0x00, /* [ ************** ************** ] */
0x00, 0x7F, 0xF8, 0x1F, 0xFC, 0x00, /* [ ************ *********** ] */
0x00, 0xFD, 0xF0, 0x1F, 0x7E, 0x00, /* [ ****** ***** ***** ****** ] */
0x01, 0x81, 0xC0, 0x07, 0x07, 0x00, /* [ ** *** *** *** ] */
0x03, 0x03, 0xC0, 0x07, 0x83, 0x80, /* [ ** **** **** *** ] */
0x03, 0x03, 0xC0, 0x07, 0x81, 0x80, /* [ ** **** **** ** ] */
0x07, 0x0F, 0xF8, 0x1F, 0xE1, 0x80, /* [ *** ********* ******** ** ] */
0x06, 0x1F, 0xFF, 0xFF, 0xE1, 0xC0, /* [ ** ************************ *** ] */
0x06, 0x3F, 0xBF, 0xF8, 0x79, 0xC0, /* [ ** ******* *********** **** *** ] */
0x06, 0xF8, 0x07, 0xE0, 0x3E, 0xC0, /* [ ** ***** ****** ***** ** ] */
0x07, 0xF0, 0x07, 0xE0, 0x1F, 0xC0, /* [ ******* ****** ******* ] */
0x0F, 0xF0, 0x07, 0xC0, 0x1F, 0xF0, /* [ ******** ***** ********* ] */
0x39, 0xC0, 0x03, 0xC0, 0x06, 0x38, /* [ *** *** **** ** *** ] */
0x30, 0xC0, 0x03, 0xC0, 0x02, 0x18, /* [ ** ** **** * ** ] */
0x60, 0x80, 0x03, 0xC0, 0x02, 0x0C, /* [ ** * **** * ** ] */
0x60, 0x80, 0x03, 0xC0, 0x02, 0x0C, /* [ ** * **** * ** ] */
0x60, 0x80, 0x07, 0xE0, 0x02, 0x0C, /* [ ** * ****** * ** ] */
0x60, 0x80, 0x07, 0xE0, 0x02, 0x0C, /* [ ** * ****** * ** ] */
0x41, 0xC0, 0x07, 0xE0, 0x03, 0x0C, /* [ * *** ****** ** ** ] */
0x61, 0xE0, 0x07, 0xE0, 0x07, 0x0C, /* [ ** **** ****** *** ** ] */
0x61, 0xE0, 0x0F, 0xF0, 0x0F, 0x0C, /* [ ** **** ******** **** ** ] */
0x63, 0xFF, 0xF8, 0x1F, 0xFF, 0x88, /* [ ** *************** ************** * ] */
0x33, 0xFF, 0xE0, 0x07, 0xFF, 0x98, /* [ ** ************* ************ ** ] */
0x3F, 0xFF, 0xE0, 0x07, 0xFD, 0xF8, /* [ ***************** ********* ****** ] */
0x3F, 0xFF, 0xC0, 0x07, 0xF0, 0x70, /* [ **************** ******* *** ] */
0x1C, 0x0F, 0xC0, 0x03, 0xC0, 0x70, /* [ *** ****** **** *** ] */
0x1C, 0x07, 0xC0, 0x03, 0xC0, 0x70, /* [ *** ***** **** *** ] */
0x1C, 0x03, 0xC0, 0x03, 0x80, 0x70, /* [ *** **** *** *** ] */
0x0C, 0x03, 0xC0, 0x03, 0x80, 0x70, /* [ ** **** *** *** ] */
0x0C, 0x03, 0xC0, 0x07, 0x80, 0x70, /* [ ** **** **** *** ] */
0x0E, 0x01, 0xC0, 0x07, 0x00, 0x60, /* [ *** *** *** ** ] */
0x07, 0x01, 0xF0, 0x1F, 0x00, 0xE0, /* [ *** ***** ***** *** ] */
0x03, 0x00, 0xF8, 0x1F, 0x01, 0xC0, /* [ ** ***** ***** *** ] */
0x03, 0x81, 0xFF, 0xFF, 0x01, 0x80, /* [ *** ***************** ** ] */
0x03, 0x81, 0xFF, 0xFF, 0x03, 0x80, /* [ *** ***************** *** ] */
0x00, 0xFF, 0xFC, 0x1F, 0xFE, 0x00, /* [ ************** ************ ] */
0x00, 0x7F, 0xF0, 0x07, 0xFE, 0x00, /* [ *********** ********** ] */
0x00, 0x3F, 0xC0, 0x07, 0xF8, 0x00, /* [ ******** ******** ] */
0x00, 0x03, 0xC0, 0x07, 0x80, 0x00, /* [ **** **** ] */
0x00, 0x03, 0xE0, 0x07, 0x80, 0x00, /* [ ***** **** ] */
0x00, 0x00, 0xF0, 0x1E, 0x00, 0x00, /* [ **** **** ] */
0x00, 0x00, 0x3F, 0xF8, 0x00, 0x00, /* [ *********** ] */
0x00, 0x00, 0x1F, 0xF8, 0x00, 0x00, /* [ ********** ] */
0x00, 0x00, 0x07, 0xE0, 0x00, 0x00, /* [ ****** ] */
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* [ ] */
};
unsigned char l_display_array[Y_BYTES][X_BYTES];
const unsigned char l_mask_array[8] = { 0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80 };
unsigned char disp_buf[50];
int graph_data[28];
double dbl_graph_data[FFT_N];
double FFT_windowed_input[FFT_N];
double fft_magnitudes[FFT_N];
float DTFT_input[FFT_N];
double maxval;
int maxbin, gsb;
int m, run, filter;
char hostName[128];
int inputzoom_x0 = 0;
int inputzoom_x1 = 4095;
int outputzoom_x0 = 0;
int outputzoom_x1 = 100;
unsigned char i2c_rbuf[MAX_I2C_READ_SIZE], *ucptr;
int audio_samples[TOTAL_ADC_SAMPS], *iptr;
pthread_t t_MenuThread, t_SampleThread;
pthread_attr_t attr;
//
// IIR Filter Variables
//
#define IIR_NUMBER_OF_STAGES 10 // Define the filter length (SET TO MAXIMUM of all filter coeff sets pulled in !!! )
#define SAMPLE_RATE SAMPLING_RATE // Define the analog sample rate
#define SIGLIB_IIR_DELAY_SIZE 2L // Size of IIR filter state vector
#define SIGLIB_IIR_COEFFS_PER_BIQUAD 5L // Number of coefficients per biquad
float pFilterState[ 2 * IIR_NUMBER_OF_STAGES * SIGLIB_IIR_DELAY_SIZE ]; // Need twice number of poles and zeros for BPFs and BSFs
// High Pass, 10 pole, 40hz
float IIRCoeffs_10p40hz [] = { /* IIR Coefficient array */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.988873, 0.988905, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.988873, 0.988905, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.989953, 0.989985, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.989953, 0.989985, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.992012, 0.992044, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.992012, 0.992044, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.994853, 0.994885, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.994853, 0.994885, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.998203, 0.998235, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.998203, 0.998235, /* Beta coeffs */
};
// Bandpass, 5 pole, 100 hz - 16000hz
float IIRCoeffs_5p10016khz [] = { /* IIR Coefficient array */
1.000000, 2.000000, 1.000000, /* Alpha coeffs */
0.109062, 0.002974, /* Beta coeffs */
1.000000, 2.000000, 1.000000, /* Alpha coeffs */
0.125722, 0.110945 , /* Beta coeffs */
1.000000, 2.000000, 1.000000, /* Alpha coeffs */
0.125722, 0.110945 , /* Beta coeffs */
1.000000, 2.000000, 1.000000, /* Alpha coeffs */
0.184266, 0.534542, /* Beta coeffs */
1.000000, 2.000000, 1.000000, /* Alpha coeffs */
0.184266, 0.534542, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.991202, 0.991401, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.991202, 0.991401 , /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.977130, 0.977330, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.977130, 0.977330, /* Beta coeffs */
1.000000, -2.000000, 1.000000, /* Alpha coeffs */
-1.971604, 0.971806, /* Beta coeffs */
};
void lcd_init(void);
void lcd_erase(void);
void lcd_clear_area(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom);
void lcd_invert_area(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom);
void lcd_horz_line(unsigned char left, unsigned char right, unsigned char row);
void lcd_vert_line(unsigned char top, unsigned char bottom, unsigned char column);
void lcd_clr_horz_line(unsigned char left, unsigned char right, unsigned char row);
void lcd_clr_vert_line(unsigned char top, unsigned char bottom, unsigned char column);
void lcd_box(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom);
void lcd_clr_box(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom);
void lcd_glyph(unsigned char left, unsigned char top, unsigned char width, unsigned char height,
unsigned char const *glyph, unsigned char store_width);
void lcd_text(unsigned char left, unsigned char top, unsigned char font, char *str);
void lcd_update(unsigned char top, unsigned char bottom);
void put_serial_to_lcd(unsigned char data);
void write_input(void);
void write_output(void);
unsigned int microseconds(void);
int get_data(int fd, int bytes_to_read);
void sfiDtft(float f, float *re, float *im);
float ZoomIn(int peak_pt);
void mkWebPage( float zf, float zfreq);
// Fast Approximations from ARM CMSIS DSP LIBRARY
float arm_sin_f32(float x);
float arm_cos_f32(float x);
void SDAIir( float * input_buffer, float * output_buffer, float * pState, float * pCoeffs, int NumberOfStages, int SampleLength);
void SIFIir(float * pState, const int NumberOfStages);
void quit( void );
void * MenuThread(void * tdata);
void * SampleThread(void * tdata);
void mkInpGNUplt( float zbin, float zfreq);
void mkOutpGNUplt( void);
void graph_signal_spectrum( float zf, float zfreq);
void write_graphdata(void);
//
// Do Werk here
//
int main() {
register int i, j, k;
int ret, log2_N, jobs, N, mb = mbox_open();
unsigned t[2];
double max, multiplier;
float zf, freq, f0, f1;
int rc;
long g;
FILE *f;
struct GPU_FFT_COMPLEX *base;
struct GPU_FFT *fft;
log2_N = 12; // 12 = 4096 // 8 <= log2_N <= 21
jobs = 1;
N = 1<<log2_N; // FFT length
ret = gpu_fft_prepare(mb, log2_N, GPU_FFT_FWD, jobs, &fft); // call once
switch(ret) {
case -1: printf("Unable to enable V3D. Please check your firmware is up to date.\n"); return -1;
case -2: printf("log2_N=%d not supported. Try between 8 and 21.\n", log2_N); return -1;
case -3: printf("Out of memory. Try a smaller batch or increase GPU memory.\n"); return -1;
case -4: printf("Unable to map Videocore peripherals into ARM memory space.\n"); return -1;
}
if(wiringPiSetup() == -1) {
printf("Unable to start wiringPi: %s\n");
return -1;
}
// Initialize and set thread detached attribute
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
run = 1;
filter = 0;
gethostname(hostName, 127);
atexit(quit);
wiringPiSPISetup(1,31000000) ; // lcd spi bus
pinMode(5,OUTPUT); // lcd chip select
pinMode(6,OUTPUT); // lcd register select
lcd_init();
lcd_erase();
lcd_glyph(0,0,47,62,img_rplogo_bmp_char_table,6);
lcd_update(SCRN_TOP, SCRN_BOTTOM);
lcd_text(53,3,FONT_TERMINAL_SIX_DOT,"FFT Analysis");
lcd_update(SCRN_TOP,SCRN_BOTTOM);
SIFIir( pFilterState, IIR_NUMBER_OF_STAGES );
// Create Thread
rc = pthread_create(&t_MenuThread, &attr, MenuThread, (void *)g);
if (rc) {
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
// Create Thread
rc = pthread_create(&t_SampleThread, &attr, SampleThread, (void *)g);
if (rc) {
printf("ERROR; return code from pthread_create() is %d\n", rc);
exit(-1);
}
while(run) {
ucptr = &i2c_rbuf[0];
iptr = &audio_samples[0];
// Pack the bytes together
// Samples from ADC are 12 bits
for(m = 0; m < TOTAL_ADC_SAMPS; m++) {
*iptr = (int)(*ucptr++ & 0xFF); // LSB
*iptr++ |= ((int)(*ucptr++ & 0xFF)) << 8; // MSB
}
// Remove DC bias from amplifier (ADC reference = 2.5 VDC, 12 bit samples, FSC=4096)
for(i = 0; i < TOTAL_ADC_SAMPS; i++)
audio_samples[i] -= 2048;
// Fill input arrays
for(i = 0; i < TOTAL_ADC_SAMPS; i++)
DTFT_input[i] = (float)audio_samples[i];
#if 0
// Mix in other sine wave(s) or use for testing digital filter
f = fopen("outputdata.dat", "r+");
for(i = 0; i < TOTAL_ADC_SAMPS; i++) {
// DTFT_input[i] += (1*1000 * sin(65.185547 * (2 * GPU_FFT_PI) * i / SAMPLING_RATE));
fscanf(f, "%d,%f,%f\n", &rc, &f0, &f1);
printf("%d, %f, %f\n", rc,f0,f1);
DTFT_input[i] += (f0*1000 * sin(f1 * (2 * GPU_FFT_PI) * i / SAMPLING_RATE));
}
fclose(f);
#endif
if(filter) {
// Apply digital IIR filter, 40 hz, High Pass Butterworth
SDAIir( DTFT_input, DTFT_input, pFilterState, IIRCoeffs_10p40hz, 10, 4096 );
// SDAIir( DTFT_input, DTFT_input, pFilterState, IIRCoeffs_5p10016khz, 5, 4096 );
}
for(i = 0; i < TOTAL_ADC_SAMPS; i++)
FFT_windowed_input[i] = (double)DTFT_input[i];
// Apply a Hanning window to the input array
for(i = 0; i < FFT_N; i++) {
multiplier = 0.5 * (1 - cos(2 * GPU_FFT_PI * i / FFT_N));
FFT_windowed_input[i] = multiplier * FFT_windowed_input[i];
}
// Instruct the FFT function where the input data is
base = fft->in;
for(i=0; i < N; i++)
base[i].re = base[i].im = 0; // zero imaginary parts
for(i = 0; i < N; i++) {
base[i].re = FFT_windowed_input[i]; // real parts
base[i].im = 0;
}
// Perform the FFT
gpu_fft_execute(fft);
// Point to the output array
base = fft->out;
// Compute "sum of squares" magnitude of the real parts
for(i = 0; i < N; i++)
fft_magnitudes[i] = (base[i].re * base[i].re) + (base[i].im * base[i].im);
// Find the maximum bin energy
k = 0;
j = 0;
max = 0;
for( i= 0; i < N / 2; i++) {
if(fft_magnitudes[i] >= max) {
max=fft_magnitudes[i];
j = i;
}
}
maxbin = j;
maxval = fft_magnitudes[j];
// Normalize
for(i = 0; i < FFT_N/2; i++) {
dbl_graph_data[i] = fft_magnitudes[i];
fft_magnitudes[i] = (1.0 * fft_magnitudes[i]) / maxval;
}
// Find the real peak via golden search
usleep(1); // Yield to OS
t[0] = microseconds(); // Measure how long Zoom function takes
zf = ZoomIn(j);
t[1] = microseconds();
// Save the data for gnuplot
mkInpGNUplt( zf, freq );
write_input( );
// Save the data for gnuplot
mkOutpGNUplt();
write_output();
// Calculate the frequency
freq = HZ_PER_BIN * zf;
// Display the results on 128x64 LCD
sprintf(disp_buf,"jbin=%d ",j);
lcd_text(53,15,FONT_TERMINAL_SIX_DOT,disp_buf);
sprintf(disp_buf,"zbin=%.2f",zf);
lcd_text(53,24,FONT_TERMINAL_SIX_DOT,disp_buf);
sprintf(disp_buf,"us=%u ",(t[1] - t[0]));
lcd_text(53,33,FONT_TERMINAL_SIX_DOT,disp_buf);
sprintf(disp_buf,"Fj=%8.3f",(float)j * HZ_PER_BIN) ;
lcd_text(53,42,FONT_TERMINAL_SIX_DOT,disp_buf);
sprintf(disp_buf,"Fz=%8.3f",freq);
lcd_text(53,51,FONT_TERMINAL_SIX_DOT,disp_buf);
lcd_update(SCRN_TOP,SCRN_BOTTOM);
mkWebPage(zf, freq );
// graph_signal_spectrum( zf, freq );
}
gpu_fft_release(fft); // Videocore memory lost if not freed !
}
//
// The sampling thread
//
void * SampleThread(void * tdata) {
int fd;
if((fd = wiringPiI2CSetup(0x50)) < 0)
printf("I2CSetup Failed, %i\n",fd);
while(run) {
// Start sampling the MSP430 5419A coprocessor (external microcontroller)
wiringPiI2CWrite(fd, 0x20);
get_data( fd, MAX_I2C_READ_SIZE);
}
close(fd);
}
void graph_signal_spectrum( float zf, float zfreq) {
double max;
int s, i;
unsigned char l, t, b;
unsigned int g, v, k;
double ftmp[28];
int centerfreqs[] = { 3,4,5,7,9,11,14,18,23,28,36,46,57,73,92,115,147,184,230,289,368,460,579,736,920,1150,1472,1840 };
for(i = 0; i < 28; i++)
ftmp[i] = dbl_graph_data[ centerfreqs[i] ] ;
// Find the maximum
max = 0;
for( i= 0; i < 28; i++) {
if( ftmp[i] >= max) {
max = ftmp[i] ;
}
}
// Normalize to 50
for(i = 0; i < 28; i++) {
graph_data[i] = (int)( (50.0 * ftmp[i] ) / max );
if(graph_data[i] > 50 ) graph_data[i] = 50;
}
write_graphdata();
lcd_clear_area( SCRN_LEFT, SCRN_TOP, SCRN_RIGHT, SCRN_BOTTOM );
for( g = 0; g < 28; g++ ) {
l = 14 + ( g * 3 );
b = 50 - graph_data[ g ];
lcd_box( l, b, l+2, 50 );
}
lcd_vert_line( 0, 53, 13 ); // VERTICAL Y axis line
lcd_horz_line( 10, 13, 0 );
lcd_horz_line( 10, 13, 10 );
lcd_horz_line( 10, 13, 20 );
lcd_horz_line( 10, 13, 30 );
lcd_horz_line( 10, 13, 40 );
lcd_horz_line( 10, 13, 50 );
// Draw Grid
for( v = 0; v < 5; v++ ) {
for( t = 0, k = 0; k < 6; k++ ) {
lcd_horz_line( 15 + t, 15 + t, v * 10 );
t += 15;
}
}
strcpy( ( char * )disp_buf, "10" );
lcd_text( 0, 0, FONT_TERMINAL_FIVE_DOT, ( char * )disp_buf );
strcpy( ( char * )disp_buf, "8" );
lcd_text( 3, 10, FONT_TERMINAL_FIVE_DOT, ( char * )disp_buf );
strcpy( ( char * )disp_buf, "6" );
lcd_text( 3, 20, FONT_TERMINAL_FIVE_DOT, ( char * )disp_buf );
strcpy( ( char * )disp_buf, "4" );
lcd_text( 3, 30, FONT_TERMINAL_FIVE_DOT, ( char * )disp_buf );
strcpy( ( char * )disp_buf, "2" );
lcd_text( 3, 40, FONT_TERMINAL_FIVE_DOT, ( char * )disp_buf );
sprintf(disp_buf,"%.2f %8.3f %d",zf, zfreq, filter);
lcd_text(0,57,FONT_TERMINAL_SIX_DOT,disp_buf);
lcd_update( SCRN_TOP, SCRN_BOTTOM );
}
//
// The menu thread
//
void * MenuThread(void * tdata) {
char s[128];
char tmpData[50];
printf("Raspberry Pi - Simple Spectrum Analyzer\n");
printf("=======================================\n");
while(run) {
if(fgets(s, 80, stdin) != NULL) {
if(s[0] == 'q') {
printf("Quiting from q press..\n");
run = 0;
usleep(100000);
break;
} else if(s[0] == 'f' || s[0] == 'F' ) {
filter ^= 1;
} else if( strstr(s, "ix0=") != NULL ) {
strcpy(tmpData, strstr(s, "ix0=") + 4);
inputzoom_x0 = atoi(tmpData);
printf("inputzoom_x0 = %d\n", inputzoom_x0 );
} else if( strstr(s, "ix1=") != NULL ) {
strcpy(tmpData, strstr(s, "ix1=") + 4);
inputzoom_x1 = atoi(tmpData);
printf("inputzoom_x1 = %d\n", inputzoom_x1 );
} else if( strstr(s, "ox0=") != NULL ) {
strcpy(tmpData, strstr(s, "ox0=") + 4);
outputzoom_x0 = atoi(tmpData);
printf("outputzoom_x0 = %d\n", outputzoom_x0 );
} else if( strstr(s, "ox1=") != NULL ) {
strcpy(tmpData, strstr(s, "ox1=") + 4);
outputzoom_x1 = atoi(tmpData);
printf("outputzoom_x1 = %d\n", outputzoom_x1 );
} else if(s[0] == 'r' || s[0] == 'R' ) {
inputzoom_x0 = 0;
inputzoom_x1 = 4095;
outputzoom_x0 = 0;
outputzoom_x1 = 2000;
} else if( strstr(s, "gsb=") != NULL ) {
strcpy(tmpData, strstr(s, "gsb=") + 4);
gsb = atoi(tmpData);
printf("gsb = %d\n", gsb );
}
}
usleep(100000);
}
}
//
//
float ZoomIn(int peak_pt) {
// proprietary, sorry
}
//
// The discrete Time Fourier transform
//
void sfiDtft( float f, float * re, float * im ) {
// proprietary, sorry
}
//
// Measure Time
//
unsigned int microseconds(void) {
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
return ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
}
//
// Get data from i2c i/o stream
//
int get_data( int fd, int bytes_to_read) {
int i;
int bytes_read;
int rr = 500;
char * wp;
bytes_read = 0;
i = read(fd, i2c_rbuf, bytes_to_read);
if(i > 0)
bytes_read += i;
while (--rr && bytes_read < bytes_to_read) {
usleep(100);
i = read(fd,&(i2c_rbuf[bytes_read]),bytes_to_read - bytes_read);
if(i > 0)
bytes_read += i;
}
return bytes_read;
}
//
// Write Data to file
//
void write_input(void) {
FILE *f;
int i;
f = fopen("inputdata.dat", "w+");
if(f == NULL) {
printf("File open error for write %s\n", "inputdata.dat");
return;
}
for(i = 0; i < FFT_N; ++i) {
fprintf(f,"%d %f\n",i,FFT_windowed_input[i]);
}
fclose(f);
}
//
// Write LCD Graph Data to file
//
void write_graphdata(void) {
FILE *f;
int i;
f = fopen("graphdata.dat", "w+");
if(f == NULL) {
printf("File open error for write %s\n", "graphdata.dat");
return;
}
for(i = 0; i < 28; ++i) {
fprintf(f,"%d %d\n",i,graph_data[i]);
}
fclose(f);
}
//
// Write Data to file
//
void write_output(void) {
FILE *f;
int i;
f = fopen("outputdata.dat", "w+");
if(f == NULL) {
printf("File open error for write %s\n", "outputdata.dat");
return;
}
for(i = 0; i < FFT_N / 2; ++i) {
fprintf(f,"%d %f %f\n",i, fft_magnitudes[i], i * HZ_PER_BIN);
}
fclose(f);
}
//
// Make the web page showing the graphs
//
void mkWebPage( float zbin, float zfreq ) {
FILE *f;
int x = 0;
time_t tt = time(NULL);
struct tm tm = *localtime(&tt);
char tbuf[128];
system("gnuplot inputdata.plt");
system("gnuplot outputdata.plt");
f = fopen("evotmp.html", "w");
if(f == NULL) {
printf("File open error %s\n", "evotmp.html");
return;
}
fputs("<!doctype html>", f);
fputs("<html>", f);
fputs("<head>", f);
fputs("<meta charset=UTF-8>", f);
fprintf(f, "<meta %s%c%c\n", "Content-Type:text/html;charset=iso-8859-1>", 13, 10);
// fputs("<meta http-equiv=Refresh Content=2; URL=index.html>", f);
fprintf(f,"<title>Pievo @%s</title> ", hostName);
fputs("<style type=text/css>", f);
fputs("th { text-align: center; }", f);
fputs("td { text-align: center; }", f);
fputs("p { text-align: center; }", f);
fputs("th { color: black; }", f);
fputs("th { background-color: lightgray; }", f);
fputs("td { font-family: arial; font-size: 14pt; }", f);
fputs("th { font-family: arial; font-size: 12pt; }", f);
fputs("p { font-family: arial; font-size: 11pt; }", f);
fputs("</style>",f);
fputs("</head>", f);
fputs("<body>", f);
fprintf(f,"<h1><center><font size=+3>[Raspberry Pi GPU FFT], Last Update %d-%02d-%02d %02d:%02d:%02d %s</font></h1>", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, tm.tm_zone );
fprintf(f,"<h1><center><font size=+3>Freq at Maxbin = %.3f Hz : Maxbin = %.3f : Filter = %s</font></h1>", zfreq, zbin, (filter)?"ON":"OFF" );
fputs("</table>", f);
fputs("<table align='center'>", f);
fputs("<tr>",f);
fputs("<td><img src=./inputdata.png width=1600 height=500></td>", f);
fputs("</tr>",f);
fputs("<td><img src=./outputdata.png width=1600 height=500></td>", f);
fputs("</tr>",f);
fputs("</table>", f);
fputs("</body>", f);
fputs("</html>", f);
fclose(f);
system("sudo cp evotmp.html /var/www/html/index.html");
system("sudo cp inputdata.png /var/www/html/inputdata.png");
system("sudo cp outputdata.png /var/www/html/outputdata.png");
}
//
// Make gnuplot input.plt file
//
void mkInpGNUplt( float zbin, float zfreq) {
FILE *f;
char tbuf[128];
f = fopen("inputdata.plt", "w+");
if(f == NULL) {
printf("File open error %s\n", "inputdata.plt");
return;
}
fputs("set terminal png large enhanced font arial size 1600,500\n", f);
fputs("set title 'Windowed Samples'\n", f);
fputs("set output 'inputdata.png'\n", f);
fputs("set tics scale 5\n", f);
fputs("set mxtics 5\n", f);
fputs("set mytics 8\n", f);
fputs("unset autoscale\n", f);
fputs("set grid\n", f);
// fprintf(f, "set label 'Freq at Maxbin = %.3f Hz' at 55,3100\n", zfreq );
// fprintf(f, "set label 'Maxbin = %.3f' at 55,2600\n", zbin );
// fprintf(f, "set label 'Filter = %s' at 55,2100\n", (filter)?"ON":"OFF" );
fputs("set ylabel 'ADC Counts'\n", f);
fprintf(f, "set xrange[%d:%d]\n", inputzoom_x0, inputzoom_x1);
fputs("set yrange[-5000:5000]\n", f);
fputs("plot 'inputdata.dat' with lines lw 1 lt rgb 'red'\n", f);
fputs("exit 0", f);
fclose(f);
}
//
// Make gnuplot output.plt file
//
void mkOutpGNUplt( void) {
FILE *f;
char tbuf[128];
f = fopen("outputdata.plt", "w+");
if(f == NULL) {
printf("File open error %s\n", "outputdata.plt");
return;
}
fputs("set terminal png large enhanced font arial size 1600,500\n", f);
fputs("set output 'outputdata.png'\n", f);
fputs("set title 'Frequency Spectrum'\n", f);
fputs("set tics scale 2\n", f);
fputs("set xtics 10\n", f);
fputs("set ytics 0.1\n", f);
fputs("unset autoscale\n", f);
fputs("set grid\n", f);
fputs("set ylabel 'Magnitude'\n", f);
fprintf(f, "set xrange[%d:%d]\n", outputzoom_x0, outputzoom_x1);
fputs("set yrange[0:1]\n", f);
fputs("set style data boxes\n", f);
fputs("set boxwidth 0.4 relative\n", f);
fputs("set style fill solid 1\n", f);
fputs("plot 'outputdata.dat' using 1:2 with boxes lt rgb 'blue'\n", f);
fputs("exit 0", f);
fclose(f);
}
//
// Shutdown gracefully
//
void quit( void ) {
int rc;
long t;
void *status;
printf("test program quiting...\n");
run = 0;
printf("Thread Joining\n");
/* Free attribute and wait for the other threads */
pthread_attr_destroy(&attr);
rc = pthread_join(t_MenuThread, &status);
if (rc) {
printf("ERROR; return code from pthread_join() is %d\n", rc);
}
// rc = pthread_join(t_SampleThread, &status);
// if (rc) {
// printf("ERROR; return code from pthread_join() is %d\n", rc);
// }
// printf("QUIT: completed join with Menu thread having a status of %ld\n",(long)status);
printf("bye bye ...\n");
}
void SDAIir( float * input_buffer, float * output_buffer, float * pState, float * pCoeffs, int NumberOfStages, int SampleLength) {
float FeedbackSumOfProducts, pSrc, pDst;
int i, j;
for (j = 0; j < SampleLength; j++)
{
pSrc = *input_buffer++;
for (i = 0; i < NumberOfStages; i++)
{
FeedbackSumOfProducts = pSrc - (*(pCoeffs+3) * *pState) -
(*(pCoeffs+4) * *(pState+1)); /* Feedback */
pDst = (*pCoeffs * FeedbackSumOfProducts) +
(*(pCoeffs + 1) * *pState) +
(*(pCoeffs + 2) * *(pState+1)); /* Feedforward */
*(pState+1) = *pState; /* Move delayed samples */
*pState = FeedbackSumOfProducts;
pSrc = pDst; /* Set input for next stage */
pState += SIGLIB_IIR_DELAY_SIZE; /* Increment array pointers */
pCoeffs += SIGLIB_IIR_COEFFS_PER_BIQUAD;
}
pState -= (SIGLIB_IIR_DELAY_SIZE * NumberOfStages); /* Reset array pointers */
pCoeffs -= (SIGLIB_IIR_COEFFS_PER_BIQUAD * NumberOfStages);
*output_buffer++ = pDst;
}
}
void SIFIir(float * pState, const int NumberOfStages) {
int i;
// Initialise the filter state array to 0
for (i = 0; i < (NumberOfStages * SIGLIB_IIR_DELAY_SIZE); i++) {
*pState++ = 0;
}
}
//
// ******************************************************************
// Fast ARM CMSIS DSP library function for cos and sin
// ******************************************************************
//
#define TABLE_SIZE 256
static const float sinTable[259] = {
-0.024541229009628296f, 0.000000000000000000f, 0.024541229009628296f,
0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
0.122410677373409270f, 0.146730467677116390f,
0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
0.313681751489639280f, 0.336889863014221190f,
0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
0.492898195981979370f, 0.514102756977081300f,
0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
0.653172850608825680f, 0.671558976173400880f,
0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
0.788346409797668460f, 0.803207516670227050f,
0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
0.893224298954010010f, 0.903989315032958980f,
0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
0.963776051998138430f, 0.970031261444091800f,
0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
0.997290432453155520f, 0.998795449733734130f,
0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
0.992479562759399410f, 0.989176511764526370f,
0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
0.949528157711029050f, 0.941544055938720700f,
0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
0.870086967945098880f, 0.857728600502014160f,
0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
0.757208824157714840f, 0.740951120853424070f,
0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
0.615231573581695560f, 0.595699310302734380f,
0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
0.449611335992813110f, 0.427555084228515630f,
0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
0.266712754964828490f, 0.242980182170867920f,
0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
0.073564566671848297f, 0.049067676067352295f,
0.024541229009628296f, 0.000000000000000122f, -0.024541229009628296f,
-0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
-0.122410677373409270f, -0.146730467677116390f,
-0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
-0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
-0.313681751489639280f, -0.336889863014221190f,
-0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
-0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
-0.492898195981979370f, -0.514102756977081300f,
-0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
-0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
-0.653172850608825680f, -0.671558976173400880f,
-0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
-0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
-0.788346409797668460f, -0.803207516670227050f,
-0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
-0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
-0.893224298954010010f, -0.903989315032958980f,
-0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
-0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
-0.963776051998138430f, -0.970031261444091800f,
-0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
-0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
-0.997290432453155520f, -0.998795449733734130f,
-0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
-0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
-0.992479562759399410f, -0.989176511764526370f,
-0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
-0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
-0.949528157711029050f, -0.941544055938720700f,
-0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
-0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
-0.870086967945098880f, -0.857728600502014160f,
-0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
-0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
-0.757208824157714840f, -0.740951120853424070f,
-0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
-0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
-0.615231573581695560f, -0.595699310302734380f,
-0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
-0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
-0.449611335992813110f, -0.427555084228515630f,
-0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
-0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
-0.266712754964828490f, -0.242980182170867920f,
-0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
-0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
-0.073564566671848297f, -0.049067676067352295f,
-0.024541229009628296f, -0.000000000000000245f, 0.024541229009628296f
};
static const float cosTable[260] = {
0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
0.998795449733734130f, 0.997290432453155520f, 0.995184719562530520f,
0.992479562759399410f, 0.989176511764526370f,
0.985277652740478520f, 0.980785250663757320f, 0.975702106952667240f,
0.970031261444091800f, 0.963776051998138430f, 0.956940352916717530f,
0.949528157711029050f, 0.941544055938720700f,
0.932992815971374510f, 0.923879504203796390f, 0.914209783077239990f,
0.903989315032958980f, 0.893224298954010010f, 0.881921291351318360f,
0.870086967945098880f, 0.857728600502014160f,
0.844853579998016360f, 0.831469595432281490f, 0.817584812641143800f,
0.803207516670227050f, 0.788346409797668460f, 0.773010432720184330f,
0.757208824157714840f, 0.740951120853424070f,
0.724247097969055180f, 0.707106769084930420f, 0.689540565013885500f,
0.671558976173400880f, 0.653172850608825680f, 0.634393274784088130f,
0.615231573581695560f, 0.595699310302734380f,
0.575808167457580570f, 0.555570244789123540f, 0.534997642040252690f,
0.514102756977081300f, 0.492898195981979370f, 0.471396744251251220f,
0.449611335992813110f, 0.427555084228515630f,
0.405241310596466060f, 0.382683426141738890f, 0.359895050525665280f,
0.336889863014221190f, 0.313681751489639280f, 0.290284663438797000f,
0.266712754964828490f, 0.242980182170867920f,
0.219101235270500180f, 0.195090323686599730f, 0.170961886644363400f,
0.146730467677116390f, 0.122410677373409270f, 0.098017141222953796f,
0.073564566671848297f, 0.049067676067352295f,
0.024541229009628296f, 0.000000000000000061f, -0.024541229009628296f,
-0.049067676067352295f, -0.073564566671848297f, -0.098017141222953796f,
-0.122410677373409270f, -0.146730467677116390f,
-0.170961886644363400f, -0.195090323686599730f, -0.219101235270500180f,
-0.242980182170867920f, -0.266712754964828490f, -0.290284663438797000f,
-0.313681751489639280f, -0.336889863014221190f,
-0.359895050525665280f, -0.382683426141738890f, -0.405241310596466060f,
-0.427555084228515630f, -0.449611335992813110f, -0.471396744251251220f,
-0.492898195981979370f, -0.514102756977081300f,
-0.534997642040252690f, -0.555570244789123540f, -0.575808167457580570f,
-0.595699310302734380f, -0.615231573581695560f, -0.634393274784088130f,
-0.653172850608825680f, -0.671558976173400880f,
-0.689540565013885500f, -0.707106769084930420f, -0.724247097969055180f,
-0.740951120853424070f, -0.757208824157714840f, -0.773010432720184330f,
-0.788346409797668460f, -0.803207516670227050f,
-0.817584812641143800f, -0.831469595432281490f, -0.844853579998016360f,
-0.857728600502014160f, -0.870086967945098880f, -0.881921291351318360f,
-0.893224298954010010f, -0.903989315032958980f,
-0.914209783077239990f, -0.923879504203796390f, -0.932992815971374510f,
-0.941544055938720700f, -0.949528157711029050f, -0.956940352916717530f,
-0.963776051998138430f, -0.970031261444091800f,
-0.975702106952667240f, -0.980785250663757320f, -0.985277652740478520f,
-0.989176511764526370f, -0.992479562759399410f, -0.995184719562530520f,
-0.997290432453155520f, -0.998795449733734130f,
-0.999698817729949950f, -1.000000000000000000f, -0.999698817729949950f,
-0.998795449733734130f, -0.997290432453155520f, -0.995184719562530520f,
-0.992479562759399410f, -0.989176511764526370f,
-0.985277652740478520f, -0.980785250663757320f, -0.975702106952667240f,
-0.970031261444091800f, -0.963776051998138430f, -0.956940352916717530f,
-0.949528157711029050f, -0.941544055938720700f,
-0.932992815971374510f, -0.923879504203796390f, -0.914209783077239990f,
-0.903989315032958980f, -0.893224298954010010f, -0.881921291351318360f,
-0.870086967945098880f, -0.857728600502014160f,
-0.844853579998016360f, -0.831469595432281490f, -0.817584812641143800f,
-0.803207516670227050f, -0.788346409797668460f, -0.773010432720184330f,
-0.757208824157714840f, -0.740951120853424070f,
-0.724247097969055180f, -0.707106769084930420f, -0.689540565013885500f,
-0.671558976173400880f, -0.653172850608825680f, -0.634393274784088130f,
-0.615231573581695560f, -0.595699310302734380f,
-0.575808167457580570f, -0.555570244789123540f, -0.534997642040252690f,
-0.514102756977081300f, -0.492898195981979370f, -0.471396744251251220f,
-0.449611335992813110f, -0.427555084228515630f,
-0.405241310596466060f, -0.382683426141738890f, -0.359895050525665280f,
-0.336889863014221190f, -0.313681751489639280f, -0.290284663438797000f,
-0.266712754964828490f, -0.242980182170867920f,
-0.219101235270500180f, -0.195090323686599730f, -0.170961886644363400f,
-0.146730467677116390f, -0.122410677373409270f, -0.098017141222953796f,
-0.073564566671848297f, -0.049067676067352295f,
-0.024541229009628296f, -0.000000000000000184f, 0.024541229009628296f,
0.049067676067352295f, 0.073564566671848297f, 0.098017141222953796f,
0.122410677373409270f, 0.146730467677116390f,
0.170961886644363400f, 0.195090323686599730f, 0.219101235270500180f,
0.242980182170867920f, 0.266712754964828490f, 0.290284663438797000f,
0.313681751489639280f, 0.336889863014221190f,
0.359895050525665280f, 0.382683426141738890f, 0.405241310596466060f,
0.427555084228515630f, 0.449611335992813110f, 0.471396744251251220f,
0.492898195981979370f, 0.514102756977081300f,
0.534997642040252690f, 0.555570244789123540f, 0.575808167457580570f,
0.595699310302734380f, 0.615231573581695560f, 0.634393274784088130f,
0.653172850608825680f, 0.671558976173400880f,
0.689540565013885500f, 0.707106769084930420f, 0.724247097969055180f,
0.740951120853424070f, 0.757208824157714840f, 0.773010432720184330f,
0.788346409797668460f, 0.803207516670227050f,
0.817584812641143800f, 0.831469595432281490f, 0.844853579998016360f,
0.857728600502014160f, 0.870086967945098880f, 0.881921291351318360f,
0.893224298954010010f, 0.903989315032958980f,
0.914209783077239990f, 0.923879504203796390f, 0.932992815971374510f,
0.941544055938720700f, 0.949528157711029050f, 0.956940352916717530f,
0.963776051998138430f, 0.970031261444091800f,
0.975702106952667240f, 0.980785250663757320f, 0.985277652740478520f,
0.989176511764526370f, 0.992479562759399410f, 0.995184719562530520f,
0.997290432453155520f, 0.998795449733734130f,
0.999698817729949950f, 1.000000000000000000f, 0.999698817729949950f,
0.998795449733734130f
};
//
// FAST aaproximation routine for sine using lookup table
//
float arm_sin_f32( float x) {
float sinVal, fract, in; /* Temporary variables for input, output */
long index; /* Index variable */
unsigned long tableSize = (unsigned long) TABLE_SIZE; /* Initialise tablesize */
float wa, wb, wc, wd; /* Cubic interpolation coefficients */
float a, b, c, d; /* Four nearest output values */
float *tablePtr; /* Pointer to table */
long n;
float fractsq, fractby2, fractby6, fractby3, fractsqby2;
float oneminusfractby2;
float frby2xfrsq, frby6xfrsq;
/* input x is in radians */
/* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
in = x * 0.159154943092f;
/* Calculation of floor value of input */
n = (long) in;
/* Make negative values towards -infinity */
if(x < 0.0f)
{
n = n - 1;
}
/* Map input value to [0 1] */
in = in - (float) n;
/* Calculation of index of the table */
index = (unsigned long) (tableSize * in);
/* fractional value calculation */
fract = ((float) tableSize * in) - (float) index;
/* Checking min and max index of table */
if(index < 0)
{
index = 0;
}
else if(index > 256)
{
index = 256;
}
/* Initialise table pointer */
tablePtr = (float *) & sinTable[index];
/* Read four nearest values of input value from the sin table */
a = tablePtr[0];
b = tablePtr[1];
c = tablePtr[2];
d = tablePtr[3];
/* Cubic interpolation process */
fractsq = fract * fract;
fractby2 = fract * 0.5f;
fractby6 = fract * 0.166666667f;
fractby3 = fract * 0.3333333333333f;
fractsqby2 = fractsq * 0.5f;
frby2xfrsq = (fractby2) * fractsq;
frby6xfrsq = (fractby6) * fractsq;
oneminusfractby2 = 1.0f - fractby2;
wb = fractsqby2 - fractby3;
wc = (fractsqby2 + fract);
wa = wb - frby6xfrsq;
wb = frby2xfrsq - fractsq;
sinVal = wa * a;
wc = wc - frby2xfrsq;
wd = (frby6xfrsq) - fractby6;
wb = wb + oneminusfractby2;
/* Calculate sin value */
sinVal = (sinVal + (b * wb)) + ((c * wc) + (d * wd));
/* Return the output value */
return (sinVal);
}
//
// FAST aaproximation routine for cosine using lookup table
//
float arm_cos_f32(float x) {
float cosVal, fract, in;
long index;
unsigned long tableSize = (unsigned long) TABLE_SIZE;
float wa, wb, wc, wd;
float a, b, c, d;
float *tablePtr;
long n;
float fractsq, fractby2, fractby6, fractby3, fractsqby2;
float oneminusfractby2;
float frby2xfrsq, frby6xfrsq;
/* input x is in radians */
/* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
in = x * 0.159154943092f;
/* Calculation of floor value of input */
n = (long) in;
/* Make negative values towards -infinity */
if(x < 0.0f)
{
n = n - 1;
}
/* Map input value to [0 1] */
in = in - (float) n;
/* Calculation of index of the table */
index = (unsigned long) (tableSize * in);
/* fractional value calculation */
fract = ((float) tableSize * in) - (float) index;
/* Checking min and max index of table */
if(index < 0)
{
index = 0;
}
else if(index > 256)
{
index = 256;
}
/* Initialise table pointer */
tablePtr = (float *) & cosTable[index];
/* Read four nearest values of input value from the cos table */
a = tablePtr[0];
b = tablePtr[1];
c = tablePtr[2];
d = tablePtr[3];
/* Cubic interpolation process */
fractsq = fract * fract;
fractby2 = fract * 0.5f;
fractby6 = fract * 0.166666667f;
fractby3 = fract * 0.3333333333333f;
fractsqby2 = fractsq * 0.5f;
frby2xfrsq = (fractby2) * fractsq;
frby6xfrsq = (fractby6) * fractsq;
oneminusfractby2 = 1.0f - fractby2;
wb = fractsqby2 - fractby3;
wc = (fractsqby2 + fract);
wa = wb - frby6xfrsq;
wb = frby2xfrsq - fractsq;
cosVal = wa * a;
wc = wc - frby2xfrsq;
wd = (frby6xfrsq) - fractby6;
wb = wb + oneminusfractby2;
/* Calculate cos value */
cosVal = (cosVal + (b * wb)) + ((c * wc) + (d * wd));
/* Return the output value */
return (cosVal);
}
//
// ******************************************************************
// 128x64 Pixel LCD Routines START
// ******************************************************************
//
void lcd_init(void) {
/* initialize the port control lines to the LCD module */
LCD_CS_HI; /* set chip select high output */
LCD_RS_HI; /* set the RS line low as output */
LCD_RS_LO; /* select register for control port */
LCD_CS_LO; /* enable interface via chip select */
/* program the controller operational state */
put_serial_to_lcd( LCD_SET_ADC_NOR ); /* set ADC REVERSE */
// NEW FOCUS DISPLAY G126GLGFYSY6WT
put_serial_to_lcd( LCD_SET_SHL_REV ); /* set SHL REVERSE */
put_serial_to_lcd( LCD_SET_BIAS_0 ); /* set for the low bias mode */
put_serial_to_lcd( LCD_PWR_CTL+4 ); /* turn on the VC bit */
delay(5); /* delay for the converter on */
put_serial_to_lcd( LCD_PWR_CTL+6 ); /* now turn on VC+VR bits */
delay(5); /* delay for the regulator on */
put_serial_to_lcd( LCD_PWR_CTL+7 ); /* now turn on the VC+VR+VF */
// NEW FOCUS DISPLAY G126GLGFYSY6WT
put_serial_to_lcd( LCD_REG_RESISTOR+7 ); /* set default resistor ratio */
put_serial_to_lcd( LCD_REF_VOLT_MODE ); /* prime for the reference voltage */
// Given percentage CONTRAST calculate register value i
put_serial_to_lcd( LCD_REF_VOLT_REG + 27 ); /* set default reference voltage select */
delay(5); ; /* delay for power stabilize */
put_serial_to_lcd( LCD_DISP_ON ); /* put the display on */
put_serial_to_lcd( LCD_SET_LINE+0 ); /* set line for row 0 of display */
put_serial_to_lcd( LCD_SET_PAGE+0 ); /* set page 0 */
put_serial_to_lcd( LCD_SET_COL_HI+0 ); /* set column 0 */
put_serial_to_lcd( LCD_SET_COL_LO+0 );
LCD_CS_HI; /* force chip select back off */
}
void put_serial_to_lcd( unsigned char data ) {
wiringPiSPIDataRW(1, (unsigned char *)&data, 1);
}
void lcd_erase(void) {
unsigned char p;
unsigned char i;
LCD_CS_LO;
for(p=0; p<9; p++) {
LCD_RS_LO;
put_serial_to_lcd( LCD_SET_PAGE+p ); /* set page */
put_serial_to_lcd( LCD_SET_COL_HI+0 ); /* set column 0 */
put_serial_to_lcd( LCD_SET_COL_LO+0 );
LCD_RS_HI;
for(i=0; i<132; i++)
{
put_serial_to_lcd( 0 ); /* clear the data */
}
}
LCD_CS_HI;
}
void lcd_clear_area(unsigned char left, unsigned char top,unsigned char right, unsigned char bottom) {
unsigned char bit_pos;
unsigned char x;
unsigned char byte_offset;
unsigned char y_bits;
unsigned char remaining_bits;
unsigned char mask;
bit_pos = top & 0x07; /* get starting bit offset into byte */
for(x = left; x <= right; x++)
{
byte_offset = top >> 3; /* get byte offset into y direction */
y_bits = (bottom - top) + 1; /* get length in the y direction to write */
remaining_bits = 8 - bit_pos; /* number of bits left in byte */
mask = l_mask_array[bit_pos]; /* get mask for this bit */
while(y_bits) /* while there are still bits to write */
{
if((remaining_bits == 8) && (y_bits > 7))
{
/* here if we are byte aligned and have at least 1 byte to write */
/* do the entire byte at once instead of bit by bit */
while(y_bits > 7) /* while there are at least 8 more bits to do */
{
l_display_array[byte_offset][x] = 0x00;
byte_offset++;
y_bits -= 8;
}
}
else
{
/* here if not byte aligned or an entire byte does not need written */
/* thus do bit by bit */
l_display_array[byte_offset][x] &= ~mask;
if(l_mask_array[0] & 0x80)
{
mask >>= 1;
}
else
{
mask <<= 1;
}
y_bits--;
remaining_bits--;
if(remaining_bits == 0)
{
/* might have bust gotton byte aligned */
/* so reset for beginning of a byte */
remaining_bits = 8;
byte_offset++;
mask = l_mask_array[0];
}
}
}
}
}
void lcd_invert_area(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom) {
unsigned char bit_pos;
unsigned char x;
unsigned char byte_offset;
unsigned char y_bits;
unsigned char remaining_bits;
unsigned char mask;
bit_pos = top & 0x07; /* get starting bit offset into byte */
for(x = left; x <= right; x++)
{
byte_offset = top >> 3; /* get byte offset into y direction */
y_bits = (bottom - top) + 1; /* get length in the x direction to write */
remaining_bits = 8 - bit_pos; /* number of bits left in byte */
mask = l_mask_array[bit_pos]; /* get mask for this bit */
while(y_bits) /* while there are still bits to write */
{
if((remaining_bits == 8) && (y_bits > 7))
{
/* here if we are byte aligned and have at least 1 byte to write */
/* do the entire byte at once instead of bit by bit */
while(y_bits > 7) /* while there are at least 8 more bits to do */
{
l_display_array[byte_offset][x] ^= 0xFF;
byte_offset++;
y_bits -= 8;
}
}
else
{
/* here if not byte aligned or an entire byte does not need written */
/* thus do bit by bit */
l_display_array[byte_offset][x] ^= mask;
if(l_mask_array[0] & 0x80)
{
mask >>= 1;
}
else
{
mask <<= 1;
}
y_bits--;
remaining_bits--;
if(remaining_bits == 0)
{
/* might have bust gotton byte aligned */
/* so reset for beginning of a byte */
remaining_bits = 8;
byte_offset++;
mask = l_mask_array[0];
}
}
}
}
}
void lcd_horz_line(unsigned char left, unsigned char right, unsigned char row)
{
unsigned char bit_pos;
unsigned char byte_offset;
unsigned char mask;
unsigned char col;
bit_pos = row & 0x07; /* get the bit offset into a byte */
byte_offset = row >> 3; /* get the byte offset into x array */
mask = l_mask_array[bit_pos]; /* get the mask for this bit */
for(col = left; col <= right; col++)
{
l_display_array[byte_offset][col] |= mask;
}
}
void lcd_vert_line(unsigned char top, unsigned char bottom, unsigned char column)
{
unsigned char bit_pos;
unsigned char byte_offset;
unsigned char y_bits;
unsigned char remaining_bits;
unsigned char mask;
bit_pos = top & 0x07; /* get starting bit offset into byte */
byte_offset = top >> 3; /* get byte offset into y direction */
y_bits = (bottom - top) + 1; /* get length in the x direction to write */
remaining_bits = 8 - bit_pos; /* number of bits left in byte */
mask = l_mask_array[bit_pos]; /* get mask for this bit */
while(y_bits) /* while there are still bits to write */
{
if((remaining_bits == 8) && (y_bits > 7))
{
/* here if we are byte aligned and have at least 1 byte to write */
/* do the entire byte at once instead of bit by bit */
while(y_bits > 7) /* while there are at least 8 more bits to do */
{
l_display_array[byte_offset][column] = 0xFF;
byte_offset++;
y_bits -= 8;
}
}
else
{
/* we are not byte aligned or an entire byte does not need written */
/* do each individual bit */
l_display_array[byte_offset][column] |= mask;
if(l_mask_array[0] & 0x80)
{
mask >>= 1;
}
else
{
mask <<= 1;
}
y_bits--;
remaining_bits--;
if(remaining_bits == 0)
{
/* might have bust gotton byte aligned */
/* so reset for beginning of a byte */
remaining_bits = 8;
byte_offset++;
mask = l_mask_array[0];
}
}
}
}
void lcd_clr_horz_line(unsigned char left, unsigned char right, unsigned char row)
{
unsigned char bit_pos;
unsigned char byte_offset;
unsigned char mask;
unsigned char col;
bit_pos = row & 0x07; /* get the bit offset into a byte */
byte_offset = row >> 3; /* get the byte offset into x array */
mask = l_mask_array[bit_pos]; /* get the mask for this bit */
for(col = left; col <= right; col++)
{
l_display_array[byte_offset][col] &= ~mask;
}
}
void lcd_clr_vert_line(unsigned char top, unsigned char bottom, unsigned char column)
{
unsigned char bit_pos;
unsigned char byte_offset;
unsigned char y_bits;
unsigned char remaining_bits;
unsigned char mask;
bit_pos = top & 0x07; /* get starting bit offset into byte */
byte_offset = top >> 3; /* get byte offset into y direction */
y_bits = (bottom - top) + 1; /* get length in the x direction to write */
remaining_bits = 8 - bit_pos; /* number of bits left in byte */
mask = l_mask_array[bit_pos]; /* get mask for this bit */
while(y_bits) /* while there are still bits to write */
{
if((remaining_bits == 8) && (y_bits > 7))
{
/* here if we are byte aligned and have at least 1 byte to write */
/* do the entire byte at once instead of bit by bit */
while(y_bits > 7) /* while there are at least 8 more bits to do */
{
l_display_array[byte_offset][column] = 0x00;
byte_offset++;
y_bits -= 8;
}
}
else
{
/* we are not byte aligned or an entire byte does not need written */
/* do each individual bit */
l_display_array[byte_offset][column] &= ~mask;
if(l_mask_array[0] & 0x80)
{
mask >>= 1;
}
else
{
mask <<= 1;
}
y_bits--;
remaining_bits--;
if(remaining_bits == 0)
{
/* might have bust gotton byte aligned */
/* so reset for beginning of a byte */
remaining_bits = 8;
byte_offset++;
mask = l_mask_array[0];
}
}
}
}
void lcd_box(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom)
{
/* to draw a box requires two vertical lines */
lcd_vert_line(top,bottom,left);
lcd_vert_line(top,bottom,right);
/* and two horizonal lines */
lcd_horz_line(left,right,top);
lcd_horz_line(left,right,bottom);
}
void lcd_clr_box(unsigned char left, unsigned char top, unsigned char right, unsigned char bottom)
{
/* to undraw the box undraw the two vertical lines */
lcd_clr_vert_line(top,bottom,left);
lcd_clr_vert_line(top,bottom,right);
/* and the two horizonal lines that comprise it */
lcd_clr_horz_line(left,right,top);
lcd_clr_horz_line(left,right,bottom);
}
void lcd_glyph(unsigned char left, unsigned char top, unsigned char width, unsigned char height, unsigned char const *glyph, unsigned char store_width)
{
unsigned char bit_pos;
unsigned char byte_offset;
unsigned char y_bits;
unsigned char remaining_bits;
unsigned char mask;
unsigned char char_mask;
unsigned char x;
unsigned char const *glyph_scan;
unsigned char glyph_offset;
bit_pos = top & 0x07; /* get the bit offset into a byte */
glyph_offset = 0; /* start at left side of the glyph rasters */
char_mask = 0x80; /* initial character glyph mask */
for (x = left; x < (left + width); x++)
{
byte_offset = top >> 3; /* get the byte offset into y direction */
y_bits = height; /* get length in y direction to write */
remaining_bits = 8 - bit_pos; /* number of bits left in byte */
mask = l_mask_array[bit_pos]; /* get mask for this bit */
glyph_scan = glyph + glyph_offset; /* point to base of the glyph */
/* boundary checking here to account for the possibility of */
/* write past the bottom of the screen. */
while((y_bits) && (byte_offset < Y_BYTES)) /* while there are bits still to write */
{
/* check if the character pixel is set or not */
if(*glyph_scan & char_mask)
{
l_display_array[byte_offset][x] |= mask; /* set image pixel */
}
else
{
l_display_array[byte_offset][x] &= ~mask; /* clear the image pixel */
}
if(l_mask_array[0] & 0x80)
{
mask >>= 1;
}
else
{
mask <<= 1;
}
y_bits--;
remaining_bits--;
if(remaining_bits == 0)
{
/* just crossed over a byte boundry, reset byte counts */
remaining_bits = 8;
byte_offset++;
mask = l_mask_array[0];
}
/* bump the glyph scan to next raster */
glyph_scan += store_width;
}
/* shift over to next glyph bit */
char_mask >>= 1;
if(char_mask == 0) /* reset for next byte in raster */
{
char_mask = 0x80;
glyph_offset++;
}
}
}
void lcd_text(unsigned char left, unsigned char top, unsigned char font, char *str)
{
unsigned char x = left;
unsigned char glyph;
unsigned char width;
unsigned char height;
unsigned char store_width;
unsigned char const *glyph_ptr;
while(*str != 0x00)
{
glyph = (unsigned char)*str;
/* check to make sure the symbol is a legal one */
/* if not then just replace it with the default character */
if((glyph < fonts[font].glyph_beg) || (glyph > fonts[font].glyph_end))
{
glyph = fonts[font].glyph_def;
}
/* make zero based index into the font data arrays */
glyph -= fonts[font].glyph_beg;
width = fonts[font].fixed_width; /* check if it is a fixed width */
if(width == 0)
{
width=fonts[font].width_table[glyph]; /* get the variable width instead */
}
height = fonts[font].glyph_height;
store_width = fonts[font].store_width;
glyph_ptr = fonts[font].glyph_table + ((unsigned int)glyph * (unsigned int)store_width * (unsigned int)height);
/* range check / limit things here */
if(x > SCRN_RIGHT)
{
x = SCRN_RIGHT;
}
if((x + width) > SCRN_RIGHT+1)
{
width = SCRN_RIGHT - x + 1;
}
if(top > SCRN_BOTTOM)
{
top = SCRN_BOTTOM;
}
if((top + height) > SCRN_BOTTOM+1)
{
height = SCRN_BOTTOM - top + 1;
}
lcd_glyph(x,top,width,height,glyph_ptr,store_width); /* plug symbol into audio_samples */
x += width; /* move right for next character */
str++; /* point to next character in string */
}
}
void lcd_update(unsigned char top, unsigned char bottom)
{
unsigned char x, err, e;
unsigned char y;
unsigned char yt;
unsigned char yb;
unsigned char *colptr;
unsigned int j;
yt = top >> 3; /* setup bytes of range */
yb = bottom >> 3;
LCD_CS_LO;
for(y = yt; y <= yb; y++)
{
LCD_RS_LO;
put_serial_to_lcd( LCD_SET_PAGE+y ); /* set page */
put_serial_to_lcd( LCD_SET_COL_HI+0 ); /* set column 0 */
put_serial_to_lcd( LCD_SET_COL_LO+0 );
colptr = &l_display_array[y][0];
LCD_RS_HI;
for (x=0; x < X_BYTES; x++)
{
put_serial_to_lcd( *colptr++ );
}
}
LCD_CS_HI;
}
//
// ******************************************************************
// 128x64 Pixel LCD Routines END
// ******************************************************************
//
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment