mirror of
https://github.com/cnlohr/lolra.git
synced 2026-06-17 00:09:31 +00:00
Why no math good
This commit is contained in:
cnlohr
@@ -0,0 +1,15 @@
|
|||||||
|
all : flash
|
||||||
|
|
||||||
|
TARGET:=adcgoertzel
|
||||||
|
TARGET_MCU:=CH32V203G6U6
|
||||||
|
TARGET_MCU_PACKAGE:=CH32V203G6U6
|
||||||
|
CH32V003FUN:=../ch32v003fun/ch32v003fun
|
||||||
|
|
||||||
|
EXTRA_CFLAGS:=-Wno-unused-function -I../../lib -I../ch32v203-fft/lib
|
||||||
|
|
||||||
|
include ../ch32v003fun/ch32v003fun/ch32v003fun.mk
|
||||||
|
|
||||||
|
flash : cv_flash
|
||||||
|
clean : cv_clean
|
||||||
|
rm -rf rf_data_gen chirpbuff.dat chirpbuff.h chirpbuffinfo.h
|
||||||
|
|
||||||
@@ -0,0 +1,388 @@
|
|||||||
|
/**
|
||||||
|
|
||||||
|
MIT-like-non-ai-license
|
||||||
|
|
||||||
|
Copyright (c) 2024 Charles Lohr "CNLohr"
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||||
|
copies of the Software, and to permit persons to whom the Software is
|
||||||
|
furnished to do so, subject to the two following conditions:
|
||||||
|
|
||||||
|
The above copyright notice and this permission notice shall be included in all
|
||||||
|
copies or substantial portions of the Software.
|
||||||
|
|
||||||
|
In addition the following restrictions apply:
|
||||||
|
|
||||||
|
1. The Software and any modifications made to it may not be used for the
|
||||||
|
purpose of training or improving machine learning algorithms, including but not
|
||||||
|
limited to artificial intelligence, natural language processing, or data
|
||||||
|
mining. This condition applies to any derivatives, modifications, or updates
|
||||||
|
based on the Software code. Any usage of the Software in an AI-training dataset
|
||||||
|
is considered a breach of this License.
|
||||||
|
|
||||||
|
2. The Software may not be included in any dataset used for training or
|
||||||
|
improving machine learning algorithms, including but not limited to artificial
|
||||||
|
intelligence, natural language processing, or data mining.
|
||||||
|
|
||||||
|
|
||||||
|
3. Any person or organization found to be in violation of these restrictions
|
||||||
|
will be subject to legal action and may be held liable for any damages
|
||||||
|
resulting from such use.
|
||||||
|
|
||||||
|
If any term is unenforcable, other terms remain in-force.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||||
|
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||||
|
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||||
|
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||||
|
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||||
|
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||||
|
SOFTWARE.
|
||||||
|
|
||||||
|
**/
|
||||||
|
|
||||||
|
// NOT LORA!!! -- but experimenting with the possibility of rx.
|
||||||
|
|
||||||
|
// SETUP INSTRUCTIONS:
|
||||||
|
// (1) `make` in the optionbytes folder to configure `RESET` correctly.
|
||||||
|
// (2) Create a tone (if using the funprog, ../ch32v003fun/minichlink/minichlink -X ECLK 1:235:189:9:3 for 27.48387097MHz
|
||||||
|
// (2) or, for 24.387096762MHz - ../ch32v003fun/minichlink/minichlink -X ECLK 1:150:49:8:3
|
||||||
|
|
||||||
|
/* More notes
|
||||||
|
|
||||||
|
* Minimum sample time with DMA = fCPU / 28 (5.14MHz)
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
#include "ch32v003fun.h"
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
#define SH1107_128x128
|
||||||
|
|
||||||
|
#include "ssd1306_i2c.h"
|
||||||
|
#include "ssd1306.h"
|
||||||
|
|
||||||
|
|
||||||
|
#define PWM_PERIOD (31-1) //For 27.0MHz, use 36MHz if quadrature -- It appears to be good for *244 in the table? WHY 26MHz???!?!!?
|
||||||
|
|
||||||
|
#define ADC_BUFFSIZE 512
|
||||||
|
volatile uint16_t adc_buffer[ADC_BUFFSIZE];
|
||||||
|
|
||||||
|
void SetupADC()
|
||||||
|
{
|
||||||
|
// XXX TODO -look into PGA
|
||||||
|
// XXX TODO - Look into tag-teaming the ADCs
|
||||||
|
|
||||||
|
// PDA is analog input chl 7
|
||||||
|
GPIOA->CFGLR &= ~(0xf<<(4*7)); // CNF = 00: Analog, MODE = 00: Input
|
||||||
|
|
||||||
|
// ADC CLK is chained off of APB2.
|
||||||
|
|
||||||
|
// Reset the ADC to init all regs
|
||||||
|
RCC->APB2PRSTR |= RCC_APB2Periph_ADC1;
|
||||||
|
RCC->APB2PRSTR &= ~RCC_APB2Periph_ADC1;
|
||||||
|
|
||||||
|
// ADCCLK = 12 MHz => RCC_ADCPRE divide by 4
|
||||||
|
RCC->CFGR0 &= ~RCC_ADCPRE; // Clear out the bis in case they were set
|
||||||
|
RCC->CFGR0 |= RCC_ADCPRE_DIV2; // Fastest possible (divide-by-2) NOTE: This is OUTSIDE the specified value in the datasheet.
|
||||||
|
|
||||||
|
// Set up single conversion on chl 7
|
||||||
|
ADC1->RSQR1 = 0;
|
||||||
|
ADC1->RSQR2 = 0;
|
||||||
|
ADC1->RSQR3 = 7; // 0-9 for 8 ext inputs and two internals
|
||||||
|
|
||||||
|
// Not using injection group.
|
||||||
|
|
||||||
|
// Sampling time for channels. Careful: This has PID tuning implications.
|
||||||
|
// Note that with 3 and 3,the full loop (and injection) runs at 138kHz.
|
||||||
|
ADC1->SAMPTR2 = (0<<(3*7));
|
||||||
|
|
||||||
|
// Turn on ADC and set rule group to sw trig
|
||||||
|
// 0 = Use TRGO event for Timer 1 to fire ADC rule.
|
||||||
|
ADC1->CTLR2 = ADC_ADON | ADC_EXTTRIG | ADC_DMA;
|
||||||
|
|
||||||
|
// Reset calibration
|
||||||
|
ADC1->CTLR2 |= ADC_RSTCAL;
|
||||||
|
while(ADC1->CTLR2 & ADC_RSTCAL);
|
||||||
|
|
||||||
|
// Calibrate ADC
|
||||||
|
ADC1->CTLR2 |= ADC_CAL;
|
||||||
|
while(ADC1->CTLR2 & ADC_CAL);
|
||||||
|
|
||||||
|
// ADC_SCAN: Allow scanning.
|
||||||
|
ADC1->CTLR1 = /*ADC_Pga_64 | */ADC_SCAN;
|
||||||
|
|
||||||
|
|
||||||
|
// Turn on DMA
|
||||||
|
RCC->AHBPCENR |= RCC_AHBPeriph_DMA1;
|
||||||
|
|
||||||
|
//DMA1_Channel1 is for ADC
|
||||||
|
DMA1_Channel1->PADDR = (uint32_t)&ADC1->RDATAR;
|
||||||
|
DMA1_Channel1->MADDR = (uint32_t)adc_buffer;
|
||||||
|
DMA1_Channel1->CNTR = ADC_BUFFSIZE;
|
||||||
|
DMA1_Channel1->CFGR =
|
||||||
|
DMA_M2M_Disable |
|
||||||
|
DMA_Priority_VeryHigh |
|
||||||
|
DMA_MemoryDataSize_HalfWord |
|
||||||
|
DMA_PeripheralDataSize_HalfWord |
|
||||||
|
DMA_MemoryInc_Enable |
|
||||||
|
DMA_Mode_Circular |
|
||||||
|
DMA_DIR_PeripheralSRC;
|
||||||
|
|
||||||
|
// NVIC_SetPriority( DMA1_Channel1_IRQn, 0<<4 ); //We don't need to tweak priority.
|
||||||
|
NVIC_EnableIRQ( DMA1_Channel1_IRQn );
|
||||||
|
DMA1_Channel1->CFGR |= DMA_CFGR1_EN | DMA_IT_TC | DMA_IT_HT; // Transmission Complete + Half Empty Interrupts.
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Turn on DMA channel 1
|
||||||
|
DMA1_Channel1->CFGR |= DMA_CFGR1_EN;
|
||||||
|
|
||||||
|
// Enable continuous conversion and DMA
|
||||||
|
ADC1->CTLR2 |= ADC_DMA; // | ADC_CONT;
|
||||||
|
|
||||||
|
// start conversion
|
||||||
|
ADC1->CTLR2 |= ADC_SWSTART;// | ADC_CONT;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
static void SetupTimer1()
|
||||||
|
{
|
||||||
|
// Enable Timer 1
|
||||||
|
RCC->APB2PRSTR |= RCC_APB2Periph_TIM1;
|
||||||
|
RCC->APB2PRSTR &= ~RCC_APB2Periph_TIM1;
|
||||||
|
|
||||||
|
TIM1->PSC = 0; // Prescalar to 0x0000 (so, 48MHz base clock)
|
||||||
|
TIM1->ATRLR = PWM_PERIOD;
|
||||||
|
|
||||||
|
#ifdef PWM_OUTPUT
|
||||||
|
// PA10 = T1CH3.
|
||||||
|
GPIOA->CFGHR &= ~(0xf<<(4*2));
|
||||||
|
GPIOA->CFGHR |= (GPIO_Speed_10MHz | GPIO_CNF_OUT_PP_AF)<<(4*2);
|
||||||
|
|
||||||
|
TIM1->CCER = TIM_CC3E | TIM_CC3P;
|
||||||
|
TIM1->CHCTLR2 = TIM_OC3M_2 | TIM_OC3M_1;
|
||||||
|
TIM1->CH3CVR = 5; // Actual duty cycle (Off to begin with)
|
||||||
|
#endif
|
||||||
|
|
||||||
|
TIM1->CCER = TIM_CC1E;
|
||||||
|
TIM1->CHCTLR1 = TIM_OC1M_2 | TIM_OC1M_1;
|
||||||
|
TIM1->CH1CVR = 1;
|
||||||
|
|
||||||
|
// Setup TRGO to trigger for ADC (NOTE: Not on the 203! TIM1_TRGO is only connected to injection)
|
||||||
|
//TIM1->CTLR2 = TIM_MMS_1;
|
||||||
|
|
||||||
|
// Enable TIM1 outputs
|
||||||
|
TIM1->BDTR = TIM_MOE;
|
||||||
|
TIM1->CTLR1 = TIM_CEN;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void InnerLoop() __attribute__((noreturn));
|
||||||
|
|
||||||
|
|
||||||
|
uint32_t tc;
|
||||||
|
|
||||||
|
|
||||||
|
volatile uint16_t * adc_tail = adc_buffer;
|
||||||
|
|
||||||
|
|
||||||
|
const uint32_t g_goertzel_omega_per_sample = 41016; // 51/128 * 3.14159 * 65536
|
||||||
|
const uint32_t g_goertzel_coefficient = 106228;//2 * cos( g_goertzel_omega_per_sample / 65536 * 180 / 3.141592) * 65536;
|
||||||
|
const uint32_t g_goertzel_coefficient_s = 76781;//2 * sin( g_goertzel_omega_per_sample / 65536 * 180 / 3.141592 ) * 65536;
|
||||||
|
|
||||||
|
uint32_t g_goertzel_samples;
|
||||||
|
uint32_t g_goertzel_outs;
|
||||||
|
int32_t g_goertzelp, g_goertzelp2;
|
||||||
|
int32_t g_goertzelp_store, g_goertzelp2_store;
|
||||||
|
|
||||||
|
void DMA1_Channel1_IRQHandler( void ) __attribute__((interrupt));
|
||||||
|
void DMA1_Channel1_IRQHandler( void )
|
||||||
|
{
|
||||||
|
//GPIOD->BSHR = 1; // Turn on GPIOD0 for profiling
|
||||||
|
|
||||||
|
// Timer goes backwards when we are moving forwards.
|
||||||
|
volatile uint16_t * adc_buffer_end = 0;
|
||||||
|
volatile uint16_t * adc_buffer_top = adc_buffer + ADC_BUFFSIZE;
|
||||||
|
|
||||||
|
int32_t goertzel_coefficient = g_goertzel_coefficient;
|
||||||
|
int32_t goertzelp2 = g_goertzelp2;
|
||||||
|
int32_t goertzelp = g_goertzelp;
|
||||||
|
uint32_t goertzel_samples = g_goertzel_samples;
|
||||||
|
// Backup flags.
|
||||||
|
volatile int intfr = DMA1->INTFR;
|
||||||
|
do
|
||||||
|
{
|
||||||
|
// Clear all possible flags.
|
||||||
|
DMA1->INTFCR = DMA1_IT_GL1;
|
||||||
|
|
||||||
|
int tpl = ADC_BUFFSIZE - DMA1_Channel1->CNTR; // Warning, sometimes this is == to the base, or == 0 (i.e. might be 256, if top is 255)
|
||||||
|
tpl += ADC_BUFFSIZE;
|
||||||
|
tpl = (tpl & (ADC_BUFFSIZE-1));
|
||||||
|
if( tpl == ADC_BUFFSIZE ) tpl = 0;
|
||||||
|
|
||||||
|
adc_buffer_end = adc_buffer + ( ( tpl / 4) * 4 );
|
||||||
|
|
||||||
|
while( adc_tail != adc_buffer_end )
|
||||||
|
{
|
||||||
|
|
||||||
|
int32_t t; // 1/2 of 4096, to try to keep our numbers reasonable.
|
||||||
|
|
||||||
|
// Here is where the magic happens.
|
||||||
|
int32_t goertzel;
|
||||||
|
|
||||||
|
#define ITERATION(x) \
|
||||||
|
t = (adc_tail[x] - 2048)<<8; \
|
||||||
|
goertzel = t + ( ( (int64_t)(goertzel_coefficient) * goertzelp ) >> 32 ) - goertzelp2; \
|
||||||
|
goertzelp2 = goertzelp; \
|
||||||
|
goertzelp = goertzel; \
|
||||||
|
|
||||||
|
ITERATION( 0 );
|
||||||
|
ITERATION( 1 );
|
||||||
|
ITERATION( 2 );
|
||||||
|
ITERATION( 3 );
|
||||||
|
|
||||||
|
adc_tail+=4;
|
||||||
|
goertzel_samples+=4;
|
||||||
|
if( adc_tail == adc_buffer_top ) adc_tail = adc_buffer;
|
||||||
|
if( goertzel_samples == 128 )
|
||||||
|
{
|
||||||
|
g_goertzelp_store = goertzelp;
|
||||||
|
g_goertzelp2_store = goertzelp2;
|
||||||
|
|
||||||
|
goertzelp = 0;
|
||||||
|
goertzelp2 = 0;
|
||||||
|
|
||||||
|
g_goertzel_outs++;
|
||||||
|
goertzel_samples = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
intfr = DMA1->INTFR;
|
||||||
|
} while( intfr & DMA1_IT_GL1 );
|
||||||
|
|
||||||
|
g_goertzelp2 = goertzelp2;
|
||||||
|
g_goertzelp = goertzelp;
|
||||||
|
g_goertzel_samples = goertzel_samples;
|
||||||
|
|
||||||
|
//GPIOD->BSHR = 1<<16; // Turn off GPIOD0 for profiling
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void InnerLoop()
|
||||||
|
{
|
||||||
|
while(1){
|
||||||
|
|
||||||
|
int k;
|
||||||
|
int adcz = adc_buffer[0];
|
||||||
|
for( k = 0; k < 128; k++ )
|
||||||
|
{
|
||||||
|
int y = adc_buffer[k]-adcz + 64;
|
||||||
|
if( y < 0 ) y = 0;
|
||||||
|
if( y > 127 ) y = 127;
|
||||||
|
ssd1306_drawPixel( k, y, 1 );
|
||||||
|
}
|
||||||
|
|
||||||
|
ssd1306_refresh();
|
||||||
|
ssd1306_setbuf(0);
|
||||||
|
|
||||||
|
printf( "%3d %6d %6d\n", g_goertzel_outs,g_goertzelp2_store, g_goertzelp_store );
|
||||||
|
// Delay_Ms(940);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
SystemInit();
|
||||||
|
|
||||||
|
SysTick->CTLR = (1<<2) | 1; // HCLK
|
||||||
|
Delay_Ms(100);
|
||||||
|
|
||||||
|
|
||||||
|
printf( "System On\n" );
|
||||||
|
|
||||||
|
RCC->CTLR |= RCC_HSEON;
|
||||||
|
while( ! ( RCC->CTLR & RCC_HSERDY ) );
|
||||||
|
|
||||||
|
RCC->CFGR0 = (RCC->CFGR0 & ~RCC_SW) | RCC_SW_HSE;
|
||||||
|
|
||||||
|
RCC->CTLR &= ~RCC_PLLON;
|
||||||
|
|
||||||
|
// Switch PLL to HSE.
|
||||||
|
RCC->CFGR0 |= RCC_PLLSRC;
|
||||||
|
|
||||||
|
// x18; 8MHz x 18 = 144 MHz
|
||||||
|
RCC->CFGR0 &= ~RCC_PPRE2; // No divisor on APB1/2
|
||||||
|
RCC->CFGR0 &= ~RCC_PPRE1;
|
||||||
|
RCC->CFGR0 |= RCC_PLLMULL_0 | RCC_PLLMULL_1 | RCC_PLLMULL_2 | RCC_PLLMULL_3;
|
||||||
|
|
||||||
|
RCC->CTLR |= RCC_PLLON;
|
||||||
|
|
||||||
|
// Switch to PLL
|
||||||
|
RCC->CFGR0 = (RCC->CFGR0 & ~RCC_SW) | RCC_SW_PLL;
|
||||||
|
|
||||||
|
// Disable HSI
|
||||||
|
RCC->CTLR &= ~(RCC_HSION);
|
||||||
|
|
||||||
|
Delay_Ms(10);
|
||||||
|
|
||||||
|
|
||||||
|
printf( "CTLR: %08x / CFGR0: %08x\n", (RCC->CTLR), (RCC->CFGR0) );
|
||||||
|
RCC->AHBPCENR |= 3; //DMA2EN | DMA1EN
|
||||||
|
RCC->APB2PCENR |= RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | 0x07; // Enable all GPIO
|
||||||
|
RCC->APB1PCENR |= RCC_APB1Periph_TIM2;
|
||||||
|
|
||||||
|
SetupADC();
|
||||||
|
|
||||||
|
printf( "Setting up OLED.\n" );
|
||||||
|
ssd1306_i2c_setup();
|
||||||
|
uint8_t ret = ssd1306_i2c_init();
|
||||||
|
ssd1306_init();
|
||||||
|
ssd1306_setbuf(0);
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
int i = 0;
|
||||||
|
int k = 0;
|
||||||
|
int frame = 0;
|
||||||
|
while( 1)
|
||||||
|
{
|
||||||
|
// ssd1306_drawLine( (frame)%128, (0)%128, (0)%128, (127-frame)%128, 1 );
|
||||||
|
ssd1306_drawstr( frame%128, frame%128, "hello", 1 );
|
||||||
|
|
||||||
|
ssd1306_refresh();
|
||||||
|
ssd1306_setbuf(0);
|
||||||
|
frame++;
|
||||||
|
}
|
||||||
|
|
||||||
|
while(1);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
// turn on the op-amp
|
||||||
|
EXTEN->EXTEN_CTR |= EXTEN_OPA_EN;
|
||||||
|
|
||||||
|
// select op-amp pos pin: 0 = PA2, 1 = PD7
|
||||||
|
EXTEN->EXTEN_CTR |= EXTEN_OPA_PSEL;
|
||||||
|
|
||||||
|
// select op-amp neg pin: 0 = PA1, 1 = PD0
|
||||||
|
EXTEN->EXTEN_CTR |= EXTEN_OPA_NSEL;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
printf( "ADC Setup\n" );
|
||||||
|
|
||||||
|
SetupTimer1();
|
||||||
|
|
||||||
|
printf( "Timer 1 setup\n" );
|
||||||
|
|
||||||
|
InnerLoop();
|
||||||
|
}
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
#ifndef _FUNCONFIG_H
|
||||||
|
#define _FUNCONFIG_H
|
||||||
|
|
||||||
|
#define FUNCONF_USE_DEBUGPRINTF 1
|
||||||
|
#define FUNCONF_USE_UARTPRINTF 0
|
||||||
|
#define FUNCONF_USE_HSE 1
|
||||||
|
#define FUNCONF_SYSTICK_USE_HCLK 1
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
all : test floattest
|
||||||
|
|
||||||
|
floattest : floattestt
|
||||||
|
./floattestt
|
||||||
|
|
||||||
|
floattestt : floattestt.c
|
||||||
|
gcc -o $@ $^ -lm -g
|
||||||
|
|
||||||
|
|
||||||
|
test : ttest
|
||||||
|
./ttest
|
||||||
|
|
||||||
|
ttest : ttest.c
|
||||||
|
gcc -o $@ $^ -lm -g
|
||||||
|
|
||||||
|
clean :
|
||||||
|
rm -rf *.o ttest *~ floattestt
|
||||||
Binary file not shown.
@@ -0,0 +1,110 @@
|
|||||||
|
#include <stdio.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
uint32_t g_goertzel_omega_per_sample;
|
||||||
|
uint32_t g_goertzel_coefficient;
|
||||||
|
int32_t g_goertzel_coefficient_s;
|
||||||
|
uint32_t g_goertzel_samples;
|
||||||
|
uint32_t g_goertzel_outs;
|
||||||
|
int32_t g_goertzelp, g_goertzelp2;
|
||||||
|
int32_t g_goertzelp_store, g_goertzelp2_store;
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
|
||||||
|
g_goertzel_omega_per_sample = 4.0/128 * 3.1415926535*2.0*65536;
|
||||||
|
g_goertzel_coefficient = 2 * cos( g_goertzel_omega_per_sample / 65536.0 ) * 65536;
|
||||||
|
g_goertzel_coefficient_s = 2 * sin( g_goertzel_omega_per_sample / 65536.0 ) * 65536;
|
||||||
|
|
||||||
|
|
||||||
|
const double AomegaPerSample = g_goertzel_omega_per_sample/65536.0;
|
||||||
|
const int AnumSamples = 256; // enough to go from 0 to 2pi
|
||||||
|
double Acoeff = 2 * cos( AomegaPerSample ) * 65536;
|
||||||
|
double Acoeff_s = 2 * sin( AomegaPerSample ) * 65536;
|
||||||
|
double Asprev = AomegaPerSample * 65536;
|
||||||
|
double Asprev2 = 0;
|
||||||
|
|
||||||
|
printf( "%d / %d / %d %f / %f / %f\n", g_goertzel_omega_per_sample, g_goertzel_coefficient, g_goertzel_coefficient_s, Acoeff, Acoeff_s, AomegaPerSample );
|
||||||
|
|
||||||
|
|
||||||
|
g_goertzelp = g_goertzel_omega_per_sample;
|
||||||
|
|
||||||
|
int32_t goertzel_coefficient = g_goertzel_coefficient;
|
||||||
|
int32_t goertzelp2 = g_goertzelp2;
|
||||||
|
int32_t goertzelp = g_goertzelp;
|
||||||
|
uint32_t goertzel_samples = g_goertzel_samples;
|
||||||
|
|
||||||
|
uint32_t adc_tail;
|
||||||
|
|
||||||
|
double As;
|
||||||
|
|
||||||
|
int js = 0;
|
||||||
|
for( js = 0; js < 260/4; js++ )
|
||||||
|
{
|
||||||
|
int32_t t; // 1/2 of 4096, to try to keep our numbers reasonable.
|
||||||
|
|
||||||
|
// Here is where the magic happens.
|
||||||
|
int32_t goertzel;
|
||||||
|
|
||||||
|
#define ITERATION(x) \
|
||||||
|
t = sin( AomegaPerSample * (x+js*4) ) * 32768; \
|
||||||
|
\
|
||||||
|
/* Fixed */ \
|
||||||
|
printf( ">> %d + %d((%d<<8*%d<<8)>>32) - %d -", t, ( ( ((int64_t)(goertzel_coefficient)<<8) * (int64_t)(goertzelp<<8) ) >> 32 ), goertzel_coefficient, goertzelp, goertzelp2 ); \
|
||||||
|
goertzel = t + ( ( ((int64_t)(goertzel_coefficient)<<8) * (int64_t)(goertzelp<<8) ) >> 32 ) - goertzelp2; \
|
||||||
|
goertzelp2 = goertzelp; \
|
||||||
|
goertzelp = goertzel; \
|
||||||
|
\
|
||||||
|
/* Float */ \
|
||||||
|
As = t + ( Acoeff * Asprev ) / 65536.0 - Asprev2; \
|
||||||
|
Asprev2 = Asprev; \
|
||||||
|
Asprev = As; \
|
||||||
|
\
|
||||||
|
/*printf( "%d,%d,%d,%d\n", ( ( (int64_t)(goertzel_coefficient) * (int64_t)goertzelp ) >> 32 ) , goertzel_coefficient, goertzelp, goertzelp2 ); */ \
|
||||||
|
\
|
||||||
|
{\
|
||||||
|
int32_t rr = (((int64_t)(g_goertzel_coefficient << 15) * (int64_t)goertzelp)>>32) - (goertzelp2); \
|
||||||
|
int32_t ri = (((int64_t)(g_goertzel_coefficient_s << 15) * (int64_t)goertzelp)>>32); \
|
||||||
|
/*printf( "%3d %10d %10d %10d %10d / %9d %9d %9d * ", js*4+x, rr, ri, goertzelp, goertzelp2, goertzel_coefficient, g_goertzel_omega_per_sample, t );*/ \
|
||||||
|
/*printf( "%4d %10d %10d (%10d %10d) ", js*4+x, goertzelp, goertzelp2, goertzel_coefficient, g_goertzel_coefficient_s );*/ \
|
||||||
|
printf( "%4d %10d %10d (%10d [%d+%d*=%lld] %10d) ", js*4+x, goertzelp, goertzelp2, rr, t, g_goertzel_coefficient, ( ( (int64_t)(goertzel_coefficient<<8) * (int64_t)(goertzelp2<<8) ) >> 32 ), ri ); \
|
||||||
|
\
|
||||||
|
float Apower = Asprev*Asprev + Asprev2*Asprev2 - (Acoeff * Asprev * Asprev2); \
|
||||||
|
double ArR = 0.5 * Acoeff * Asprev / 65536 - Asprev2; \
|
||||||
|
double ArI = 0.5 * Acoeff_s * Asprev / 65536; \
|
||||||
|
/*printf( "%14.3f, %14.3f (%14.3f %14.3f)\n", Asprev, Asprev2, Acoeff,Acoeff_s );*/ \
|
||||||
|
printf( "%14.3f, %14.3f (%14.3f %14.3f %f)\n", Asprev, Asprev2, ArR, ArI, sqrt( ArR*ArR +ArI*ArI ) ); \
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
ITERATION( 0 );
|
||||||
|
ITERATION( 1 );
|
||||||
|
ITERATION( 2 );
|
||||||
|
ITERATION( 3 );
|
||||||
|
|
||||||
|
adc_tail+=4;
|
||||||
|
goertzel_samples+=4;
|
||||||
|
// if( adc_tail == adc_buffer_top ) adc_tail = adc_buffer;
|
||||||
|
|
||||||
|
/* if( goertzel_samples == 128 )
|
||||||
|
{
|
||||||
|
g_goertzelp_store = goertzelp;
|
||||||
|
g_goertzelp2_store = goertzelp2;
|
||||||
|
|
||||||
|
goertzelp = 0;
|
||||||
|
goertzelp2 = 0;
|
||||||
|
|
||||||
|
g_goertzel_outs++;
|
||||||
|
goertzel_samples = 0;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
g_goertzelp2 = goertzelp2;
|
||||||
|
g_goertzelp = goertzelp;
|
||||||
|
g_goertzel_samples = goertzel_samples;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
Reference in New Issue
Block a user