/*
|
* 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.
|
*/
|
|
/*
|
*
|
* Modification History:
|
*
|
*
|
*/
|
#include "MK8000_kf_top.h"
|
#include "mk_trace.h"
|
#include "lib_kf.h"
|
#include "mk_misc.h"
|
|
static struct KF_MAC_ADDR_T *addr_arr;
|
static size_t kf_support_num = 0;
|
static uint32_t kf_timeout_ms = 0;
|
|
static int8_t do_init = 1;
|
|
static kf_params_t g_kf_params = {
|
0.048f, // should be equal to update time
|
0.53356f, // Q = (9.8/3)^2/20
|
0.0036f, // ranging_R = 0.06^2
|
0.000625f, // azimuth_R = 0.025^2
|
0.2f, // ranging v_max in m/s
|
10.0f, // azimuth v_max in degree/s
|
1, // enable ranging velocity limitation
|
1, // enable azimuth/elevation velocity limitation
|
};
|
|
static uint16_t get_addr_index(uint8_t *mac_addr, uint8_t data_type)
|
{
|
for (uint16_t i = 0; i < kf_support_num; i++)
|
{
|
if ((memcmp(addr_arr[i].mac_addr, mac_addr, 8) == 0) && (addr_arr[i].data_type == data_type))
|
{
|
return i;
|
}
|
}
|
uint32_t min_time = addr_arr[0].elapsed_time;
|
uint16_t min_time_index = 0;
|
for (uint16_t i = 0; i < kf_support_num; i++)
|
{
|
if (addr_arr[i].elapsed_time < min_time)
|
{
|
min_time = addr_arr[i].elapsed_time;
|
min_time_index = i;
|
}
|
}
|
memcpy(addr_arr[min_time_index].mac_addr, mac_addr, 8);
|
addr_arr[min_time_index].data_type = data_type;
|
addr_arr[min_time_index].elapsed_time = 0;
|
return min_time_index;
|
}
|
|
void loc_post_kf_config(uint32_t update_period_ms,
|
struct KF_MAC_ADDR_T *mac_addr_cache,
|
struct KF_CHANNEL_CACHE_T *kf_channel_cache,
|
struct KF_MAT_VALUE_CACHE_T *mat_value_cache,
|
size_t cache_len,
|
uint32_t timeout_ms)
|
{
|
g_kf_params.dt = (float)(update_period_ms / 1000.0);
|
addr_arr = mac_addr_cache;
|
memset(addr_arr, 0, sizeof(struct KF_MAC_ADDR_T) * cache_len);
|
kf_support_num = cache_len;
|
kf_timeout_ms = timeout_ms;
|
|
MK8000_kf_config(g_kf_params, (kf_channel_env_t *)kf_channel_cache, (kf_channel_mat_value_t *)mat_value_cache, cache_len);
|
}
|
|
uint8_t loc_kf_filter(float data_meas, enum KF_DATA_TYPE_T data_type, uint8_t *mac_addr, float *data_post)
|
{
|
if (kf_support_num == 0)
|
{
|
return 0;
|
}
|
uint16_t id = get_addr_index(mac_addr, (uint8_t)data_type);
|
uint32_t now = sys_tick_ms();
|
if (addr_arr[id].elapsed_time == 0 || (now - addr_arr[id].elapsed_time) > kf_timeout_ms)
|
{
|
do_init = 1;
|
}
|
addr_arr[id].elapsed_time = now;
|
enum KF_CHANNEL_TYPE_T type = KF_CHANNEL_TYPE_RANGING;
|
if (data_type == KF_DATA_TYPE_AZIMUTH)
|
{
|
type = KF_CHANNEL_TYPE_ANGLE;
|
}
|
else if (data_type == KF_DATA_TYPE_ELEVATION)
|
{
|
if (pdoa_3d_ant_layout_get() == 0)
|
{
|
type = KF_CHANNEL_TYPE_ANGLE_POSITIVE;
|
}
|
else
|
{
|
type = KF_CHANNEL_TYPE_ANGLE;
|
}
|
}
|
MK8000_kf_processor(data_meas, id, type, do_init, data_post);
|
do_init = 0;
|
return 1;
|
}
|
