ADC까지, 신호 확인 필요(반사 신호 약함 or 뒤쪽을 찍고 있음)
This commit is contained in:
@@ -0,0 +1,145 @@
|
||||
/*******************************************************************************
|
||||
* @file echo_adc.c
|
||||
* @brief ADC121S051 echo capture driver using nrfx SPIM3
|
||||
******************************************************************************/
|
||||
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/devicetree.h>
|
||||
#include <zephyr/sys/util.h>
|
||||
#include <nrfx_spim.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "echo_adc.h"
|
||||
#include "debug_print.h"
|
||||
|
||||
#define ECHO_SCLK_NODE DT_NODELABEL(echo_sclk)
|
||||
#define ECHO_MISO_NODE DT_NODELABEL(echo_miso)
|
||||
#define ECHO_CS_NODE DT_NODELABEL(echo_cs)
|
||||
|
||||
static const struct gpio_dt_spec echo_sclk = GPIO_DT_SPEC_GET(ECHO_SCLK_NODE, gpios);
|
||||
static const struct gpio_dt_spec echo_miso = GPIO_DT_SPEC_GET(ECHO_MISO_NODE, gpios);
|
||||
static const struct gpio_dt_spec echo_cs = GPIO_DT_SPEC_GET(ECHO_CS_NODE, gpios);
|
||||
|
||||
/*
|
||||
* NRFX_SPIM_INSTANCE() takes the peripheral register symbol (for example
|
||||
* NRF_SPIM3), not a numeric instance index. Passing 3 makes p_reg invalid
|
||||
* and causes a fault inside nrfx_spim_init().
|
||||
*/
|
||||
static nrfx_spim_t echo_spim = NRFX_SPIM_INSTANCE(NRF_SPIM3);
|
||||
static bool echo_adc_initialized;
|
||||
static uint8_t raw_capture[ECHO_ADC_MAX_SAMPLES * 2];
|
||||
|
||||
static uint32_t gpio_abs_pin(const struct gpio_dt_spec *spec)
|
||||
{
|
||||
uint32_t port = (spec->port == DEVICE_DT_GET(DT_NODELABEL(gpio1))) ? 1U : 0U;
|
||||
return NRF_GPIO_PIN_MAP(port, spec->pin);
|
||||
}
|
||||
|
||||
static int echo_adc_transfer_rx(uint8_t *buf, size_t len)
|
||||
{
|
||||
nrfx_spim_xfer_desc_t xfer = NRFX_SPIM_XFER_RX(buf, len);
|
||||
int err = nrfx_spim_xfer(&echo_spim, &xfer, 0);
|
||||
|
||||
if (err != 0)
|
||||
{
|
||||
DBG_PRINTF("[ECHO] xfer fail err=%d\r\n", err);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int echo_adc_init(void)
|
||||
{
|
||||
if (echo_adc_initialized || nrfx_spim_init_check(&echo_spim))
|
||||
{
|
||||
echo_adc_initialized = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
const struct gpio_dt_spec *pins[] = { &echo_sclk, &echo_miso, &echo_cs };
|
||||
|
||||
for (size_t i = 0; i < ARRAY_SIZE(pins); i++)
|
||||
{
|
||||
if (!device_is_ready(pins[i]->port))
|
||||
{
|
||||
DBG_PRINTF("[ECHO] GPIO device not ready (%d)\r\n", (int)i);
|
||||
return -ENODEV;
|
||||
}
|
||||
}
|
||||
|
||||
nrfx_spim_config_t config = NRFX_SPIM_DEFAULT_CONFIG(
|
||||
gpio_abs_pin(&echo_sclk),
|
||||
NRF_SPIM_PIN_NOT_CONNECTED,
|
||||
gpio_abs_pin(&echo_miso),
|
||||
gpio_abs_pin(&echo_cs));
|
||||
|
||||
config.frequency = NRFX_MHZ_TO_HZ(16);
|
||||
config.mode = NRF_SPIM_MODE_3;
|
||||
config.bit_order = NRF_SPIM_BIT_ORDER_MSB_FIRST;
|
||||
config.orc = 0x00;
|
||||
config.miso_pull = NRF_GPIO_PIN_NOPULL;
|
||||
config.ss_active_high = false;
|
||||
|
||||
nrfx_err_t err = nrfx_spim_init(&echo_spim, &config, NULL, NULL);
|
||||
if (err != NRFX_SUCCESS)
|
||||
{
|
||||
DBG_PRINTF("[ECHO] init fail err=%d\r\n", err);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
echo_adc_initialized = true;
|
||||
DBG_PRINTF("[ECHO] SPIM3 init OK\r\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int echo_adc_wake(void)
|
||||
{
|
||||
int err = echo_adc_init();
|
||||
if (err)
|
||||
{
|
||||
return err;
|
||||
}
|
||||
|
||||
uint8_t discard[2];
|
||||
return echo_adc_transfer_rx(discard, sizeof(discard));
|
||||
}
|
||||
|
||||
int echo_adc_capture(uint16_t *samples, uint16_t num_samples)
|
||||
{
|
||||
if (samples == NULL)
|
||||
{
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
if ((num_samples == 0U) || (num_samples > ECHO_ADC_MAX_SAMPLES))
|
||||
{
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
int err = echo_adc_init();
|
||||
if (err)
|
||||
{
|
||||
return err;
|
||||
}
|
||||
|
||||
for (uint16_t i = 0; i < num_samples; i++)
|
||||
{
|
||||
/*
|
||||
* Keep the framing that matched the old firmware behavior:
|
||||
* one 16-clock / 2-byte SPI frame per sample.
|
||||
*/
|
||||
err = echo_adc_transfer_rx(&raw_capture[i * 2U], 2U);
|
||||
if (err)
|
||||
{
|
||||
return err;
|
||||
}
|
||||
|
||||
uint16_t raw16 = ((uint16_t)raw_capture[i * 2U] << 8) |
|
||||
(uint16_t)raw_capture[i * 2U + 1U];
|
||||
samples[i] = (raw16 >> 1) & 0x0FFF;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,17 @@
|
||||
/*******************************************************************************
|
||||
* @file echo_adc.h
|
||||
* @brief ADC121S051 echo capture driver
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef ECHO_ADC_H__
|
||||
#define ECHO_ADC_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define ECHO_ADC_MAX_SAMPLES 100
|
||||
|
||||
int echo_adc_init(void);
|
||||
int echo_adc_wake(void);
|
||||
int echo_adc_capture(uint16_t *samples, uint16_t num_samples);
|
||||
|
||||
#endif /* ECHO_ADC_H__ */
|
||||
@@ -0,0 +1,219 @@
|
||||
/*******************************************************************************
|
||||
* @file piezo.c
|
||||
* @brief Piezo TX burst, mux selection, and power control
|
||||
******************************************************************************/
|
||||
|
||||
#include <zephyr/kernel.h>
|
||||
#include <zephyr/drivers/gpio.h>
|
||||
#include <zephyr/devicetree.h>
|
||||
#include <zephyr/sys/util.h>
|
||||
#include <hal/nrf_gpio.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "piezo.h"
|
||||
#include "debug_print.h"
|
||||
|
||||
#define PIEZO_PWR_NODE DT_NODELABEL(piezo_pwr)
|
||||
#define PIEZO_PE_NODE DT_NODELABEL(piezo_pe)
|
||||
#define PIEZO_P_OUT_NODE DT_NODELABEL(piezo_p_out)
|
||||
#define PIEZO_N_OUT_NODE DT_NODELABEL(piezo_n_out)
|
||||
#define PIEZO_DMP_NODE DT_NODELABEL(piezo_dmp)
|
||||
#define MUX_EN_A_NODE DT_NODELABEL(mux_en_a)
|
||||
#define MUX_EN_B_NODE DT_NODELABEL(mux_en_b)
|
||||
#define MUX_SEL0_NODE DT_NODELABEL(mux_sel0)
|
||||
#define MUX_SEL1_NODE DT_NODELABEL(mux_sel1)
|
||||
|
||||
static const struct gpio_dt_spec piezo_pwr = GPIO_DT_SPEC_GET(PIEZO_PWR_NODE, gpios);
|
||||
static const struct gpio_dt_spec piezo_pe = GPIO_DT_SPEC_GET(PIEZO_PE_NODE, gpios);
|
||||
static const struct gpio_dt_spec piezo_p_out = GPIO_DT_SPEC_GET(PIEZO_P_OUT_NODE, gpios);
|
||||
static const struct gpio_dt_spec piezo_n_out = GPIO_DT_SPEC_GET(PIEZO_N_OUT_NODE, gpios);
|
||||
static const struct gpio_dt_spec piezo_dmp = GPIO_DT_SPEC_GET(PIEZO_DMP_NODE, gpios);
|
||||
static const struct gpio_dt_spec mux_en_a = GPIO_DT_SPEC_GET(MUX_EN_A_NODE, gpios);
|
||||
static const struct gpio_dt_spec mux_en_b = GPIO_DT_SPEC_GET(MUX_EN_B_NODE, gpios);
|
||||
static const struct gpio_dt_spec mux_sel0 = GPIO_DT_SPEC_GET(MUX_SEL0_NODE, gpios);
|
||||
static const struct gpio_dt_spec mux_sel1 = GPIO_DT_SPEC_GET(MUX_SEL1_NODE, gpios);
|
||||
|
||||
struct mux_state {
|
||||
uint8_t en_a;
|
||||
uint8_t en_b;
|
||||
uint8_t sel0;
|
||||
uint8_t sel1;
|
||||
};
|
||||
|
||||
static const struct mux_state mux_map[PIEZO_NUM_CHANNELS] = {
|
||||
{1, 0, 0, 0},
|
||||
{1, 0, 1, 0},
|
||||
{1, 0, 0, 1},
|
||||
{1, 0, 1, 1},
|
||||
{0, 1, 0, 0},
|
||||
{0, 1, 1, 0},
|
||||
};
|
||||
|
||||
static bool piezo_initialized;
|
||||
static uint32_t piezo_pe_abs;
|
||||
static uint32_t piezo_p_out_abs;
|
||||
static uint32_t piezo_n_out_abs;
|
||||
static uint32_t piezo_dmp_abs;
|
||||
static uint32_t piezo_pe_mask;
|
||||
static uint32_t piezo_p_out_mask;
|
||||
static uint32_t piezo_n_out_mask;
|
||||
static uint32_t piezo_dmp_mask;
|
||||
|
||||
static uint32_t gpio_abs_pin(const struct gpio_dt_spec *spec)
|
||||
{
|
||||
uint32_t port = (spec->port == DEVICE_DT_GET(DT_NODELABEL(gpio1))) ? 1U : 0U;
|
||||
return NRF_GPIO_PIN_MAP(port, spec->pin);
|
||||
}
|
||||
|
||||
static inline void burst_nop(void)
|
||||
{
|
||||
__asm__ volatile("nop");
|
||||
}
|
||||
|
||||
static inline void nop_delay_9(void)
|
||||
{
|
||||
burst_nop(); burst_nop(); burst_nop();
|
||||
burst_nop(); burst_nop(); burst_nop();
|
||||
burst_nop(); burst_nop(); burst_nop();
|
||||
}
|
||||
|
||||
static inline void nop_delay_14(void)
|
||||
{
|
||||
burst_nop(); burst_nop(); burst_nop(); burst_nop();
|
||||
burst_nop(); burst_nop(); burst_nop(); burst_nop();
|
||||
burst_nop(); burst_nop(); burst_nop(); burst_nop();
|
||||
burst_nop(); burst_nop();
|
||||
}
|
||||
|
||||
static inline void nop_delay_32(void)
|
||||
{
|
||||
for (int i = 0; i < 32; i++) {
|
||||
burst_nop();
|
||||
}
|
||||
}
|
||||
|
||||
static void piezo_drive_idle(void)
|
||||
{
|
||||
nrf_gpio_pin_clear(piezo_pe_abs);
|
||||
nrf_gpio_pin_clear(piezo_p_out_abs);
|
||||
nrf_gpio_pin_clear(piezo_n_out_abs);
|
||||
nrf_gpio_pin_clear(piezo_dmp_abs);
|
||||
|
||||
gpio_pin_set_dt(&mux_en_a, 0);
|
||||
gpio_pin_set_dt(&mux_en_b, 0);
|
||||
gpio_pin_set_dt(&mux_sel0, 0);
|
||||
gpio_pin_set_dt(&mux_sel1, 0);
|
||||
}
|
||||
|
||||
int piezo_init(void)
|
||||
{
|
||||
if (piezo_initialized) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const struct gpio_dt_spec *pins[] = {
|
||||
&piezo_pwr, &piezo_pe, &piezo_p_out, &piezo_n_out,
|
||||
&piezo_dmp, &mux_en_a, &mux_en_b, &mux_sel0, &mux_sel1
|
||||
};
|
||||
|
||||
for (size_t i = 0; i < ARRAY_SIZE(pins); i++) {
|
||||
if (!device_is_ready(pins[i]->port)) {
|
||||
DBG_PRINTF("[PIEZO] GPIO device not ready (%d)\r\n", (int)i);
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
int err = gpio_pin_configure_dt(pins[i], GPIO_OUTPUT_INACTIVE);
|
||||
if (err) {
|
||||
DBG_PRINTF("[PIEZO] GPIO init fail idx=%d err=%d\r\n", (int)i, err);
|
||||
return err;
|
||||
}
|
||||
}
|
||||
|
||||
piezo_pe_abs = gpio_abs_pin(&piezo_pe);
|
||||
piezo_p_out_abs = gpio_abs_pin(&piezo_p_out);
|
||||
piezo_n_out_abs = gpio_abs_pin(&piezo_n_out);
|
||||
piezo_dmp_abs = gpio_abs_pin(&piezo_dmp);
|
||||
piezo_pe_mask = BIT(piezo_pe.pin);
|
||||
piezo_p_out_mask = BIT(piezo_p_out.pin);
|
||||
piezo_n_out_mask = BIT(piezo_n_out.pin);
|
||||
piezo_dmp_mask = BIT(piezo_dmp.pin);
|
||||
|
||||
piezo_drive_idle();
|
||||
piezo_initialized = true;
|
||||
DBG_PRINTF("[PIEZO] Init OK\r\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
void piezo_power_on(void)
|
||||
{
|
||||
if (!piezo_initialized && piezo_init() != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
gpio_pin_set_dt(&piezo_pwr, 1);
|
||||
}
|
||||
|
||||
void piezo_power_off(void)
|
||||
{
|
||||
if (!piezo_initialized && piezo_init() != 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
piezo_drive_idle();
|
||||
gpio_pin_set_dt(&piezo_pwr, 0);
|
||||
}
|
||||
|
||||
int piezo_select_channel(uint8_t channel)
|
||||
{
|
||||
if (!piezo_initialized) {
|
||||
int err = piezo_init();
|
||||
if (err) {
|
||||
return err;
|
||||
}
|
||||
}
|
||||
|
||||
if (channel >= PIEZO_NUM_CHANNELS) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
gpio_pin_set_dt(&mux_en_a, mux_map[channel].en_a);
|
||||
gpio_pin_set_dt(&mux_en_b, mux_map[channel].en_b);
|
||||
gpio_pin_set_dt(&mux_sel0, mux_map[channel].sel0);
|
||||
gpio_pin_set_dt(&mux_sel1, mux_map[channel].sel1);
|
||||
|
||||
k_busy_wait(PIEZO_MUX_SETTLE_US);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void piezo_burst_sw(uint8_t cycles)
|
||||
{
|
||||
if (!piezo_initialized || cycles == 0U) {
|
||||
return;
|
||||
}
|
||||
|
||||
unsigned int key = irq_lock();
|
||||
uint32_t saved_p1_out = NRF_P1->OUT;
|
||||
uint32_t p1_ctrl_mask = piezo_p_out_mask | piezo_n_out_mask | piezo_dmp_mask;
|
||||
uint32_t p1_all_low = saved_p1_out & ~p1_ctrl_mask;
|
||||
uint32_t p1_p_high_n_low = p1_all_low | piezo_p_out_mask;
|
||||
uint32_t p1_p_low_n_high = p1_all_low | piezo_n_out_mask;
|
||||
uint32_t p1_dmp_high = p1_all_low | piezo_dmp_mask;
|
||||
|
||||
NRF_P0->OUTCLR = piezo_pe_mask;
|
||||
NRF_P1->OUT = p1_all_low;
|
||||
NRF_P0->OUTSET = piezo_pe_mask;
|
||||
|
||||
for (uint8_t i = 0; i < cycles; i++) {
|
||||
NRF_P1->OUT = p1_p_high_n_low;
|
||||
nop_delay_14();
|
||||
|
||||
NRF_P1->OUT = p1_p_low_n_high;
|
||||
nop_delay_9();
|
||||
}
|
||||
|
||||
NRF_P1->OUT = p1_dmp_high;
|
||||
nop_delay_32();
|
||||
NRF_P1->OUT = p1_all_low;
|
||||
NRF_P0->OUTCLR = piezo_pe_mask;
|
||||
irq_unlock(key);
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
/*******************************************************************************
|
||||
* @file piezo.h
|
||||
* @brief Piezo transmit and mux control driver
|
||||
******************************************************************************/
|
||||
|
||||
#ifndef PIEZO_H__
|
||||
#define PIEZO_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define PIEZO_NUM_CHANNELS 6
|
||||
#define PIEZO_SW_BURST_CYCLES 7
|
||||
#define PIEZO_MUX_SETTLE_US 1300
|
||||
#define PIEZO_POWER_STABILIZE_MS 3
|
||||
#define PIEZO_BURST_TO_ADC_DELAY_US 10
|
||||
|
||||
int piezo_init(void);
|
||||
void piezo_power_on(void);
|
||||
void piezo_power_off(void);
|
||||
int piezo_select_channel(uint8_t channel);
|
||||
void piezo_burst_sw(uint8_t cycles);
|
||||
|
||||
#endif /* PIEZO_H__ */
|
||||
@@ -57,13 +57,13 @@ static int16_t raw_to_cdeg(int16_t raw)
|
||||
int temp_init(void)
|
||||
{
|
||||
if (!adc_is_ready_dt(&temp_adc)) {
|
||||
DBG_PRINTF("[TEMP] FAIL — ADC device not ready\r\n");
|
||||
DBG_ERR("[TEMP] FAIL — ADC device not ready\r\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int err = adc_channel_setup_dt(&temp_adc);
|
||||
if (err) {
|
||||
DBG_PRINTF("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
|
||||
DBG_ERR("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
|
||||
return -2;
|
||||
}
|
||||
|
||||
@@ -79,7 +79,7 @@ int16_t temp_read_cdeg(void)
|
||||
/* 매번 채널 재설정 (배터리 ADC와 SAADC 공유) */
|
||||
int err = adc_channel_setup_dt(&temp_adc);
|
||||
if (err) {
|
||||
DBG_PRINTF("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
|
||||
DBG_ERR("[TEMP] FAIL — channel setup (err=%d)\r\n", err);
|
||||
return INT16_MIN;
|
||||
}
|
||||
|
||||
@@ -87,7 +87,7 @@ int16_t temp_read_cdeg(void)
|
||||
|
||||
err = adc_sequence_init_dt(&temp_adc, &seq);
|
||||
if (err) {
|
||||
DBG_PRINTF("[TEMP] FAIL — sequence init (err=%d)\r\n", err);
|
||||
DBG_ERR("[TEMP] FAIL — sequence init (err=%d)\r\n", err);
|
||||
return INT16_MIN;
|
||||
}
|
||||
|
||||
@@ -96,7 +96,7 @@ int16_t temp_read_cdeg(void)
|
||||
|
||||
err = adc_read_dt(&temp_adc, &seq);
|
||||
if (err) {
|
||||
DBG_PRINTF("[TEMP] FAIL — read (err=%d)\r\n", err);
|
||||
DBG_ERR("[TEMP] FAIL — read (err=%d)\r\n", err);
|
||||
return INT16_MIN;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user