/*
|
* 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_trace.h"
|
#include "mk_clock.h"
|
#include "mk_power.h"
|
#include "mk_uwb.h"
|
#include "mk_radar.h"
|
#include "mk_sleep_timer.h"
|
#include "mk_misc.h"
|
#include "uwb_api.h"
|
#include "uwb_radar_task.h"
|
#include "uwb_radar.h"
|
#if UWB_RADAR_DETECT_PROCESS_EN
|
#include "mk8000_present_interface.h"
|
#endif
|
// #include "board.h"
|
|
#define DEBUG_MEM_ADDR 0x02010000
|
#define RADAR_PRINT_EN 0
|
|
/*************************************************************************************************/
|
/*!
|
* \brief WSF event handler for uwb radar task.
|
*
|
* \param event WSF event mask.
|
* \param msg WSF message.
|
*
|
* \return None.
|
*/
|
/*************************************************************************************************/
|
|
#if UWB_RADAR_DETECT_PROCESS_EN
|
static float ref_out[256] = {0.0};
|
static struct Cf_sto cf_data[64] = {0};
|
|
static struct Present_sto present = {
|
.cf_data = cf_data,
|
.Y = ref_out,
|
.index = 0,
|
.flag = 0,
|
};
|
|
static struct Pres_para_sto pre_para = {
|
.cp_para =
|
{
|
// choose cfar threshold in each section
|
.sec_alp = {35, 33, 20, 10, 10, 15, 15},
|
// Choose cfar solution in each section,0:so cfar 1:ca cfar 2:go cfar
|
.sec_sol = {1, 1, 0, 0, 0, 1, 1},
|
// range index:[start,end,multi taget num in each section]
|
.range = {2, 36, 0},
|
// the safe section,the reference section
|
.ref_x = {5, 4, 2},
|
// Apart the range into sen_len sections
|
.sec_len = 5,
|
// 0:choose max power target
|
// 1:choose max power target in each section
|
// 2:choose max power target in first section
|
.tar_num = 3,
|
.nbor = 1,
|
.ma_val = 1000,
|
},
|
// 1:adaptive,High Freq 20/30/80Hz 0:not adaptive 1Hz
|
.train = 1,
|
// 0: no calibration,
|
// 1: take last pulse as reference, >1: take mean(rep pulses) as reference, from frame=0 to frame = rep
|
.rep = 1,
|
// Number of learning frame
|
.frame = 0,
|
.norm = 1000,
|
.apole = 0.3f,
|
.acc = 3,
|
};
|
#endif
|
|
struct sample16_pack
|
{
|
uint16_t header;
|
uint16_t length;
|
uint32_t seq;
|
float acd_out[PULSE_PERIOD_POINTS(0) * 2];
|
uint16_t check_num;
|
uint16_t end;
|
};
|
|
struct sample32_pack
|
{
|
uint16_t header;
|
uint16_t length;
|
uint32_t seq;
|
float acd_out[PULSE_PERIOD_POINTS(1) * 2];
|
uint16_t check_num;
|
uint16_t end;
|
};
|
|
struct sample64_pack
|
{
|
uint16_t header;
|
uint16_t length;
|
uint32_t seq;
|
float acd_out[PULSE_PERIOD_POINTS(2) * 2];
|
uint16_t check_num;
|
uint16_t end;
|
};
|
|
struct sample128_pack
|
{
|
uint16_t header;
|
uint16_t length;
|
uint32_t seq;
|
float acd_out[PULSE_PERIOD_POINTS(3) * 2];
|
uint16_t check_num;
|
uint16_t end;
|
};
|
|
struct sample256_pack
|
{
|
uint16_t header;
|
uint16_t length;
|
uint32_t seq;
|
float acd_out[PULSE_PERIOD_POINTS(4) * 2];
|
uint16_t check_num;
|
uint16_t end;
|
};
|
|
struct sample_1tnr_pack
|
{
|
uint16_t header;
|
uint16_t length;
|
uint32_t seq;
|
float acd_out[OUT_FRAME_LEN * UWB_RADAR_RX_PORT_NUM];
|
uint16_t check_num;
|
uint16_t end;
|
};
|
|
union xfer_sample_pack
|
{
|
#define SAMPLE_PACK_FIX_LENGTH 12
|
struct sample16_pack sample16;
|
struct sample32_pack sample32;
|
struct sample64_pack sample64;
|
struct sample128_pack sample128;
|
struct sample256_pack sample256;
|
struct sample_1tnr_pack sample_1tnr;
|
};
|
|
static union xfer_sample_pack xfer_pack = {
|
.sample_1tnr.header = 0x5555,
|
};
|
|
#if UWB_RADAR_DETECT_PROCESS_EN == 0 && UWB_RADAR_UCI_EN == 0
|
static void xfer_done(void *dev, uint32_t err_code)
|
{
|
// board_led_off(BOARD_LED_1);
|
}
|
#endif
|
|
void uwb_radar_handler(wsfEventMask_t event, const void *param)
|
{
|
// const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param;
|
// board_led_on(BOARD_LED_2);
|
|
if (uwb_is_radar_mode())
|
{
|
uint16_t data_len;
|
|
switch (uwb_app_config.session_type)
|
{
|
case SESSION_TYPE_VENDOR_RANGING_AND_RADAR:
|
{
|
switch (event)
|
{
|
case UWB_RADAR_START_SAMPLING_EVT:
|
{
|
/* Creat radar event */
|
uwb_radar_creat_event();
|
phy_sts_pkt_cfg_set(3);
|
radar_enable();
|
power_on_radio(1, 1);
|
radar_start(radar_env.rx_ant_idx);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
}
|
break;
|
|
case UWB_RADAR_SAMPLING_COMPLETE_EVT:
|
{
|
/* Data preprocessing, XIP 19.8ms */
|
power_off_radio();
|
radar_disable();
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
|
radar_data_process((uint32_t *)DEBUG_MEM_ADDR, xfer_pack.sample_1tnr.acd_out, &data_len);
|
uwb_radar_destroy_event();
|
|
#if UWB_RADAR_UCI_EN
|
uwbapi_report_radar_raw_data(data_len, (uint8_t *)xfer_pack.sample_1tnr.acd_out);
|
#else
|
#if UWB_RADAR_DETECT_PROCESS_EN
|
// board_led_on(BOARD_LED_1);
|
mcu_target_detect_process(xfer_pack.sample_1tnr.acd_out, PULSE_PERIOD_POINTS(UWB_RADAR_PULSE_PERIOD), &pre_para, &present);
|
/* Object detection, the result shows */
|
if (present.flag)
|
{
|
uint16_t distance = (uint16_t)(((float)present.index * 0.15f) * 100);
|
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "Presence: YES, Radar Distance: %ucm\r\n", distance);
|
}
|
else
|
{
|
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "Presence: NO, Radar Distance: 0cm\r\n");
|
}
|
// board_led_off(BOARD_LED_1);
|
|
#else
|
// board_led_on(BOARD_LED_1);
|
LOG_INFO(TRACE_MODULE_APP, "Radar output data len: %u\r\n", data_len);
|
xfer_pack.sample_1tnr.length = data_len + SAMPLE_PACK_FIX_LENGTH;
|
xfer_pack.sample_1tnr.seq = radar_env.rx_ant_idx;
|
uint16_t check_num = uwb_radar_data_checksum((const uint8_t *)xfer_pack.sample_1tnr.acd_out, data_len);
|
|
#if UWB_RADAR_1TNR_MODE == 1
|
xfer_pack.sample_1tnr.check_num = check_num;
|
xfer_pack.sample_1tnr.end = 0xAAAA;
|
#else
|
if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_16NS)
|
{
|
xfer_pack.sample16.check_num = check_num;
|
xfer_pack.sample16.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_32NS)
|
{
|
xfer_pack.sample32.check_num = check_num;
|
xfer_pack.sample32.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_64NS)
|
{
|
xfer_pack.sample64.check_num = check_num;
|
xfer_pack.sample64.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_128NS)
|
{
|
xfer_pack.sample128.check_num = check_num;
|
xfer_pack.sample128.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_256NS)
|
{
|
xfer_pack.sample256.check_num = check_num;
|
xfer_pack.sample256.end = 0xAAAA;
|
}
|
#endif
|
uart_send(DUMP_DATA_PORT, (uint8_t *)(&xfer_pack), xfer_pack.sample_1tnr.length, xfer_done);
|
#endif
|
#endif
|
}
|
break;
|
}
|
}
|
break;
|
|
case SESSION_TYPE_VENDOR_ALONE_RADAR:
|
{
|
const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param;
|
if (msg != NULL)
|
{
|
if (msg->event == UWB_RADAR_ALONE_WAKEUP_MSG)
|
{
|
uwb_radar_alone_continue_trigger();
|
}
|
}
|
else
|
{
|
switch (event)
|
{
|
case UWB_RADAR_START_SAMPLING_EVT:
|
{
|
phy_sts_pkt_cfg_set(3);
|
radar_enable();
|
power_on_radio(1, 1);
|
radar_start(radar_env.rx_ant_idx);
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_SLEEP);
|
}
|
break;
|
|
case UWB_RADAR_SAMPLING_COMPLETE_EVT:
|
{
|
/* Data preprocessing, XIP 19.8ms */
|
power_off_radio();
|
radar_disable();
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
|
radar_data_process((uint32_t *)DEBUG_MEM_ADDR, xfer_pack.sample_1tnr.acd_out, &data_len);
|
|
#if UWB_RADAR_UCI_EN
|
uwbapi_report_radar_raw_data(data_len, (uint8_t *)xfer_pack.sample_1tnr.acd_out);
|
#else
|
#if RADAR_PRINT_EN
|
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "--------------%u--------------\r\n", data_len);
|
for (uint8_t i = 0; i < PULSE_PERIOD_POINTS(UWB_RADAR_PULSE_PERIOD); i++)
|
{
|
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "%.2f %.2f\n", xfer_pack.sample_1tnr.acd_out[i * 2],
|
xfer_pack.sample_1tnr.acd_out[i * 2 + 1]);
|
}
|
#endif
|
#if UWB_RADAR_DETECT_PROCESS_EN
|
// board_led_on(BOARD_LED_1);
|
mcu_target_detect_process(xfer_pack.sample_1tnr.acd_out, PULSE_PERIOD_POINTS(UWB_RADAR_PULSE_PERIOD), &pre_para, &present);
|
/* Object detection, the result shows */
|
if (present.flag)
|
{
|
uint16_t distance = (uint16_t)(((float)present.index * 0.15f) * 100);
|
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\nPresence: YES, Radar Distance: %ucm\r\n", distance);
|
}
|
else
|
{
|
LOG_INFO(TRACE_MODULE_APP | TRACE_NO_OPTION, "\r\nPresence: NO, Radar Distance: 0cm\r\n");
|
}
|
// board_led_off(BOARD_LED_1);
|
#else
|
// board_led_on(BOARD_LED_1);
|
LOG_INFO(TRACE_MODULE_APP, "Radar output data len: %u\r\n", data_len);
|
xfer_pack.sample_1tnr.length = data_len + SAMPLE_PACK_FIX_LENGTH;
|
xfer_pack.sample_1tnr.seq = radar_env.rx_ant_idx;
|
uint16_t check_num = uwb_radar_data_checksum((const uint8_t *)xfer_pack.sample_1tnr.acd_out, data_len);
|
#if UWB_RADAR_1TNR_MODE == 1
|
xfer_pack.sample_1tnr.check_num = check_num;
|
xfer_pack.sample_1tnr.end = 0xAAAA;
|
#else
|
if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_16NS)
|
{
|
xfer_pack.sample16.check_num = check_num;
|
xfer_pack.sample16.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_32NS)
|
{
|
xfer_pack.sample32.check_num = check_num;
|
xfer_pack.sample32.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_64NS)
|
{
|
xfer_pack.sample64.check_num = check_num;
|
xfer_pack.sample64.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_128NS)
|
{
|
xfer_pack.sample128.check_num = check_num;
|
xfer_pack.sample128.end = 0xAAAA;
|
}
|
else if (uwb_app_config.session_param.uwb_radar_pulse_period == UWB_RADAR_PULSE_PERIOD_256NS)
|
{
|
xfer_pack.sample256.check_num = check_num;
|
xfer_pack.sample256.end = 0xAAAA;
|
}
|
#endif
|
uart_send(DUMP_DATA_PORT, (uint8_t *)(&xfer_pack), xfer_pack.sample_1tnr.length, xfer_done);
|
#endif
|
#endif
|
}
|
break;
|
}
|
}
|
}
|
break;
|
}
|
}
|
else
|
{
|
radar_disable();
|
power_off_radio();
|
power_mode_request(POWER_UNIT_APP, POWER_MODE_POWER_DOWN);
|
uwb_radar_force_destroy_event();
|
if (SESSION_TYPE_VENDOR_ALONE_RADAR == uwb_app_config.session_type)
|
{
|
uwb_radar_alone_timer_clr();
|
}
|
}
|
// board_led_off(BOARD_LED_2);
|
}
|
