feat: MEMS microphone software handling for TX15 and TX16SMK3 (#7328)

Co-authored-by: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
3djc
2026-06-03 13:31:40 +10:00
committed by GitHub
co-authored by Claude Sonnet 4.6
parent 13bba22a4d
commit a7d730fded
39 changed files with 1385 additions and 4 deletions
+4
View File
@@ -208,6 +208,10 @@ bool isBacklightEnabled();
int audioInit();
void audioConsumeCurrentBuffer();
#if defined(PDM_CLOCK)
#include "pdm_software_driver.h"
#endif
// Telemetry driver
#define INTMODULE_FIFO_SIZE 512
#define TELEMETRY_FIFO_SIZE 512
@@ -0,0 +1,289 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include "board.h"
#include "hal/gpio.h"
#include "stm32_gpio.h"
#include "stm32_dma.h"
#include "stm32_hal_ll.h"
#if defined(PDM_CAPTURE_DMA)
static constexpr uint32_t PDM_SAI_MCKDIV =
PDM_SAI_KER_FREQ / PDM_CLOCK_FREQ;
static_assert(PDM_SAI_MCKDIV >= 1 && PDM_SAI_MCKDIV <= 63,
"PDM_SAI MCKDIV out of range for SAI_xCR1_MCKDIV (6 bits)");
// Burst size: ~6 ms of PDM at 1.6 MHz (multiple of 32 required for word packing).
static constexpr uint32_t PDM_BURST_BITS = 10080;
static constexpr uint32_t PDM_BURST_WORDS = (PDM_BURST_BITS + 31) / 32;
static uint32_t pdmBurstBuf[PDM_BURST_WORDS];
static uint8_t lastSoundLevel = 0;
// 40000 bytes at 1.6 MHz = ~25 ms ring; gives ~21 ms slack for audio-task stalls.
static constexpr uint32_t PDM_RING_BYTES = 40000;
static uint8_t pdmRingBuf[PDM_RING_BYTES] __DMA_NO_CACHE;
static uint32_t pdmRingReadPos = 0;
// CIC integrator + comb state (used by pdmConvertToPCM, defined further down).
// Declared here so pdmStart() can zero them at session boundaries — without a
// reset the leftover integrator state from the previous session would emit a
// DC transient on the first decoded burst.
static int32_t cic_i1 = 0, cic_i2 = 0, cic_i3 = 0;
static int32_t cic_p1 = 0, cic_p2 = 0, cic_p3 = 0;
static uint32_t cic_bitsSeen = 0;
static bool pdmRunning = false;
void pdmStart()
{
if (pdmRunning) return;
// Per-session state reset. Must happen before the DMA starts producing data
// so the audio-task path never sees a half-reset filter.
cic_i1 = cic_i2 = cic_i3 = 0;
cic_p1 = cic_p2 = cic_p3 = 0;
cic_bitsSeen = 0;
pdmRingReadPos = 0;
lastSoundLevel = 0;
gpio_init_af(PDM_CLOCK, PDM_CLOCK_GPIO_AF, GPIO_PIN_SPEED_VERY_HIGH);
gpio_init(PDM_DATA, GPIO_IN, GPIO_PIN_SPEED_VERY_HIGH);
SET_BIT(RCC->APB2ENR, RCC_APB2ENR_SAI1EN);
(void)READ_BIT(RCC->APB2ENR, RCC_APB2ENR_SAI1EN);
SAI_Block_TypeDef* block = PDM_SAI_BLOCK;
CLEAR_BIT(block->CR1, SAI_xCR1_SAIEN);
while (READ_BIT(block->CR1, SAI_xCR1_SAIEN)) {}
block->CR1 = (0U << SAI_xCR1_MODE_Pos) // Master TX
| (0U << SAI_xCR1_PRTCFG_Pos) // Free protocol
| (4U << SAI_xCR1_DS_Pos) // 16-bit data
| SAI_xCR1_NODIV // BCLK = ker_ck / MCKDIV
| (PDM_SAI_MCKDIV << SAI_xCR1_MCKDIV_Pos);
block->CR2 = (1U << SAI_xCR2_FTH_Pos); // FIFO threshold = 1/4 full
block->FRCR = (15U << 0) // FRL = 15 (16-bit frame)
| (7U << 8); // FSALL = 7
block->SLOTR = (0U << 8) // NBSLOT = 0 -> 1 slot
| (1U << 16); // SLOTEN slot 0
// Prime the FIFO so the block starts clocking immediately (no underrun).
for (int i = 0; i < 4; ++i) block->DR = 0U;
SET_BIT(block->CR1, SAI_xCR1_SAIEN);
// TIM15 is free because FLYSKY_GIMBAL is OFF.
SET_BIT(RCC->APB2ENR, RCC_APB2ENR_TIM15EN);
(void)READ_BIT(RCC->APB2ENR, RCC_APB2ENR_TIM15EN);
stm32_dma_enable_clock(PDM_CAPTURE_DMA);
PDM_CAPTURE_TIMER->CR1 = 0;
PDM_CAPTURE_TIMER->PSC = 0;
PDM_CAPTURE_TIMER->ARR = (PDM_CAPTURE_TIMER_FREQ / PDM_CLOCK_FREQ) - 1U;
PDM_CAPTURE_TIMER->CNT = 0;
PDM_CAPTURE_TIMER->EGR = TIM_EGR_UG; // load PSC/ARR
PDM_CAPTURE_TIMER->SR = 0; // clear update flag
PDM_CAPTURE_TIMER->DIER = TIM_DIER_UDE; // fire DMA request on every UEV
LL_DMA_DeInit(PDM_CAPTURE_DMA, PDM_CAPTURE_DMA_STREAM);
LL_DMA_InitTypeDef dmaInit;
LL_DMA_StructInit(&dmaInit);
dmaInit.PeriphRequest = PDM_CAPTURE_DMA_REQUEST;
dmaInit.Mode = LL_DMA_MODE_CIRCULAR;
dmaInit.Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
dmaInit.PeriphOrM2MSrcAddress = (uintptr_t)&PDM_DATA_GPIO_PORT->IDR;
dmaInit.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
dmaInit.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
dmaInit.MemoryOrM2MDstAddress = (uintptr_t)pdmRingBuf;
dmaInit.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
dmaInit.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
dmaInit.NbData = PDM_RING_BYTES;
dmaInit.Priority = LL_DMA_PRIORITY_HIGH;
LL_DMA_Init(PDM_CAPTURE_DMA, PDM_CAPTURE_DMA_STREAM, &dmaInit);
LL_DMA_EnableStream(PDM_CAPTURE_DMA, PDM_CAPTURE_DMA_STREAM);
PDM_CAPTURE_TIMER->CR1 |= TIM_CR1_CEN;
pdmRunning = true;
}
void pdmStop()
{
if (!pdmRunning) return;
// Stop DMA pacing first so no further requests fire.
PDM_CAPTURE_TIMER->CR1 &= ~TIM_CR1_CEN;
PDM_CAPTURE_TIMER->DIER = 0;
// Disable the DMA stream and wait for it to actually stop. A subsequent
// pdmStart() calls LL_DMA_DeInit which expects the stream idle.
LL_DMA_DisableStream(PDM_CAPTURE_DMA, PDM_CAPTURE_DMA_STREAM);
while (LL_DMA_IsEnabledStream(PDM_CAPTURE_DMA, PDM_CAPTURE_DMA_STREAM)) {}
// Disable the SAI block — this stops the PDM clock to the mic, putting it
// into low-power mode.
SAI_Block_TypeDef* block = PDM_SAI_BLOCK;
CLEAR_BIT(block->CR1, SAI_xCR1_SAIEN);
while (READ_BIT(block->CR1, SAI_xCR1_SAIEN)) {}
pdmRunning = false;
}
static uint32_t pdmRingAvailable()
{
const uint32_t ndtr =
LL_DMA_GetDataLength(PDM_CAPTURE_DMA, PDM_CAPTURE_DMA_STREAM);
const uint32_t writePos = PDM_RING_BYTES - ndtr;
if (writePos >= pdmRingReadPos) return writePos - pdmRingReadPos;
return PDM_RING_BYTES - pdmRingReadPos + writePos;
}
bool pdmCapture()
{
uint32_t avail = pdmRingAvailable();
if (avail < PDM_BURST_BITS) return false;
// Skip ahead if DMA is about to overwrite unread data.
if (avail > PDM_RING_BYTES - PDM_BURST_BITS) {
const uint32_t target_avail = PDM_BURST_BITS + PDM_BURST_BITS / 2;
const uint32_t skip = avail - target_avail;
pdmRingReadPos = (pdmRingReadPos + skip) % PDM_RING_BYTES;
}
static_assert(PDM_BURST_BITS % 32U == 0U,
"PDM_BURST_BITS must be a multiple of 32");
uint32_t src = pdmRingReadPos;
for (uint32_t w = 0; w < PDM_BURST_WORDS; ++w) {
uint32_t acc = 0;
for (uint32_t b = 0; b < 32; ++b) {
const uint8_t s = pdmRingBuf[src];
if (++src >= PDM_RING_BYTES) src = 0;
acc = (acc << 1) | ((s >> PDM_DATA_GPIO_PIN) & 1U);
}
pdmBurstBuf[w] = acc;
}
pdmRingReadPos = src;
return true;
}
bool pdmUpdateSoundLevel()
{
if (!pdmCapture()) return false;
// Ones-density of the PDM bitstream approximates signal amplitude.
uint32_t ones = 0;
for (uint32_t w = 0; w < PDM_BURST_WORDS; ++w) {
ones += __builtin_popcount(pdmBurstBuf[w]);
}
const uint32_t total = PDM_BURST_WORDS * 32U;
const int32_t mid = (int32_t)(total / 2U);
int32_t dev = (int32_t)ones - mid;
if (dev < 0) dev = -dev;
uint32_t level = (uint32_t)dev * 200U / total;
if (level > 100U) level = 100U;
lastSoundLevel = (uint8_t)level;
return true;
}
uint8_t pdmGetSoundLevel()
{
return lastSoundLevel;
}
// ---------------------------------------------------------------------------
// PDM -> PCM conversion: 3rd-order CIC decimator (R = PDM_PCM_DECIMATION).
//
// Structure (Hogenauer):
// integrator ×3 at 1 MHz -> down-sample by R -> comb ×3 at 1 MHz / R
//
// Integrators run on 1-bit input (0 or 1), so for R=64 and N=3 the worst-case
// magnitude that has to be representable is R^N = 262 144 — fits easily in
// int32_t. 2's-complement wrap-around through the integrators is intentional
// and cancels in the comb stage.
//
// DC gain of the filter is R^N. Silence (50/50 bitstream) therefore sits at
// R^N / 2, and the useful signal swing is ±R^N / 2. We subtract the DC offset
// and scale so that full swing maps to the int16 range.
// ---------------------------------------------------------------------------
// PDM_POST_GAIN_SHIFT is defined in pdm_software_driver.h so trimSilence
// can scale its threshold from the same value.
// CIC integrator + comb state is declared near the top of the file (so
// pdmStart() can reset it on session boundaries).
uint32_t pdmConvertToPCM(int16_t* pcm, uint32_t max)
{
if (!pcm || max == 0) return 0;
constexpr uint32_t R = PDM_PCM_DECIMATION;
constexpr int32_t G = (int32_t)(R * R * R);
constexpr int32_t DC = G / 2;
// floor_log2(DC) - 14: ensures (c3 - DC) >> SCALE_SHIFT fits in int16.
// Works for any R, not just powers of two.
constexpr int SCALE_SHIFT = (31 - __builtin_clz((unsigned)(DC))) - 14;
static_assert(SCALE_SHIFT >= 0, "Decimation too small to fit PCM range");
uint32_t produced = 0;
const uint32_t totalBits = PDM_BURST_WORDS * 32U;
for (uint32_t idx = 0; idx < totalBits && produced < max; ++idx) {
const uint32_t word = pdmBurstBuf[idx >> 5];
const int32_t bit = (int32_t)((word >> (31U - (idx & 31U))) & 1U);
cic_i1 += bit;
cic_i2 += cic_i1;
cic_i3 += cic_i2;
if (++cic_bitsSeen == R) {
cic_bitsSeen = 0;
const int32_t c1 = cic_i3 - cic_p1; cic_p1 = cic_i3;
const int32_t c2 = c1 - cic_p2; cic_p2 = c1;
const int32_t c3 = c2 - cic_p3; cic_p3 = c2;
int32_t s = (c3 - DC) >> SCALE_SHIFT;
s <<= PDM_POST_GAIN_SHIFT;
if (s > 32767) s = 32767;
if (s < -32768) s = -32768;
pcm[produced++] = (int16_t)s;
}
}
return produced;
}
#endif // PDM_CAPTURE_DMA
@@ -0,0 +1,46 @@
/*
* Copyright (C) EdgeTX
*
* Based on code named
* opentx - https://github.com/opentx/opentx
* th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x
*
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#pragma once
#include <stdint.h>
// Decimation factor: output rate = PDM_CLOCK_FREQ / PDM_PCM_DECIMATION.
// R=100, PDM_CLOCK=1.6 MHz → SRC_RATE=16000 Hz exactly (no resampler needed).
#define PDM_PCM_DECIMATION 100
// Post-CIC gain (left shift before int16 saturation; +6 dB per step).
// Default calibrated at 4. Override per target in hal.h before this header
// is reached. trimSilence() in pdm_wav_recorder.cpp scales its threshold
// from this same value, so changing it keeps trim in sync automatically.
#ifndef PDM_POST_GAIN_SHIFT
#define PDM_POST_GAIN_SHIFT 5
#endif
// Bring up SAI clock + TIM15 + DMA capture and reset filter state.
// Idempotent. Call when entering a context that needs the mic.
void pdmStart();
// Stop the DMA pacer, disable SAI block (the PDM clock to the mic stops).
void pdmStop();
bool pdmUpdateSoundLevel();
uint8_t pdmGetSoundLevel();
bool pdmCapture();
uint32_t pdmConvertToPCM(int16_t* pcm, uint32_t max);