From cc432b761c884a0bd8e9d83db0a4e26109fc08b1 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期五, 08 十一月 2024 15:35:38 +0800
Subject: [PATCH] 安邦手环GPS删除部分无用数据和修改4G波特率9600出厂测试固件
---
keil/include/components/algo/src/kf/MK8000_kf_top.c | 142 +++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 142 insertions(+), 0 deletions(-)
diff --git a/keil/include/components/algo/src/kf/MK8000_kf_top.c b/keil/include/components/algo/src/kf/MK8000_kf_top.c
new file mode 100644
index 0000000..e861f91
--- /dev/null
+++ b/keil/include/components/algo/src/kf/MK8000_kf_top.c
@@ -0,0 +1,142 @@
+/*
+ * 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;
+}
--
Gitblit v1.9.3