Code: Select all

#include "stm32f4xx.h"
#include "arm_math.h"
#include <string.h>
#include <stdio.h>

// ===================
// ADF4351 PLL CONTROL
// ===================

#define ADF4351_CS_PIN      GPIO_PIN_4
#define ADF4351_CS_PORT     GPIOA
#define ADF4351_SCK_PIN     GPIO_PIN_5
#define ADF4351_MOSI_PIN    GPIO_PIN_7
#define ADF4351_LE_PIN      GPIO_PIN_4  // Same as CS

// ADF4351 Register structure
typedef struct {
    uint32_t R0;  // INT and FRAC values
    uint32_t R1;  // Phase and modulus
    uint32_t R2;  // LDF, LDP, PD polarity, etc
    uint32_t R3;  // SD, clk divider, CSR
    uint32_t R4;  // Output power, RF output enable
    uint32_t R5;  // LD pin mode
} ADF4351_Regs;

// PLL Configuration structure
typedef struct {
    uint32_t frequency_hz;   // Output frequency in Hz
    uint32_t ref_freq_hz;    // Reference frequency (25MHz TCXO)
    uint16_t int_value;      // Integer part of division
    uint16_t frac_value;     // Fractional part
    uint16_t mod_value;      // Modulus (4095 for fractional-N)
    uint8_t  rf_divider;     // RF output divider (1, 2, 4, 8, 16, 32, 64)
    uint8_t  output_power;   // Output power (0-3, corresponds to -4 to +5dBm)
} PLL_Config;

/**
 * Initialize SPI for ADF4351 communication
 */
void ADF4351_SPI_Init(void) {
    // Enable clocks
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
    
    // Configure GPIO pins for SPI1
    // PA4 = CS (output), PA5 = SCK, PA7 = MOSI
    GPIOA->MODER |= (2 << (5*2)) | (2 << (7*2));  // Alternate function
    GPIOA->MODER |= (1 << (4*2));                  // CS as output
    GPIOA->AFR[0] |= (5 << (5*4)) | (5 << (7*4)); // AF5 = SPI1
    GPIOA->OSPEEDR |= (3 << (5*2)) | (3 << (7*2)) | (3 << (4*2)); // High speed
    
    // Set CS high (idle)
    GPIOA->BSRR = (1 << 4);
    
    // Configure SPI1
    SPI1->CR1 = 0;
    SPI1->CR1 |= SPI_CR1_MSTR;      // Master mode
    SPI1->CR1 |= SPI_CR1_BR_2;      // Baud rate = fPCLK/32 (5.25MHz @ 168MHz)
    SPI1->CR1 |= SPI_CR1_CPOL;      // Clock polarity high when idle
    SPI1->CR1 |= SPI_CR1_CPHA;      // Clock phase: capture on 2nd edge
    SPI1->CR1 |= SPI_CR1_SSM | SPI_CR1_SSI; // Software CS management
    SPI1->CR1 |= SPI_CR1_SPE;       // Enable SPI
}

/**
 * Write 32-bit register to ADF4351
 */
void ADF4351_WriteRegister(uint32_t data) {
    // CS low
    GPIOA->BSRR = (1 << (4 + 16));
    
    // Send 4 bytes (MSB first)
    for (int i = 3; i >= 0; i--) {
        while (!(SPI1->SR & SPI_SR_TXE));
        SPI1->DR = (data >> (i * 8)) & 0xFF;
    }
    
    // Wait for transmission complete
    while (SPI1->SR & SPI_SR_BSY);
    
    // Small delay
    for (volatile int i = 0; i < 100; i++);
    
    // CS high (latch data)
    GPIOA->BSRR = (1 << 4);
    
    // Latch pulse width (minimum 20ns, we're way over that)
    for (volatile int i = 0; i < 100; i++);
}

/**
 * Calculate PLL parameters for desired frequency
 * Uses fractional-N mode for 1Hz resolution
 */
void ADF4351_CalculateFrequency(PLL_Config *config, uint32_t freq_hz) {
    config->frequency_hz = freq_hz;
    config->ref_freq_hz = 25000000;  // 25MHz TCXO
    config->mod_value = 4095;         // Maximum modulus for best phase noise
    
    // Determine RF divider (output must be 2.2-4.4GHz from VCO)
    uint8_t divider = 1;
    uint32_t vco_freq = freq_hz;
    
    while (vco_freq < 2200000000UL) {
        divider *= 2;
        vco_freq *= 2;
    }
    
    config->rf_divider = divider;
    
    // Calculate N = INT + (FRAC/MOD)
    // N = (VCO_freq * RF_div) / (PFD_freq * channel_spacing)
    // For 25MHz reference and 1Hz channel spacing:
    uint32_t pfd_freq = config->ref_freq_hz; // Assuming R=1
    
    float N = (float)vco_freq / (float)pfd_freq;
    config->int_value = (uint16_t)N;
    config->frac_value = (uint16_t)((N - config->int_value) * config->mod_value);
    
    config->output_power = 3;  // Maximum output power (+5dBm)
}

/**
 * Initialize ADF4351 and set frequency
 */
void ADF4351_Init(uint32_t frequency_hz) {
    PLL_Config config;
    ADF4351_CalculateFrequency(&config, frequency_hz);
    
    // Register 5: Reserved
    uint32_t R5 = 0x00580005;
    ADF4351_WriteRegister(R5);
    
    // Register 4: Output power, RF output enable, AUX output settings
    uint32_t R4 = 0x008C803C;  // Output power = +5dBm, RF output enabled
    ADF4351_WriteRegister(R4);
    
    // Register 3: CLK DIV MODE, Clock divider value
    uint32_t R3 = 0x000004B3;  // Clk div mode = 0, ABP=1 (3ns)
    ADF4351_WriteRegister(R3);
    
    // Register 2: Digital lock detect, LDF, charge pump, etc
    uint32_t R2 = 0x08008E42;  // LDF = Frac-N, CP = 2.5mA, LDP = 6ns
    ADF4351_WriteRegister(R2);
    
    // Register 1: Phase value and modulus
    uint32_t R1 = 0x08008001;  // Phase = 1
    R1 |= ((uint32_t)config.mod_value << 3);  // Set modulus
    ADF4351_WriteRegister(R1);
    
    // Register 0: Integer and fractional values
    uint32_t R0 = 0x00000000;
    R0 |= ((uint32_t)config.int_value << 15);   // INT value
    R0 |= ((uint32_t)config.frac_value << 3);   // FRAC value
    ADF4351_WriteRegister(R0);
    
    // Small delay for PLL to lock
    for (volatile int i = 0; i < 1000000; i++);
}

/**
 * Set new frequency (fast frequency change)
 */
void ADF4351_SetFrequency(uint32_t frequency_hz) {
    PLL_Config config;
    ADF4351_CalculateFrequency(&config, frequency_hz);
    
    // Only need to update R0 (INT/FRAC) for frequency change
    uint32_t R0 = 0x00000000;
    R0 |= ((uint32_t)config.int_value << 15);
    R0 |= ((uint32_t)config.frac_value << 3);
    ADF4351_WriteRegister(R0);
}

// ============
// RDS ENCODER
// ============

#define RDS_BITRATE         1187.5f     // RDS bit rate in Hz
#define RDS_GROUP_LENGTH    104         // Bits per RDS group
#define RDS_CARRIER_FREQ    57000.0f    // 57kHz RDS subcarrier

typedef struct {
    char ps_name[9];        // Program Service name (8 chars + null)
    char radio_text[65];    // Radio Text (64 chars + null)
    uint16_t pi_code;       // Program Identification
    uint8_t pty;            // Program Type
    uint8_t tp;             // Traffic Program flag
    uint8_t ta;             // Traffic Announcement flag
} RDS_Data;

// RDS encoder state
typedef struct {
    RDS_Data data;
    uint8_t group_buffer[13];  // 104 bits = 13 bytes
    uint16_t bit_position;
    uint8_t ps_segment;        // Current PS segment (0-3)
    uint8_t rt_segment;        // Current RT segment
} RDS_Encoder;

static RDS_Encoder rds_encoder = {0};

/**
 * Calculate syndrome for RDS error checking (10-bit checkword)
 */
uint16_t RDS_CalculateSyndrome(uint16_t block, uint16_t offset) {
    uint32_t data = ((uint32_t)block << 10);
    uint32_t poly = 0x5B9; // Generator polynomial
    
    // Polynomial division
    for (int i = 0; i < 16; i++) {
        if (data & 0x2000000) {
            data ^= (poly << 14);
        }
        data <<= 1;
    }
    
    return ((data >> 14) & 0x3FF) ^ offset;
}

/**
 * Initialize RDS encoder
 */
void RDS_Init(const char *ps_name, const char *radio_text, uint16_t pi_code) {
    memset(&rds_encoder, 0, sizeof(RDS_Encoder));
    
    strncpy(rds_encoder.data.ps_name, ps_name, 8);
    strncpy(rds_encoder.data.radio_text, radio_text, 64);
    rds_encoder.data.pi_code = pi_code;
    rds_encoder.data.pty = 0;  // No program type
    rds_encoder.data.tp = 0;
    rds_encoder.data.ta = 0;
}

/**
 * Generate RDS Group 0A (Basic tuning and switching + PS)
 */
void RDS_GenerateGroup0A(uint8_t *buffer) {
    // Block 1: PI code
    uint16_t block1 = rds_encoder.data.pi_code;
    uint16_t check1 = RDS_CalculateSyndrome(block1, 0x0FC); // Offset word A
    
    // Block 2: Group type 0A
    uint16_t block2 = 0;
    block2 |= (0x0 << 12);  // Group type 0
    block2 |= (0x0 << 11);  // Version A
    block2 |= (rds_encoder.data.tp << 10);
    block2 |= (rds_encoder.data.pty << 5);
    block2 |= (rds_encoder.data.ta << 4);
    block2 |= rds_encoder.ps_segment;  // PS segment address
    uint16_t check2 = RDS_CalculateSyndrome(block2, 0x198); // Offset word B
    
    // Block 3: AF codes (not implemented, using filler)
    uint16_t block3 = 0xCD15;  // Filler code
    uint16_t check3 = RDS_CalculateSyndrome(block3, 0x168); // Offset word C
    
    // Block 4: 2 characters of PS name
    uint16_t block4 = (rds_encoder.data.ps_name[rds_encoder.ps_segment * 2] << 8) |
                      rds_encoder.data.ps_name[rds_encoder.ps_segment * 2 + 1];
    uint16_t check4 = RDS_CalculateSyndrome(block4, 0x1B4); // Offset word D
    
    // Pack into buffer
    buffer[0] = (block1 >> 8);
    buffer[1] = (block1 & 0xFF);
    buffer[2] = (check1 >> 2);
    buffer[3] = ((check1 & 0x03) << 6) | ((block2 >> 10) & 0x3F);
    buffer[4] = (block2 >> 2);
    buffer[5] = ((block2 & 0x03) << 6) | ((check2 >> 4) & 0x3F);
    buffer[6] = ((check2 & 0x0F) << 4) | ((block3 >> 12) & 0x0F);
    buffer[7] = (block3 >> 4);
    buffer[8] = ((block3 & 0x0F) << 4) | ((check3 >> 6) & 0x0F);
    buffer[9] = ((check3 & 0x3F) << 2) | ((block4 >> 14) & 0x03);
    buffer[10] = (block4 >> 6);
    buffer[11] = ((block4 & 0x3F) << 2) | ((check4 >> 8) & 0x03);
    buffer[12] = check4 & 0xFF;
    
    // Advance PS segment
    rds_encoder.ps_segment = (rds_encoder.ps_segment + 1) % 4;
}

/**
 * Generate 57kHz RDS subcarrier modulated with data
 * This should be called from a timer interrupt at audio sample rate
 */
float RDS_GenerateSample(float sample_rate, uint32_t *phase_acc) {
    static uint8_t rds_group[13];
    static uint16_t bit_counter = 0;
    static uint8_t current_bit = 0;
    
    // Generate new group if needed
    if (bit_counter == 0) {
        RDS_GenerateGroup0A(rds_group);
    }
    
    // Get current bit
    uint8_t byte_index = bit_counter / 8;
    uint8_t bit_index = 7 - (bit_counter % 8);
    current_bit = (rds_group[byte_index] >> bit_index) & 0x01;
    
    // Advance bit counter at RDS bit rate
    static float bit_phase = 0;
    bit_phase += RDS_BITRATE / sample_rate;
    if (bit_phase >= 1.0f) {
        bit_phase -= 1.0f;
        bit_counter = (bit_counter + 1) % RDS_GROUP_LENGTH;
    }
    
    // Generate 57kHz carrier with BPSK modulation
    float phase_increment = (2.0f * PI * RDS_CARRIER_FREQ) / sample_rate;
    *phase_acc += (uint32_t)(phase_increment * 4294967296.0f);
    
    // BPSK: phase shift of 180° for bit value
    float carrier_phase = (*phase_acc / 4294967296.0f) * 2.0f * PI;
    if (current_bit) {
        carrier_phase += PI;  // 180° phase shift for '1'
    }
    
    return arm_sin_f32(carrier_phase) * 0.1f;  // 10% injection level
}

// ====================
// STEREO MPX GENERATOR
// ====================

#define PILOT_FREQ      19000.0f    // 19kHz pilot tone
#define SUBCARRIER_FREQ 38000.0f    // 38kHz stereo subcarrier

typedef struct {
    uint32_t pilot_phase;
    uint32_t subcarrier_phase;
    float audio_left;
    float audio_right;
} MPX_Generator;

static MPX_Generator mpx_gen = {0};

/**
 * Generate stereo MPX composite signal
 * Output = (L+R) + 0.1*19kHz_pilot + (L-R)*cos(38kHz)
 */
float MPX_GenerateSample(float left, float right, float sample_rate) {
    float composite = 0;
    
    // Main channel (L+R)
    float main_channel = (left + right) * 0.45f;  // 45% modulation each
    composite += main_channel;
    
    // 19kHz pilot tone (10% modulation)
    float pilot_increment = (2.0f * PI * PILOT_FREQ) / sample_rate;
    mpx_gen.pilot_phase += (uint32_t)(pilot_increment * 4294967296.0f);
    float pilot = arm_sin_f32((mpx_gen.pilot_phase / 4294967296.0f) * 2.0f * PI);
    composite += pilot * 0.1f;
    
    // 38kHz subcarrier with (L-R) DSB-SC modulation
    float subchannel = (left - right) * 0.45f;
    float sub_increment = (2.0f * PI * SUBCARRIER_FREQ) / sample_rate;
    mpx_gen.subcarrier_phase += (uint32_t)(sub_increment * 4294967296.0f);
    float subcarrier = arm_cos_f32((mpx_gen.subcarrier_phase / 4294967296.0f) * 2.0f * PI);
    composite += subchannel * subcarrier;
    
    return composite;
}

// ================
// AUDIO PROCESSING
// ================

#define PREEMPHASIS_TAU     75e-6f  // 75μs time constant (US standard)
#define SAMPLE_RATE         48000.0f

typedef struct {
    float last_input;
    float last_output;
    float alpha;  // Filter coefficient
} PreEmphasis_Filter;

static PreEmphasis_Filter preemph_left = {0};
static PreEmphasis_Filter preemph_right = {0};

/**
 * Initialize pre-emphasis filter (75μs)
 */
void Audio_InitPreEmphasis(PreEmphasis_Filter *filter, float sample_rate) {
    float fc = 1.0f / (2.0f * PI * PREEMPHASIS_TAU);  // Corner frequency ≈ 2.1kHz
    filter->alpha = 1.0f / (1.0f + sample_rate / (2.0f * PI * fc));
    filter->last_input = 0;
    filter->last_output = 0;
}

/**
 * Apply pre-emphasis filter
 */
float Audio_PreEmphasis(PreEmphasis_Filter *filter, float input) {
    // High-pass filter: y[n] = α*(y[n-1] + x[n] - x[n-1])
    float output = filter->alpha * (filter->last_output + input - filter->last_input);
    filter->last_input = input;
    filter->last_output = output;
    return output;
}

/**
 * Soft clipper/limiter to prevent overmodulation
 */
float Audio_SoftClip(float input, float threshold) {
    if (input > threshold) {
        return threshold + (input - threshold) / (1.0f + ((input - threshold) / 0.1f));
    } else if (input < -threshold) {
        return -threshold + (input + threshold) / (1.0f - ((input + threshold) / 0.1f));
    }
    return input;
}

/**
 * AGC (Automatic Gain Control)
 */
typedef struct {
    float gain;
    float target_level;
    float attack_coeff;
    float release_coeff;
} AGC_State;

static AGC_State agc = {
    .gain = 1.0f,
    .target_level = 0.7f,
    .attack_coeff = 0.01f,
    .release_coeff = 0.001f
};

float Audio_AGC(float input) {
    float output = input * agc.gain;
    float level = fabsf(output);
    
    // Adjust gain based on level
    if (level > agc.target_level) {
        // Attack: reduce gain quickly
        agc.gain -= (level - agc.target_level) * agc.attack_coeff;
    } else {
        // Release: increase gain slowly
        agc.gain += (agc.target_level - level) * agc.release_coeff;
    }
    
    // Limit gain range
    if (agc.gain < 0.1f) agc.gain = 0.1f;
    if (agc.gain > 2.0f) agc.gain = 2.0f;
    
    return output;
}

// ========================
// MAIN PROCESSING FUNCTION
// ========================

/**
 * Process complete audio chain: AGC → Pre-emphasis → MPX → RDS
 * Call this from audio sample rate interrupt (e.g., 48kHz)
 */
float FM_ProcessAudio(float left_in, float right_in) {
    // Step 1: AGC
    left_in = Audio_AGC(left_in);
    right_in = Audio_AGC(right_in);
    
    // Step 2: Pre-emphasis (75μs)
    float left_preemph = Audio_PreEmphasis(&preemph_left, left_in);
    float right_preemph = Audio_PreEmphasis(&preemph_right, right_in);
    
    // Step 3: Soft clipping to prevent overmodulation
    left_preemph = Audio_SoftClip(left_preemph, 0.9f);
    right_preemph = Audio_SoftClip(right_preemph, 0.9f);
    
    // Step 4: Generate stereo MPX composite
    float mpx_output = MPX_GenerateSample(left_preemph, right_preemph, SAMPLE_RATE);
    
    // Step 5: Add RDS subcarrier
    static uint32_t rds_phase = 0;
    float rds_signal = RDS_GenerateSample(SAMPLE_RATE, &rds_phase);
    mpx_output += rds_signal;
    
    // Final limiting to ±75kHz deviation
    mpx_output = Audio_SoftClip(mpx_output, 1.0f);
    
    return mpx_output;
}

// ===================
// MAIN INITIALIZATION
// ===================

/**
 * Initialize complete FM transmitter system
 */
void FM_Transmitter_Init(void) {
    // Initialize ADF4351 PLL at 88.5 MHz
    ADF4351_SPI_Init();
    ADF4351_Init(88500000);  // 88.5 MHz
    
    // Initialize audio processing
    Audio_InitPreEmphasis(&preemph_left, SAMPLE_RATE);
    Audio_InitPreEmphasis(&preemph_right, SAMPLE_RATE);
    
    // Initialize RDS
    RDS_Init("MYCALL  ", "This is a test of the RDS system - STM32F4 power!", 0x1234);
    
    // TODO: Initialize I2S/DAC for audio input/output
    // TODO: Initialize timer interrupt at 48kHz for audio processing
}

/**
 * Change transmit frequency
 */
void FM_SetFrequency(float freq_mhz) {
    uint32_t freq_hz = (uint32_t)(freq_mhz * 1000000.0f);
    ADF4351_SetFrequency(freq_hz);
}

/**
 * Update RDS text
 */
void FM_UpdateRDS(const char *ps_name, const char *radio_text) {
    strncpy(rds_encoder.data.ps_name, ps_name, 8);
    strncpy(rds_encoder.data.radio_text, radio_text, 64);
}

// ======
//  MAIN
// ======

int main(void) {
    // System initialization
    SystemInit();
    
    // Initialize FM transmitter
    FM_Transmitter_Init();
    
    // Set frequency to 88.5 MHz
    FM_SetFrequency(88.5f);