/*
|
* Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and
|
* its subsidiaries and affiliates (collectly called MKSEMI).
|
*
|
* All rights reserved.
|
*
|
* Redistribution and use in source and binary forms, with or without
|
* modification, are permitted provided that the following conditions are met:
|
*
|
* 1. Redistributions of source code must retain the above copyright notice,
|
* this list of conditions and the following disclaimer.
|
*
|
* 2. Redistributions in binary form, except as embedded into an MKSEMI
|
* integrated circuit in a product or a software update for such product,
|
* must reproduce the above copyright notice, this list of conditions and
|
* the following disclaimer in the documentation and/or other materials
|
* provided with the distribution.
|
*
|
* 3. Neither the name of MKSEMI nor the names of its contributors may be used
|
* to endorse or promote products derived from this software without
|
* specific prior written permission.
|
*
|
* 4. This software, with or without modification, must only be used with a
|
* MKSEMI integrated circuit.
|
*
|
* 5. Any software provided in binary form under this license must not be
|
* reverse engineered, decompiled, modified and/or disassembled.
|
*
|
* THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED
|
* WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
|
* MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
* DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY
|
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
|
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
*/
|
|
#include "mk_uwb.h"
|
#include "mk_radar.h"
|
#include "mk_trace.h"
|
#include "mk_misc.h"
|
#include "mk_lsp.h"
|
|
#ifndef UWB_RADAR_1TNR_MODE
|
/* 1TnR radar mdoe: 0: Single, 1: Overlay */
|
#define UWB_RADAR_1TNR_MODE 0
|
#endif
|
|
#ifndef UWB_RADAR_RX_PORT_NUM
|
/* 1TnR mode RX port number */
|
#define UWB_RADAR_RX_PORT_NUM 1
|
#endif
|
|
#ifndef UWB_RADAR_STS_LENGTH
|
#define UWB_RADAR_STS_LENGTH 2
|
#endif
|
|
/* Overlay Length of Frames */
|
#define OVERLAY_FRAME_LENGTH 128
|
|
/* Cut off unwantted data at both ends */
|
#define CUT_OFF_LINES(n) ((n * 1024) / (OVERLAY_FRAME_LENGTH * 2))
|
|
/* Ignore n KB data at head */
|
#define HEAD_LEN (2)
|
|
/* Ignore n KB data at tail */
|
#define TAIL_LEN (2)
|
|
/* Snapshot size, x = STS segment length: 0, 1, 2 */
|
#define ACCUMULATE_ROWS(x) (DUMP_DATA_SIZE(x) / (OVERLAY_FRAME_LENGTH * 2))
|
|
#if UWB_RADAR_1TNR_MODE == 0
|
/* Accumulation times, depends on STS length and Frame length */
|
#define N_SNAPSHOTS (ACCUMULATE_ROWS(UWB_RADAR_STS_LENGTH))
|
#else
|
/* Accumulation times, depends on STS length, Frame length and RX port number */
|
#define N_SNAPSHOTS (ACCUMULATE_ROWS(UWB_RADAR_STS_LENGTH) / (1 << (UWB_RADAR_RX_PORT_NUM - 1)))
|
#endif
|
|
#if OVERLAY_FRAME_LENGTH == 256
|
#if (64 == N_SNAPSHOTS)
|
#elif (128 == N_SNAPSHOTS)
|
#elif (256 == N_SNAPSHOTS)
|
#else
|
#error "N_SNAPSHOTS is not supported"
|
#endif
|
#elif OVERLAY_FRAME_LENGTH == 128
|
#if (128 == N_SNAPSHOTS)
|
#elif (256 == N_SNAPSHOTS)
|
#elif (512 == N_SNAPSHOTS)
|
#else
|
#error "N_SNAPSHOTS is not supported"
|
#endif
|
#else
|
#error "Frame length is not supported"
|
#endif
|
|
/* Radar RX window, unit: us */
|
#define UWB_RADAR_RX_WINDOW (100)
|
/* Radar event prefetch time, unit: us */
|
#define UWB_RADAR_PREFETCH_TIME (100)
|
|
#if UWB_RADAR_1TNR_MODE == 1
|
#if UWB_RADAR_STS_LENGTH == 2
|
#define UWB_RADAR_RX_WINDOW2 64
|
#define UWB_RADAR_RX_WINDOW3 48
|
#else
|
#define UWB_RADAR_RX_WINDOW2 48
|
#define UWB_RADAR_RX_WINDOW3 40
|
#endif
|
#endif
|
|
/* 1TnR Frame interval for Overlay work mode, unit: us */
|
#define FRAME_INTERVAL 300
|
|
/* The factors influencing the peak index value are twofold:
|
* 1) the scheduled transmission time (The configuration of the low 2 bits of the 0x40000424 register)
|
* 2) the value of the discard (0x40002040 bit8~19)
|
*/
|
/* The low 2 bits of the 0x40000424 register are set to 0
|
* |----------------------------------------------------------------------------|
|
* | Discard Symbols(24/25/26/27/28/29) | 24 | 25 | 26 | 27 | 28 | 29 | |
|
* |----------------------------------------------------------------------------|
|
* | Pulse period 16ns (0 Delay Point) | 4 | 12 | 4 | 12 | 4 | 12 | |
|
* |----------------------------------------------------------------------------|
|
* |----------------------------------------------------------------------------|
|
* | Pulse period 32ns (0 Delay Point) | 20 | 28 | 4 | 12 | 20 | 28 | |
|
* |----------------------------------------------------------------------------|
|
* |----------------------------------------------------------------------------|
|
* | Pulse period 64ns (0 Delay Point) | 36 | 44 | 52 | 60 | 4 | 12 | |
|
* |----------------------------------------------------------------------------|
|
* |----------------------------------------------------------------------------|
|
* | Pulse period 128ns (0 Delay Point) | 100 | 108 | 116 | 124 | 4 | 12 | |
|
* |----------------------------------------------------------------------------|
|
* |----------------------------------------------------------------------------|
|
* | Pulse period 256ns (0 Delay Point) | 228 | 236 | 244 | 252 | 4 | 12 | |
|
* |----------------------------------------------------------------------------|
|
*/
|
|
#if UWB_RADAR_2G_MODE_EN
|
|
/* 1ns per sample, 24 ~ 27 */
|
#define DISCARD_SYMBOLS 25
|
|
/* pusle peak postion table */
|
static const uint8_t peak_idx_table[5][4] = {{4, 12, 4, 12}, {20, 28, 4, 12}, {36, 44, 52, 60}, {100, 108, 116, 124}, {228, 236, 244, 252}};
|
#else
|
|
/* 2ns per sample, 12 ~ 15 */
|
#define DISCARD_SYMBOLS 12
|
|
/* pusle peak postion table */
|
static const uint8_t peak_idx_table[5][4] = {{1, 5, 1, 5}, {0, 4, 8, 12}, {8, 12, 16, 20}, {8, 12, 16, 20}, {72, 76, 80, 84}};
|
#endif
|
|
/* 1TnR Rx port list */
|
static uint8_t rx_ant_id[] = {0, 1, 2, 4, 5, 6, 8, 9, 10};
|
|
#if UWB_RADAR_PULSE_PERIOD == 4 && OVERLAY_FRAME_LENGTH != 256
|
#error "Pulse period and Frame length are not match"
|
#endif
|
|
static struct UWB_RADAR_T default_radar_cfg = {
|
.channel_num = 9,
|
.rx_gain_level = 14,
|
.lna_gain_level = 5,
|
.pulse_period = UWB_RADAR_PULSE_PERIOD_64NS,
|
.bandwidth = 0x02,
|
.ant_id = UWB_RX_ANT_0,
|
.sts_len = UWB_RADAR_STS_SEGLEN64,
|
.tx_power_level = 46,
|
};
|
|
struct reg_tmp_stored
|
{
|
#define STORE_REG_COUNT_MAX 40
|
#define STORE_REG 0
|
#define STORE_VAL 1
|
uint32_t magic;
|
uint32_t reg_count;
|
uint32_t reg_store[STORE_REG_COUNT_MAX][2];
|
};
|
|
static struct reg_tmp_stored reg_save;
|
|
static uint32_t calibration_peak_idx = 0;
|
|
#if 1
|
static int16_t allones[N_SNAPSHOTS] __attribute__((aligned(4)));
|
#else
|
static int16_t allones[512] = {
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001, 0x0001,
|
};
|
#endif
|
|
/**
|
* \fn
|
* \brief calculate the average of complex input data
|
*
|
* \param [in] data_in Input data
|
* \param [in] data_size Half length of input data
|
* \param [out] data_out Output data
|
*/
|
|
static void average_data_calc_complex_int(const int32_t *data_in, uint32_t data_size, float *data_out)
|
{
|
int dout_i = 0;
|
int dout_q = 0;
|
|
for (uint32_t index = 0; index < data_size; index++)
|
{
|
dout_i += data_in[2 * index];
|
dout_q += data_in[2 * index + 1];
|
}
|
data_out[0] = (float)dout_i / (float)data_size;
|
data_out[1] = (float)dout_q / (float)data_size;
|
}
|
|
#if UWB_RADAR_1TNR_MODE == 1
|
/*
|
* This module will do the following functions:
|
* 1) Take the I/Q input and remove DC component
|
* 2) accumulate the pulses
|
*/
|
static void radar_output_1tnr_one_pulse(const int32_t *data_input, uint32_t data_in_len, float *data_output, uint32_t pulse_period_points)
|
{
|
/* Temporary storage of accumulated monopulse results
|
* The supported pulse length 16/32/64/128/256 samples */
|
int32_t period_samples[OUT_FRAME_LEN];
|
float period_samples_out[OUT_FRAME_LEN];
|
uint32_t pulse_period_num = data_in_len / pulse_period_points;
|
|
int32_t data_i;
|
int32_t data_q;
|
|
float data_avg[2];
|
|
const int32_t *data_in;
|
float *data_out;
|
|
// LOG_INFO(TRACE_MODULE_APP, "Pulse period points %d period number %d\r\n", pulse_period_points, pulse_period_num);
|
|
for (uint8_t n = 0; n < UWB_RADAR_RX_PORT_NUM; n++)
|
{
|
data_in = &data_input[n * data_in_len * 2];
|
data_out = &data_output[n * pulse_period_points * 2];
|
|
// Step1 - accumulate the pulses
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
data_i = 0;
|
data_q = 0;
|
|
for (uint32_t j = 0; j < pulse_period_num; j++)
|
{
|
data_i += data_in[i * 2 + pulse_period_points * 2 * j];
|
data_q += data_in[i * 2 + pulse_period_points * 2 * j + 1];
|
}
|
|
period_samples[i * 2] = data_i;
|
period_samples[i * 2 + 1] = data_q;
|
}
|
|
/* Step2 - calculate the DC component */
|
average_data_calc_complex_int(period_samples, pulse_period_points, data_avg);
|
|
// LOG_INFO(TRACE_MODULE_APP, "DC %f %f\r\n", data_avg[0], data_avg[1]);
|
|
/* Step3 - remove DC component, Search peak index */
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
period_samples_out[i * 2] = (float)period_samples[i * 2] - data_avg[0];
|
period_samples_out[i * 2 + 1] = (float)period_samples[i * 2 + 1] - data_avg[1];
|
}
|
|
#if 0
|
float tmp_val;
|
float peak_val = 0.0;
|
if (!calibration_peak_idx)
|
{
|
/* Step3 Search peak index : I^2 + Q^2*/
|
lsp_mult_f32(period_samples_out, period_samples_out, data_out, pulse_period_points * 2);
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
tmp_val = data_out[i * 2] + data_out[i * 2 + 1];
|
if (tmp_val > peak_val)
|
{
|
peak_val = tmp_val;
|
calibration_peak_idx = (uint16_t)i;
|
}
|
}
|
|
calibration_peak_idx = calibration_peak_idx - ((calibration_peak_idx - 4) % 8);
|
LOG_INFO(TRACE_MODULE_APP, "ANT%d Raw max index %d\n", n, calibration_peak_idx);
|
}
|
#endif
|
|
/* Step4 - Peak translation, it takes 22us */
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
uint32_t idx = ((pulse_period_points - calibration_peak_idx + i) > (pulse_period_points - 1)) ? (i - calibration_peak_idx)
|
: (pulse_period_points - calibration_peak_idx + i);
|
|
data_out[idx * 2] = period_samples_out[i * 2];
|
data_out[idx * 2 + 1] = period_samples_out[i * 2 + 1];
|
}
|
}
|
}
|
|
/*
|
* 128 samples are a group, used for pulse accumulation
|
* The 64K I/Q data in the upper half is from PORT 1,
|
* and the 64K I/Q data in the lower half is from PORT 0.
|
*/
|
static void radar_1tnr_raw_data_preprocess(enum UWB_RADAR_STS_SEGLEN_T sts_seg_len, uint32_t *data_in, int32_t *data_out)
|
{
|
// 64k I/Q data
|
int16_t samp_array_d1[512]; // 512 samples (I and Q) for first data in the word
|
int16_t samp_array_d2[512]; // 512 samples (I and Q) for second data in the word
|
uint32_t data_read = 0;
|
uint32_t offset = 0;
|
|
uint32_t acc_columns = (uint32_t)ACCUMULATE_ROWS(sts_seg_len) >> (UWB_RADAR_RX_PORT_NUM - 1);
|
|
uint32_t *mem_data;
|
int32_t *acc_out;
|
|
/*
|
Port 2 |Port 1 |Port 0 |
|
1 5.....|1 5.....|1 5.............| 32KB
|
2 6.....|2 6.....|2 6.............| 32KB
|
3 7.....|3 7.....|3 7.............| 32KB
|
4 8.....|4 8.....|4 8.............| 32KB
|
*/
|
// 8KW each block
|
uint32_t seg_in_blk = 1024 * 8;
|
if ((sts_seg_len == UWB_RADAR_STS_SEGLEN64) || (sts_seg_len == UWB_RADAR_STS_SEGLEN32))
|
{
|
seg_in_blk = seg_in_blk >> (UWB_RADAR_RX_PORT_NUM - 1);
|
if (seg_in_blk < 1024 * 2)
|
{
|
return;
|
}
|
}
|
else
|
{
|
return;
|
}
|
|
for (uint8_t i = 0; i < UWB_RADAR_RX_PORT_NUM; i++)
|
{
|
mem_data = &data_in[seg_in_blk * (UWB_RADAR_RX_PORT_NUM - 1 - i)];
|
acc_out = &data_out[i * OVERLAY_FRAME_LENGTH * 2];
|
|
/* Outer loop for the 4 banks, each bank will have 32 kbytes or 8 kwords */
|
for (uint32_t blocks = 0; blocks < BANK_NUM; blocks++)
|
{
|
offset = blocks * 1024 * 8;
|
|
/* Inner loop for words in that bank, each word has 2 samples (IQ) */
|
/* 1 snapshot will be 16 words, so accumulate every 16 words */
|
for (uint32_t columns = 0; columns < OVERLAY_FRAME_LENGTH / BANK_NUM / 2; columns++)
|
{
|
/* Remove head 4KB and tail 2KB */
|
for (uint32_t rows = CUT_OFF_LINES(HEAD_LEN); rows < (acc_columns - CUT_OFF_LINES(TAIL_LEN)); rows++)
|
{
|
data_read = mem_data[offset + rows * (OVERLAY_FRAME_LENGTH / BANK_NUM / 2) + columns];
|
|
// Extract two samples from this word
|
samp_array_d1[rows] = (int16_t)((data_read) & (0xffff));
|
samp_array_d2[rows] = (int16_t)((data_read >> (16)) & (0xffff));
|
}
|
|
/* Accumulate across snapshots , complex so will output 2 values */
|
lsp_cmplx_inner_product_int8((const int8_t *)&samp_array_d1[CUT_OFF_LINES(HEAD_LEN)], (const int8_t *)allones,
|
(int32_t *)(acc_out + (4 * (BANK_NUM * columns + blocks))),
|
(acc_columns - CUT_OFF_LINES(HEAD_LEN) - CUT_OFF_LINES(TAIL_LEN)), 0);
|
lsp_cmplx_inner_product_int8((const int8_t *)&samp_array_d2[CUT_OFF_LINES(HEAD_LEN)], (const int8_t *)allones,
|
(int32_t *)(acc_out + (4 * (BANK_NUM * columns + blocks) + 2)),
|
(acc_columns - CUT_OFF_LINES(HEAD_LEN) - CUT_OFF_LINES(TAIL_LEN)), 0);
|
}
|
}
|
}
|
}
|
|
#else
|
|
/*
|
* This module will do the following functions:
|
* 1) Take the I/Q input and remove DC component
|
* 2) accumulate the pulses
|
*/
|
static void radar_output_one_pulse(const int32_t *data_in, uint32_t data_in_len, float *data_out, uint32_t pulse_period_points)
|
{
|
/* Temporary storage of accumulated monopulse results
|
* The supported pulse length 16/32/64/128/256 samples */
|
int32_t period_samples[OUT_FRAME_LEN];
|
float period_samples_out[OUT_FRAME_LEN];
|
uint32_t pulse_period_num = data_in_len / pulse_period_points;
|
|
int32_t data_i;
|
int32_t data_q;
|
|
float data_avg[2];
|
|
// LOG_INFO(TRACE_MODULE_APP, "Pulse period points %d period number %d\r\n", pulse_period_points, pulse_period_num);
|
|
// Step1 - accumulate the pulses
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
data_i = 0;
|
data_q = 0;
|
|
for (uint32_t j = 0; j < pulse_period_num; j++)
|
{
|
data_i += data_in[i * 2 + pulse_period_points * 2 * j];
|
data_q += data_in[i * 2 + pulse_period_points * 2 * j + 1];
|
}
|
|
period_samples[i * 2] = data_i;
|
period_samples[i * 2 + 1] = data_q;
|
}
|
|
/* Step2 - calculate the DC component */
|
average_data_calc_complex_int(period_samples, pulse_period_points, data_avg);
|
|
// LOG_INFO(TRACE_MODULE_APP, "DC %f %f\r\n", data_avg[0], data_avg[1]);
|
|
/* Step3 - remove DC component, Search peak index */
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
period_samples_out[i * 2] = (float)period_samples[i * 2] - data_avg[0];
|
period_samples_out[i * 2 + 1] = (float)period_samples[i * 2 + 1] - data_avg[1];
|
}
|
|
#if 0
|
float tmp_val;
|
float peak_val = 0.0;
|
if (!calibration_peak_idx)
|
{
|
/* Step3 - Search peak index : I^2 + Q^2*/
|
lsp_mult_f32(period_samples_out, period_samples_out, data_out, pulse_period_points * 2);
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
tmp_val = data_out[i * 2] + data_out[i * 2 + 1];
|
if (tmp_val > peak_val)
|
{
|
peak_val = tmp_val;
|
calibration_peak_idx = i;
|
}
|
}
|
|
LOG_INFO(TRACE_MODULE_APP, "Raw max index %d\r\n", calibration_peak_idx);
|
}
|
#endif
|
|
/* Step4 - Peak translation, it takes 22us */
|
for (uint32_t i = 0; i < pulse_period_points; i++)
|
{
|
uint32_t idx = ((pulse_period_points - calibration_peak_idx + i) > (pulse_period_points - 1)) ? (i - calibration_peak_idx)
|
: (pulse_period_points - calibration_peak_idx + i);
|
|
data_out[idx * 2] = period_samples_out[i * 2];
|
data_out[idx * 2 + 1] = period_samples_out[i * 2 + 1];
|
}
|
}
|
|
static void radar_raw_data_preprocess(enum UWB_RADAR_STS_SEGLEN_T sts_seg_len, uint32_t *mem_data, int32_t *accum_out)
|
{
|
int16_t samp_array_d1[512]; // 512 samples (I and Q) for first data in the word
|
int16_t samp_array_d2[512]; // 512 samples (I and Q) for second data in the word
|
uint32_t data_read = 0;
|
uint32_t offset = 0;
|
uint32_t acc_columns = (uint32_t)ACCUMULATE_ROWS(sts_seg_len);
|
|
/* Outer loop for the 4 banks, each bank will have 32 kbytes or 8 kwords */
|
for (uint32_t blocks = 0; blocks < BANK_NUM; blocks++)
|
{
|
offset = blocks * 1024 * 8;
|
|
/* Inner loop for words in that bank, each word has 2 samples (IQ) */
|
/* 1 snapshot will be (OVERLAY_FRAME_LENGTH / BANK_NUM / 2) words, so accumulate every (OVERLAY_FRAME_LENGTH / BANK_NUM / 2) words */
|
for (uint32_t columns = 0; columns < OVERLAY_FRAME_LENGTH / BANK_NUM / 2; columns++)
|
{
|
/* Discard the first 4KB data and the end 2KB data */
|
for (uint32_t rows = CUT_OFF_LINES(HEAD_LEN); rows < acc_columns - CUT_OFF_LINES(TAIL_LEN); rows++)
|
{
|
data_read = mem_data[offset + rows * (OVERLAY_FRAME_LENGTH / BANK_NUM / 2) + columns];
|
|
// Extract two samples from this word
|
samp_array_d1[rows] = (int16_t)((data_read) & (0xffff));
|
samp_array_d2[rows] = (int16_t)((data_read >> (16)) & (0xffff));
|
}
|
|
/* Accumulate across up to 512 snapshots , complex so will output 2 values */
|
lsp_cmplx_inner_product_int8((const int8_t *)&samp_array_d1[CUT_OFF_LINES(HEAD_LEN)], (const int8_t *)allones,
|
(int32_t *)((uint32_t)accum_out + (16 * (BANK_NUM * columns + blocks))),
|
acc_columns - CUT_OFF_LINES(HEAD_LEN) - CUT_OFF_LINES(TAIL_LEN), 0);
|
lsp_cmplx_inner_product_int8((const int8_t *)&samp_array_d2[CUT_OFF_LINES(HEAD_LEN)], (const int8_t *)allones,
|
(int32_t *)((uint32_t)accum_out + (16 * (BANK_NUM * columns + blocks) + 8)),
|
acc_columns - CUT_OFF_LINES(HEAD_LEN) - CUT_OFF_LINES(TAIL_LEN), 0);
|
}
|
}
|
}
|
#endif
|
|
void radar_data_process(uint32_t *raw_data_addr, float *data_out, uint16_t *data_len)
|
{
|
// ASSERT(raw_data_addr != NULL, "raw data address invalid");
|
// ASSERT(data_out != NULL, "data output address invalid");
|
// ASSERT(data_len != NULL, "data length address invalid");
|
#if UWB_RADAR_1TNR_MODE == 1
|
int32_t accum_out[UWB_RADAR_RX_PORT_NUM * OVERLAY_FRAME_LENGTH * 2];
|
|
radar_1tnr_raw_data_preprocess(default_radar_cfg.sts_len, raw_data_addr, accum_out);
|
|
radar_output_1tnr_one_pulse(accum_out, OVERLAY_FRAME_LENGTH, data_out, PULSE_PERIOD_POINTS(default_radar_cfg.pulse_period));
|
|
*data_len = (uint16_t)(PULSE_PERIOD_POINTS(default_radar_cfg.pulse_period) * 2 * 4 * UWB_RADAR_RX_PORT_NUM);
|
#else
|
int32_t accum_out[OVERLAY_FRAME_LENGTH * 2];
|
|
radar_raw_data_preprocess(default_radar_cfg.sts_len, raw_data_addr, accum_out);
|
|
radar_output_one_pulse(accum_out, OVERLAY_FRAME_LENGTH, data_out, PULSE_PERIOD_POINTS(default_radar_cfg.pulse_period));
|
|
*data_len = (uint16_t)(PULSE_PERIOD_POINTS(default_radar_cfg.pulse_period) * 2 * 4);
|
#endif
|
}
|
|
void radar_config(struct UWB_RADAR_T *radar_cfg)
|
{
|
ASSERT(radar_cfg != NULL, "radar_cfg address invalid");
|
|
default_radar_cfg.request = radar_cfg->request;
|
default_radar_cfg.ant_id = radar_cfg->ant_id;
|
default_radar_cfg.bandwidth = radar_cfg->bandwidth;
|
default_radar_cfg.channel_num = radar_cfg->channel_num;
|
default_radar_cfg.lna_gain_level = radar_cfg->lna_gain_level;
|
default_radar_cfg.rx_gain_level = radar_cfg->rx_gain_level;
|
default_radar_cfg.tx_power_level = radar_cfg->tx_power_level;
|
default_radar_cfg.sts_len = radar_cfg->sts_len;
|
default_radar_cfg.pulse_period = radar_cfg->pulse_period;
|
default_radar_cfg.ranging_tx_power_level = radar_cfg->ranging_tx_power_level;
|
|
#if UWB_RADAR_2G_MODE_EN
|
calibration_peak_idx = peak_idx_table[default_radar_cfg.pulse_period][DISCARD_SYMBOLS - 24];
|
#else
|
calibration_peak_idx = peak_idx_table[default_radar_cfg.pulse_period][DISCARD_SYMBOLS - 12];
|
#endif
|
// LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "Raw max index %d\n", calibration_peak_idx);
|
|
// 1T1R mode, the ant id can be configured from host
|
rx_ant_id[0] = default_radar_cfg.ant_id;
|
|
// Initialize
|
for (int kk = 0; kk < N_SNAPSHOTS; kk++)
|
{
|
allones[kk] = 0x0001;
|
}
|
LOG_INFO(TRACE_MODULE_APP, "STS len %u Frame len %u N_SNAPSHOTS %u\r\n", 16 << default_radar_cfg.sts_len, OVERLAY_FRAME_LENGTH, N_SNAPSHOTS);
|
}
|
|
int radar_enable(void)
|
{
|
uint32_t val;
|
uint32_t reg;
|
uint32_t sts_key;
|
|
switch (default_radar_cfg.pulse_period)
|
{
|
case UWB_RADAR_PULSE_PERIOD_16NS:
|
sts_key = 0x00000000;
|
break;
|
|
case UWB_RADAR_PULSE_PERIOD_32NS:
|
sts_key = 0x55555555;
|
break;
|
|
case UWB_RADAR_PULSE_PERIOD_64NS:
|
sts_key = 0xEEEEEEEE;
|
break;
|
|
case UWB_RADAR_PULSE_PERIOD_128NS:
|
sts_key = 0xFEFEFEFE;
|
break;
|
|
case UWB_RADAR_PULSE_PERIOD_256NS:
|
sts_key = 0xFFFEFFFE;
|
break;
|
}
|
|
/**************************************SYS RADIO REG*********************************************/
|
reg = 0x40000404;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count = 1;
|
|
val = val & ~0xF0000U;
|
#if UWB_RADAR_2G_MODE_EN
|
// Set LPF_BW 500M | RX ADC sample rate 2Gs/s | pulse width 0:2ns(500M), 1:0.92ns(1G), 2:0.75ns(1.3G)
|
val |= ((1U << 18) | ((default_radar_cfg.bandwidth & 0x3U) << 16));
|
#else
|
// Set LPF_BW 250M | RX ADC sample rate 1Gs/s | pulse width 0:2ns(500M), 1:0.92ns(1G), 2:0.75ns(1.3G)
|
val |= ((1U << 19) | ((default_radar_cfg.bandwidth & 0x3U) << 16));
|
#endif
|
REG_WRITE(reg, val);
|
|
// LO calibrate
|
REG_WRITE(0x40006000, 0x80000400);
|
|
while ((REG_READ(0x40006004) & 0x00000008) == 0)
|
{
|
}
|
|
reg = 0x40000628;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Set rx antenna
|
REG_WRITE(0x40000628, (1 << 6) | rx_ant_code[default_radar_cfg.ant_id]);
|
|
/*****************MAC config********************/
|
reg = 0x5000A004;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// The loopback function must be enabled in MK8000 radar mode
|
REG_WRITE(reg, 0x18C);
|
|
reg = 0x5000A044;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Mac SIFS 1.6us = 0x64 * 16ns
|
REG_WRITE(reg, 0x640064);
|
/**********************************************/
|
|
// val = REG_READ(0x4000002C);
|
// REG_WRITE(0x4000002C, val | (1 << 30) | (1 << 25));
|
|
reg = 0x40000608;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// LPF ADC LPF_BN bias current %50
|
val = REG_READ(0x40000608);
|
val |= 0x4U;
|
REG_WRITE(reg, val);
|
|
reg = 0x40000700;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Disable RX AGC, and use register to configure the gain of LNA and filter;
|
val = (1U << 31) | ((default_radar_cfg.rx_gain_level & 0x1FU) << 16) | (default_radar_cfg.lna_gain_level & 0x7U);
|
REG_WRITE(reg, val);
|
|
/******************************************TXBE REG*********************************************/
|
|
/* These four parameters are compatible with ranging mode
|
* ### trx mode 15.4z bprf
|
* ### Channel NUM, Channel switching will involve reconfiguration of transmit power
|
* ### MEAN_PRF_64M
|
* ### Data bit rate 6.8M
|
*/
|
|
// Set tx Preamble length 16, sfd 8
|
reg = 0x40001000;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// set preamble length 16
|
val = 0x20E368;
|
// clear tx preamble length and SFD length
|
val = val & ~((0x7U << 10) | (0x3FU << 13) | (0x7U << 20));
|
// SFD length 8, Mean prf 64M
|
val |= (0x7U << 13) | (0x2U << 20);
|
REG_WRITE(reg, val);
|
|
reg = 0x40001008;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Set sts tx segment length 16/32/64
|
// clear tx STS_SEGNUM and STS_SEGLEN and EN_CUST_STS
|
val = val & ~((0x3U << 4) | (0x7U << 6) | (0x1U << 9));
|
// set sts segment length and customized sts bit
|
val |= ((0x1U << 9) | (default_radar_cfg.sts_len << 6));
|
REG_WRITE(reg, val);
|
|
reg = 0x4000103C;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Set STS polarity of 1 and 0
|
REG_WRITE(reg, (0x1U << 5));
|
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x4000101C;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x4000101C);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40001020;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40001020);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40001024;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40001024);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40001028;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40001028);
|
reg_save.reg_count++;
|
/// TXSTSKEY
|
REG_WRITE(0x4000101C, sts_key);
|
REG_WRITE(0x40001020, sts_key);
|
REG_WRITE(0x40001024, sts_key);
|
REG_WRITE(0x40001028, sts_key);
|
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x4000100C;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x4000100C);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40001010;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40001010);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40001014;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40001014);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40001018;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40001018);
|
reg_save.reg_count++;
|
REG_WRITE(0x4000100C, 0x0);
|
REG_WRITE(0x40001010, 0x0);
|
REG_WRITE(0x40001014, 0x0);
|
REG_WRITE(0x40001018, 0x0);
|
|
/******************************************RXBE REG*********************************************/
|
reg = 0x40002000;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
val = 0x72814U;
|
// Clear tx STS_SEGNUM and STS_SEGLEN and EN_CUST_STS
|
val = val & ~(0x7U << 10);
|
// Set rx sts len, Customized sts bit
|
val |= ((default_radar_cfg.sts_len << 10) | (0x1U << 13));
|
REG_WRITE(reg, val);
|
|
reg = 0x40002004;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Off main tap phase compensation
|
val |= (0x1U << 27);
|
REG_WRITE(reg, val);
|
|
#define RX_DUMP_EN 1
|
#define DUMP_IQ_AFTER_RX_CTRL 1 /* IQ data after rx_ctrl 64bit 500M 7bit */
|
#define DUMP_IQ_BEFORE_RX_CTRL 2 /* IQ data before rx_ctrl 64bit 500M 7bit */
|
#define DUMP_IQ_DS2 3 /* IQ data after DS2 64bit 500M 8bit */
|
#define DUMP_IQ_DS1 4 /* IQ data after DS1 128bit 1G 8bit */
|
#define DUMP_I_2G_ADC 5 /* I component data from ADC 128bit 2G 7bit */
|
#define DUMP_Q_2G_ADC 6 /* Q component data from ADC 128bit 2G 7bit */
|
|
#define DUMP_POS_STS 0 /* 0 : Dump STS */
|
#define DUMP_POS_RXEN 1 /* 1 : rxen */
|
#define DUMP_POS_BURST_DETECTION 2 /* 2 : burst detection */
|
#define DUMP_POS_PHR 3 /* 3 : phr */
|
#define TRUNC_MODE 0
|
|
reg = 0x40002040;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Set rx debug reg
|
#if UWB_RADAR_2G_MODE_EN
|
val = (uint32_t)((DUMP_IQ_DS1 & 0x7) | ((DUMP_POS_RXEN & 0x3) << 3) | ((TRUNC_MODE & 0x1) << 5) | (RX_DUMP_EN << 7) | ((DISCARD_SYMBOLS & 0xfff) << 8));
|
#else
|
val = (uint32_t)((DUMP_IQ_DS2 & 0x7) | ((DUMP_POS_RXEN & 0x3) << 3) | ((TRUNC_MODE & 0x1) << 5) | (RX_DUMP_EN << 7) | ((DISCARD_SYMBOLS & 0xfff) << 8));
|
#endif
|
REG_WRITE(reg, val);
|
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40002028;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40002028);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x4000202C;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x4000202C);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40002030;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40002030);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40002034;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40002034);
|
reg_save.reg_count++;
|
/// RXSTSKEY
|
REG_WRITE(0x40002028, sts_key);
|
REG_WRITE(0x4000202C, sts_key);
|
REG_WRITE(0x40002030, sts_key);
|
REG_WRITE(0x40002034, sts_key);
|
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40002018;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40002018);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x4000201C;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x4000201C);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40002020;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40002020);
|
reg_save.reg_count++;
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = 0x40002024;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = REG_READ(0x40002024);
|
reg_save.reg_count++;
|
REG_WRITE(0x40002018, 0x0);
|
REG_WRITE(0x4000201C, 0x0);
|
REG_WRITE(0x40002020, 0x0);
|
REG_WRITE(0x40002024, 0x0);
|
|
/******************************************RXFE REG*********************************************/
|
|
reg = 0x40003040;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Disable enAGC
|
val &= 0xFFFFFFFE;
|
REG_WRITE(reg, val);
|
|
/******************************************SYNC REG*********************************************/
|
reg = 0x40004048;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// clear SFD length
|
val = val & ~0xFFU;
|
// set SFD length 8
|
val |= 0xBU;
|
REG_WRITE(reg, val);
|
|
reg = 0x40004054;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Burst detection absolution threshold to set maximum,no synchronization
|
val |= 0x1FFFFFU;
|
REG_WRITE(reg, val);
|
|
/******************************************RXFE REG*********************************************/
|
reg = 0x40003020;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
// Disable PLL(disable CFO loop and TO loop)
|
val |= 0x3U;
|
REG_WRITE(reg, val);
|
|
reg = 0x40003034;
|
val = REG_READ(reg);
|
reg_save.reg_store[reg_save.reg_count][STORE_REG] = reg;
|
reg_save.reg_store[reg_save.reg_count][STORE_VAL] = val;
|
reg_save.reg_count++;
|
#if UWB_RADAR_2G_MODE_EN
|
// Set 2G ADC sampling input
|
REG_WRITE(reg, 0);
|
#else
|
// Set 1G ADC sampling input
|
REG_WRITE(reg, 1);
|
#endif
|
|
/******************************************SYS REG*********************************************/
|
|
if (default_radar_cfg.ranging_tx_power_level != default_radar_cfg.tx_power_level)
|
{
|
uwb_tx_power_set(default_radar_cfg.tx_power_level);
|
}
|
|
reg_save.magic = 0;
|
for (uint32_t i = 0; i < reg_save.reg_count; i++)
|
{
|
reg_save.magic ^= reg_save.reg_store[i][STORE_REG];
|
reg_save.magic ^= reg_save.reg_store[i][STORE_VAL];
|
}
|
|
// LOG_INFO(TRACE_MODULE_DRIVER, "%d reg val stored, magic %x\r\n", reg_save.reg_count, reg_save.magic);
|
|
// enable dump mode
|
phy_dump_en(1);
|
|
return 0;
|
}
|
|
int radar_disable(void)
|
{
|
phy_dump_en(0);
|
ASSERT(reg_save.reg_count < STORE_REG_COUNT_MAX, "The number of radar registers stored is overwritten");
|
|
uint32_t val = 0;
|
for (uint32_t i = 0; i < reg_save.reg_count; i++)
|
{
|
val ^= reg_save.reg_store[i][STORE_REG];
|
val ^= reg_save.reg_store[i][STORE_VAL];
|
}
|
|
ASSERT(val == reg_save.magic, "Radar Storage content is overwritten");
|
|
// recovery site
|
for (uint32_t j = 0; j < reg_save.reg_count; j++)
|
{
|
REG_WRITE(reg_save.reg_store[j][STORE_REG], reg_save.reg_store[j][STORE_VAL]);
|
}
|
|
if (default_radar_cfg.ranging_tx_power_level != default_radar_cfg.tx_power_level)
|
{
|
uwb_tx_power_set(default_radar_cfg.ranging_tx_power_level);
|
}
|
|
return 0;
|
}
|
|
void radar_start(uint8_t ant_idx)
|
{
|
uint32_t lock = int_lock();
|
|
uwb_rx_antenna_open((enum UWB_RX_ANT_T)rx_ant_id[ant_idx]);
|
|
// enter debuge mode
|
enter_debug_mode();
|
|
#if UWB_RADAR_1TNR_MODE == 1
|
static uint32_t loopback_anchor_point;
|
|
uint32_t rx_win = US_TO_PHY_TIMER_COUNT(UWB_RADAR_RX_WINDOW);
|
uint32_t sts_len = UWB_RADAR_STS_LENGTH;
|
if (ant_idx == 0)
|
{
|
loopback_anchor_point = REG_READ(0x40000418) + US_TO_PHY_TIMER_COUNT(UWB_RADAR_PREFETCH_TIME);
|
}
|
else if (ant_idx == 1)
|
{
|
rx_win = US_TO_PHY_TIMER_COUNT(UWB_RADAR_RX_WINDOW2);
|
sts_len = (sts_len >= 1) ? sts_len - 1 : 0;
|
}
|
else if (ant_idx == 2)
|
{
|
rx_win = US_TO_PHY_TIMER_COUNT(UWB_RADAR_RX_WINDOW3);
|
sts_len = (sts_len >= 2) ? sts_len - 2 : 0;
|
}
|
uint32_t target = loopback_anchor_point + US_TO_PHY_TIMER_COUNT(FRAME_INTERVAL * ant_idx);
|
|
// Delay discard_symbols, dump data
|
uint32_t val = (uint32_t)(4 | (1 << 3) | (1 << 7) | ((DISCARD_SYMBOLS & 0xfff) << 8));
|
REG_WRITE(0x40002040, val);
|
|
// TXSTS
|
val = REG_READ(0x40001008);
|
// clear sts seglen
|
val = val & ~(0x7U << 6);
|
// STS_SEGLEN
|
val |= (sts_len << 6);
|
REG_WRITE(0x40001008, val);
|
|
// RXSTS
|
val = REG_READ(0x40002000);
|
// clear sts seglen
|
val = val & ~(0x7U << 10);
|
// STS_SEGLEN
|
val |= (sts_len << 10);
|
REG_WRITE(0x40002000, val);
|
|
// Rx Timeout
|
REG_WRITE(0x40000408, target + rx_win);
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target & 0xFFFFFFFC, 0, NULL, 0);
|
mac_start();
|
#else
|
#if 0
|
mac_tx(EVT_MODE_MAC_PHY_ASAP, 0, 0, NULL, 0);
|
// reset DPTX DPRX
|
SYSCON->SYS_RMU &= ~((1U << 25) | (1U << 24));
|
SYSCON->SYS_RMU |= ((1U << 25) | (1U << 24));
|
|
// Rx Timeout
|
REG_WRITE(0x40000408, REG_READ(0x40000418) + US_TO_PHY_TIMER_COUNT(UWB_RADAR_RX_WINDOW));
|
|
// sw trigger <==> void mac_start(void)
|
REG_WRITE(0x5000A01C, 1);
|
#else
|
uint32_t target = REG_READ(0x40000418) + US_TO_PHY_TIMER_COUNT(UWB_RADAR_PREFETCH_TIME);
|
// Rx Timeout
|
REG_WRITE(0x40000408, target + US_TO_PHY_TIMER_COUNT(UWB_RADAR_RX_WINDOW));
|
mac_tx(EVT_MODE_MAC_ASAP_PHY_FIX, target & 0xFFFFFFFC, 0, NULL, 0);
|
mac_start();
|
#endif
|
#endif
|
int_unlock(lock);
|
}
|
