diff options
Diffstat (limited to 'center_fw/src/main.c')
| -rw-r--r-- | center_fw/src/main.c | 337 |
1 files changed, 233 insertions, 104 deletions
diff --git a/center_fw/src/main.c b/center_fw/src/main.c index 274abad..59bd7a1 100644 --- a/center_fw/src/main.c +++ b/center_fw/src/main.c @@ -17,11 +17,54 @@ #include "global.h" #include "8b10b.h" +#include "crc32.h" +#include "protocol.h" +#include "xorshift.h" -static uint16_t adc_data[64*2]; static volatile struct state_8b10b_dec st_8b10b_dec; -static void quicksort(uint16_t *head, uint16_t *tail); +/* Modulation constants */ +#define THRESHOLD_ADC_COUNTS 28500 /* ADC counts */ +#define MIN_RECTIFIER_MARGIN 5000 /* ADC counts */ +#define SAMPLES_PER_BAUD 16 +#define OVERSAMPLING_RATIO 16 +#define SAMPLING_PHASE (SAMPLES_PER_BAUD / 2) +#define LED_DEAD_TIME 4 /* in ADC samples */ +#ifndef CONFIG_MODULE_ADDRESS +#warn "CONFIG_MODULE_ADDRESS is not defined, defaulting to 0." +#define CONFIG_MODULE_ADDRESS 0 +#endif /* CONFIG_MODULE_ADDRESS */ + +#define DEBUG_DISABLE_DRIVERS 1 + + +volatile union { + struct data_packet packet; + uint8_t bytes[sizeof(struct data_packet)]; +} rx_buf; + +volatile ssize_t rx_pos; +volatile bool packet_received; +volatile bool rng_reset; +uint32_t packet_rng_state = 0; + +struct data_packet foobar; +int global_brightness; +int channel_mask; + +struct error_counters { + int crc_errors; + int receive_overflows; + int processing_overflows; + int decoding_errors; +} errors; + +/* generated by ./gamma.py */ +uint16_t brightness_lut[16] = { + 54, 247, 604, 1137, 1857, 2774, 3894, 5223, + 6768, 8534, 10525, 12745, 15199, 17891, 20823, 24000 +}; + int main(void) { /* Configure clocks for 64 MHz system clock. @@ -49,40 +92,35 @@ int main(void) { RCC->AHBENR |= RCC_AHBENR_DMA1EN; RCC->APBENR1 |= RCC_APBENR1_TIM3EN | RCC_APBENR1_DBGEN; - RCC->APBENR2 |= RCC_APBENR2_TIM1EN | RCC_APBENR2_ADCEN; + RCC->APBENR2 |= RCC_APBENR2_TIM1EN | RCC_APBENR2_ADCEN | RCC_APBENR2_TIM14EN; RCC->IOPENR |= RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN | RCC_IOPENR_GPIOCEN; - /* - TIM1->PSC = 0; - TIM1->ARR = nominal_period; - TIM1->DIER = TIM_DIER_UIE | TIM_DIER_CC1IE; - TIM1->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN; - TIM1->CCR1 = 3000; - NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn); - NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 0); - NVIC_EnableIRQ(TIM1_CC_IRQn); - NVIC_SetPriority(TIM1_CC_IRQn, 0); - */ + TIM14->CR1 = TIM_CR1_ARPE | TIM_CR1_OPM; + /* External clock mode, with TIM 3 as source */ + TIM14->PSC = 0; + static_assert(125 * (SAMPLES_PER_BAUD - LED_DEAD_TIME) * OVERSAMPLING_RATIO <= 0xffff); + TIM14->ARR = 125 * (SAMPLES_PER_BAUD - LED_DEAD_TIME) * OVERSAMPLING_RATIO; + TIM14->CCER = TIM_CCER_CC1E; + TIM14->DIER = TIM_DIER_CC1IE; + NVIC_EnableIRQ(TIM14_IRQn); + NVIC_SetPriority(TIM14_IRQn, 1<<6); + + for (int i=0; i<COUNT_OF(brightness_lut); i++) { + } xfr_8b10b_reset((struct state_8b10b_dec *)&st_8b10b_dec); + rx_pos = -1; + packet_received = false; + rng_reset = false; + memset(&errors, 0, sizeof(errors)); TIM3->CR1 = TIM_CR1_ARPE; TIM3->CR2 = (2<<TIM_CR2_MMS_Pos); /* Update event on TRGO */ TIM3->PSC = 0; /* We sample 32 times per 1 kHz AC cycle, and use 32 times oversampling. */ - TIM3->ARR = 125*16; /* Output 64 MHz / 125 = 512 kHz signal */ + TIM3->ARR = 125; /* Output 64 MHz / 125 = 512.0 kHz signal */ TIM3->CR1 |= TIM_CR1_CEN; - DMAMUX1[0].CCR = 5; /* ADC */ - DMA1_Channel1->CPAR = (uint32_t)&ADC1->DR; - DMA1_Channel1->CMAR = (uint32_t)(void *)adc_data; - DMA1_Channel1->CNDTR = COUNT_OF(adc_data); - DMA1_Channel1->CCR = (1<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_HTIE | DMA_CCR_TCIE; - DMA1_Channel1->CCR |= DMA_CCR_EN; - - NVIC_EnableIRQ(DMA1_Channel1_IRQn); - NVIC_SetPriority(DMA1_Channel1_IRQn, 64); - ADC1->ISR = ADC_ISR_CCRDY | ADC_ISR_ADRDY; /* Clear CCRDY */ ADC1->CR = ADC_CR_ADVREGEN; delay_us(20); @@ -90,8 +128,8 @@ int main(void) { while (ADC1->CR & ADC_CR_ADCAL) { /* wait. */ } - ADC1->CFGR1 = (1<<ADC_CFGR1_EXTEN_Pos) | (3<<ADC_CFGR1_EXTSEL_Pos) | ADC_CFGR1_DMAEN | ADC_CFGR1_DMACFG; /* TIM3 TRGO */ - ADC1->CFGR2 = (1<<ADC_CFGR2_CKMODE_Pos) | (4<<ADC_CFGR2_OVSR_Pos) | (1<<ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_OVSE; + ADC1->CFGR1 = (1<<ADC_CFGR1_EXTEN_Pos) | (3<<ADC_CFGR1_EXTSEL_Pos); /* TIM3 TRGO */ + ADC1->CFGR2 = (1<<ADC_CFGR2_CKMODE_Pos) | (3<<ADC_CFGR2_OVSR_Pos) | (0<<ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_TOVS | ADC_CFGR2_OVSE; ADC1->CHSELR = (1<<4); /* Enable input 4 -> PA4 (Vdiff)*/ while (!(ADC1->ISR & ADC_ISR_CCRDY)) { /* wait. */ @@ -102,6 +140,9 @@ int main(void) { while (!(ADC1->ISR & ADC_ISR_ADRDY)) { /* wait. */ } + ADC1->IER = ADC_IER_EOCIE; + NVIC_EnableIRQ(ADC1_IRQn); + NVIC_SetPriority(ADC1_IRQn, 0); ADC1->CR |= ADC_CR_ADSTART; GPIOA->MODER = OUT(0) | IN(1) | OUT(2) | OUT(3) | ANALOG(4) | OUT(5) | OUT(6) | IN(7) | ANALOG(9) | ANALOG(10) | OUT(11) | ANALOG(12)| AF(13) | AF(14); @@ -111,73 +152,187 @@ int main(void) { DBG->APBFZ1 |= DBG_APB_FZ1_DBG_TIM3_STOP; DBG->APBFZ2 |= DBG_APB_FZ2_DBG_TIM1_STOP; while (42) { + if (packet_received) { + if (rng_reset) { + packet_rng_state = xorshift32(1); + } + + for(size_t i=0; i<sizeof(rx_buf.packet); i++) { + packet_rng_state = xorshift32(packet_rng_state); + // rx_buf.bytes[i] ^= packet_rng_state; FIXME DEBUG + } + + uint32_t crc_state = crc32_reset(); + for(size_t i=0; i<offsetof(struct data_packet, crc); i++) { + crc_state = crc32_update(crc_state, rx_buf.bytes[i]); + } + crc_state = crc32_finalize(crc_state); + + if (crc_state == rx_buf.packet.crc) { + /* good packet received */ + int val = rx_buf.packet.brightness[CONFIG_MODULE_ADDRESS/2]; + if (CONFIG_MODULE_ADDRESS & 1) { + val >>= 4; + } + global_brightness = val; + channel_mask = rx_buf.packet.channels[CONFIG_MODULE_ADDRESS]; + + } else { + errors.crc_errors++; + } + + packet_received = false; + } } } -/* -void TIM1_BRK_UP_TRG_COM_IRQHandler(void) { - TIM1->SR &= ~TIM_SR_UIF; -} +int16_t sym_dump[512]; +size_t sym_dump_pos = 0; + +uint8_t adc_dump[32]; +size_t adc_dump_pos = 0; + +uint8_t bit_dump[4096]; +size_t bit_dump_pos = 0; -void TIM1_CC_IRQHandler(void) { - TIM1->SR &= ~TIM_SR_CC1IF; +void gdb_dump(void) { } -*/ -static size_t received_symbols = 0; -static int symbol_buf[64]; -static size_t received_bits = 0; -static int16_t bit_buf[256]; -size_t adc_reduced_pos = 0; -static uint8_t adc_reduced[4096]; - -void DMA1_Channel1_IRQHandler(void) { - static int sampling_phase = 0; - static int last_sample = 0; - - uint16_t *buf = (DMA1->ISR & DMA_ISR_HTIF1) ? &adc_data[0] : &adc_data[COUNT_OF(adc_data)/2]; - DMA1->IFCR = DMA_IFCR_CGIF1; + +void ADC1_IRQHandler(void) { + static int phase = 0; + static int last_bit = 0; GPIOB->BSRR = (1<<7); - const int threshold_adc_counts = 28500; - const int sample_per_baud = 16; + /* Read sample and apply threshold */ + int sample = ADC1->DR; /* resets the EOC interrupt flag */ + int bit = sample > THRESHOLD_ADC_COUNTS; + int bit_margin = ((int)sample) - THRESHOLD_ADC_COUNTS; + if (bit_margin < 0) { + bit_margin = -bit_margin; + } + + adc_dump[adc_dump_pos] = (sample>>10) & 0x3f; - for (size_t i=0; i<COUNT_OF(adc_data)/2; i++) { - int sample = buf[i]; + /* Find edges and compute current phase */ + if (bit && !last_bit) { /* rising edge */ + phase = 0; + adc_dump[adc_dump_pos] |= 0x40; - adc_reduced[adc_reduced_pos] = (sample & 0xffff)>>9; + } else if (last_bit && !bit) { /* falling edge */ + phase = 0; + adc_dump[adc_dump_pos] |= 0x40; - if ((last_sample <= threshold_adc_counts && sample >= threshold_adc_counts) || - (last_sample >= threshold_adc_counts && sample <= threshold_adc_counts)){ - sampling_phase = sample_per_baud / 4; /* /2 for half baud sampling point, /2 for sinusoidal edge shape */ + } else { + phase ++; + if (phase == SAMPLES_PER_BAUD) { + phase = 0; + } + } - } else if (sampling_phase == 0) { - int bit = sample > threshold_adc_counts; - adc_reduced[adc_reduced_pos] |= 0x80; + /* Trigger 8b10b sample */ + if (phase == SAMPLING_PHASE) { + adc_dump[adc_dump_pos] |= 0x80; - bit_buf[received_bits] = bit; - received_bits = (received_bits+1) % COUNT_OF(bit_buf); + bit_dump[bit_dump_pos] = bit; + bit_dump_pos++; + if (bit_dump_pos == COUNT_OF(bit_dump)) { + bit_dump_pos = 0; + gdb_dump(); + } - int rc = xfr_8b10b_feed_bit((struct state_8b10b_dec *)&st_8b10b_dec, bit); - if (rc > -K_CODES_LAST) { - symbol_buf[received_symbols] = rc; - received_symbols = (received_symbols+1) % COUNT_OF(symbol_buf); + int rc = xfr_8b10b_feed_bit((struct state_8b10b_dec *)&st_8b10b_dec, bit); + if (rc > -K_CODES_LAST) { + sym_dump[sym_dump_pos++] = rc; + if (sym_dump_pos == COUNT_OF(sym_dump)) { + sym_dump_pos = 0; } - sampling_phase = sample_per_baud; + if (rc < 0) { + if (rc == -K28_1) { + rng_reset = true; + rx_pos = 0; + + } else if (rc == -K27_7) { + if (rx_pos >= 0) { + rx_pos = 0; + } + } else { + rx_pos = -1; + } + } else { + if (packet_received) { + /* receive buffer overflow */ + rx_pos = -1; + errors.processing_overflows++; + + } else { + if (rx_pos == sizeof(rx_buf.packet)) { + /* receive buffer overflow */ + rx_pos = -1; + errors.receive_overflows++; + } + + rx_buf.bytes[rx_pos] = rc; + rx_pos++; + if (rx_pos == sizeof(rx_buf.packet)) { + packet_received = true; + } + } + } } else { - sampling_phase--; + errors.decoding_errors++; } + } + + adc_dump_pos++; + if (adc_dump_pos == COUNT_OF(adc_dump)) { + adc_dump_pos = 0; + } - adc_reduced_pos++; - if (adc_reduced_pos == COUNT_OF(adc_reduced)) { - adc_reduced_pos =0; + /* Trigger synchronous rectifier */ + if (phase == SAMPLES_PER_BAUD - LED_DEAD_TIME || bit != last_bit || bit_margin < MIN_RECTIFIER_MARGIN) { /* reset */ + GPIOA->BRR = (1<<11); /* RECT1 */ + GPIOC->BRR = (1<<15); /* RECT2 */ + GPIOA->BRR = (1<<6); + + } else if (phase == LED_DEAD_TIME) { /* set */ + if (bit) { + GPIOA->BSRR = (1<<6); +#ifndef DEBUG_DISABLE_DRIVERS + GPIOC->BSRR = (1<<15); /* RECT2 */ +#endif + } else { +#ifndef DEBUG_DISABLE_DRIVERS + GPIOA->BSRR = (1<<11); /* RECT1 */ +#endif } - last_sample = sample; + + int nibble = (bit ? (channel_mask >> 4) : channel_mask) & 0x0f; + int b0 = (nibble>>0) & 1; + int b1 = (nibble>>1) & 1; + int b2 = (nibble>>2) & 1; + int b3 = (nibble>>3) & 1; + +#ifndef DEBUG_DISABLE_DRIVERS + GPIOA->BSRR = (b0<<2) | (b3<<3) | (b2<<5); + GPIOB->BSRR = (b1<<3); +#endif + TIM14->CCR1 = brightness_lut[global_brightness]; + TIM14->CR1 |= TIM_CR1_CEN; } + last_bit = bit; GPIOB->BRR = (1<<7); } +void TIM14_IRQHandler(void) { + TIM14->SR = 0; + + /* Reset all LED outputs */ + GPIOA->BRR = (1<<2) | (1<<3) | (1<<5); + GPIOB->BRR = (1<<3); +} + void delay_us(int duration_us) { while (duration_us--) { for (int i=0; i<32; i++) { @@ -186,6 +341,14 @@ void delay_us(int duration_us) { } } +void *memset(void *s, int c, size_t n) { + uint8_t *b = (uint8_t *)s; + while (n--) { + *b++ = c; + } + return s; +} + void NMI_Handler(void) { asm volatile ("bkpt"); } @@ -208,37 +371,3 @@ void __libc_init_array (void) __attribute__((weak)); void __libc_init_array () { } -/* https://github.com/openmv/openmv/blob/2e8d5d505dbe695b8009d832e5ef7691009148e1/src/omv/common/array.c#L117 */ -static void quicksort(uint16_t *head, uint16_t *tail) { - while (head < tail) { - uint16_t *h = head - 1; - uint16_t *t = tail; - uint16_t v = tail[0]; - for (;;) { - do { - ++h; - } while (h < t && h[0] < v); - do { - --t; - } while (h < t && v < t[0]); - if (h >= t) { - break; - } - uint16_t x = h[0]; - h[0] = t[0]; - t[0] = x; - } - uint16_t x = h[0]; - h[0] = tail[0]; - tail[0] = x; - // do the smaller recursive call first, to keep stack within O(log(N)) - if (t - head < tail - h - 1) { - quicksort(head, t); - head = h + 1; - } else { - quicksort(h + 1, tail); - tail = t; - } - } -} - |