Index: prj/MDK/YC3121_Keil.uvoptx
===================================================================
--- /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/prj/MDK/YC3121_Keil.uvoptx (revision 923)
+++ /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/prj/MDK/YC3121_Keil.uvoptx (working copy)
@@ -273,6 +273,18 @@
0
0
+
+ 2
+ 6
+ 1
+ 0
+ 0
+ 0
+ ..\..\user\app_rssi_filter.c
+ app_rssi_filter.c
+ 0
+ 0
+
@@ -283,7 +295,7 @@
0
3
- 6
+ 7
1
0
0
@@ -303,7 +315,7 @@
0
4
- 7
+ 8
1
0
0
@@ -315,7 +327,7 @@
4
- 8
+ 9
1
0
0
@@ -335,7 +347,7 @@
0
5
- 9
+ 10
1
0
0
@@ -347,7 +359,7 @@
5
- 10
+ 11
1
0
0
@@ -359,7 +371,7 @@
5
- 11
+ 12
1
0
0
@@ -379,7 +391,7 @@
0
6
- 12
+ 13
1
0
0
@@ -391,7 +403,7 @@
6
- 13
+ 14
1
0
0
@@ -403,7 +415,7 @@
6
- 14
+ 15
1
0
0
@@ -415,7 +427,7 @@
6
- 15
+ 16
1
0
0
@@ -427,7 +439,7 @@
6
- 16
+ 17
1
0
0
@@ -439,7 +451,7 @@
6
- 17
+ 18
1
0
0
@@ -451,7 +463,7 @@
6
- 18
+ 19
1
0
0
@@ -463,7 +475,7 @@
6
- 19
+ 20
1
0
0
@@ -475,7 +487,7 @@
6
- 20
+ 21
4
0
0
@@ -487,7 +499,7 @@
6
- 21
+ 22
1
0
0
@@ -507,7 +519,7 @@
0
7
- 22
+ 23
2
0
0
@@ -519,7 +531,7 @@
7
- 23
+ 24
2
0
0
Index: prj/MDK/YC3121_Keil.uvprojx
===================================================================
--- /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/prj/MDK/YC3121_Keil.uvprojx (revision 923)
+++ /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/prj/MDK/YC3121_Keil.uvprojx (working copy)
@@ -412,6 +412,11 @@
1
..\..\user\app_util.c
+
+ app_rssi_filter.c
+ 1
+ ..\..\user\app_rssi_filter.c
+
Index: user/app_rssi_filter.h
===================================================================
--- /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/user/app_rssi_filter.h (nonexistent)
+++ /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/user/app_rssi_filter.h (working copy)
@@ -0,0 +1,39 @@
+/**
+ * @file app_rssi_filter.h
+ * @brief
+ * @date 2021-09-24
+ */
+
+#ifndef __APP_RSSI_FILTER_H_
+#define __APP_RSSI_FILTER_H_
+
+#include "yc3121.h"
+#include "type.h"
+
+
+#define RSSI_LOG 1
+
+
+
+//2*K+1 == 滑动中值算法buf长度
+#define K 8
+
+//2*N+1 == 不滑动中值buf长度
+#define N 5
+
+//rssi滤波
+int8_t rssiFilter(uint8_t now_rssi);
+
+//重置无感相关参数
+void rssiFilterReset(uint8_t isResetBle);
+
+//获取rssi瞬态值
+int8_t btGetRssi(void);
+
+//获取rssi滤波值
+int8_t btGetFliterRssi(void);
+
+#endif
+
+
+
Index: user/app_rssi_filter.c
===================================================================
--- /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/user/app_rssi_filter.c (nonexistent)
+++ /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/user/app_rssi_filter.c (working copy)
@@ -0,0 +1,267 @@
+#include "app_rssi_filter.h"
+#include "app_util.h"
+#include "yc_ipc.h"
+
+//RSSI算法选择 0：滑动中值和平均加权 1：不滑动-中值算法 2: 滑动 卡尔曼滤波
+uint8_t rssi_arithmetic_choose = 0;
+
+uint8_t mid_value_buf[2 * K + 1]; //滑动中值buf
+uint8_t mid_value_buf_index;
+
+uint8_t mid_value_buf_1[2 * N + 1]; //不滑动中值buf
+uint8_t mid_value_buf_index_1;
+
+uint8_t isBufFull;
+
+
+//冒泡排序
+//len：数组长度
+//dataPtr:数组
+void bubbleSort(uint8_t len, uint8_t *dataPtr)
+{
+ int i, j, temp;
+ for (i = 0; i < len - 1; i++)
+ {
+ for (j = 0; j < len - 1 - i; j++)
+ {
+ if (dataPtr[j] > dataPtr[j + 1])
+ {
+ temp = dataPtr[j];
+ dataPtr[j] = dataPtr[j + 1];
+ dataPtr[j + 1] = temp;
+ }
+ }
+ }
+}
+
+//rssi处理函数
+//input: 从汇编读到的瞬时值 滑动中值 和 平均加权
+//return:输出的有效值
+int8_t slideMedianFilter(int8_t now_rssi)
+{
+ int8_t rssi_after_filter = 0;
+ int16_t sum_rssi = 0;
+ int size = 2 * K + 1;
+ int8_t sorted_buf[size];
+
+ if (now_rssi == 0)
+ return 0;
+ if (!isBufFull && mid_value_buf_index < 2 * K + 2)
+ {
+ mid_value_buf[mid_value_buf_index++] = now_rssi;
+ isBufFull = (mid_value_buf_index == size) ? 1 : 0;
+ if (isBufFull)
+ mid_value_buf_index = 0;
+ return 0;
+ }
+ else
+ {
+ mid_value_buf[mid_value_buf_index++] = now_rssi;
+ if (mid_value_buf_index == size)
+ mid_value_buf_index = 0;
+ }
+ memcpy(sorted_buf, mid_value_buf, 2 * K + 1);
+ bubbleSort(2 * K + 1, (uint8_t*)sorted_buf);
+
+ for (uint8_t i = 0; i <= 5; i++)
+ {
+ sum_rssi += sorted_buf[i];
+ }
+ rssi_after_filter = sum_rssi/6 ;
+
+#if RSSI_LOG
+ MyPrintf("(%02d,%02d)\r\n", now_rssi, rssi_after_filter);
+#endif
+
+ return rssi_after_filter;
+}
+
+
+//rssi处理函数
+//input: 从汇编读到的瞬时值 不滑动平均
+//return:输出的有效值
+uint8_t medianFilter(uint8_t now_rssi)
+{
+ if (now_rssi == 0)
+ return 0;
+
+ if (mid_value_buf_index_1 < 2 * N + 1)
+ {
+ //MyPrintf("median_filter now rssi:%02d\r\n",now_rssi);
+ mid_value_buf_1[mid_value_buf_index_1++] = now_rssi;
+ return 0;
+ }
+
+ mid_value_buf_1[mid_value_buf_index_1] = now_rssi;
+ mid_value_buf_index_1 = 0;
+
+ uint8_t sorted_buf[2 * N + 1];
+ memcpy(sorted_buf, mid_value_buf_1, 2 * N + 1);
+ bubbleSort(2 * N + 1, sorted_buf);
+ uint8_t rssi_after_filter = sorted_buf[N];
+
+#if RSSI_LOG
+ MyPrintf("median_filter rssi:%02d\r\n",rssi_after_filter);
+#endif
+ return rssi_after_filter;
+
+}
+
+
+//reset无感相关参数
+//input: uint8_t isResetBle
+// 参数为1，重置BLE Buff
+void rssiFilterReset(uint8_t isResetBle)
+{
+ if(isResetBle)
+ {
+ if (rssi_arithmetic_choose == 0)
+ {
+ mid_value_buf_index = 0;
+ isBufFull = 0;
+ }
+ else
+ {
+ mid_value_buf_index_1 = 0;
+ }
+ }
+// else {
+// mid_value_buf_index_24g = 0;
+// isPkeBufFull = 0;
+// }
+}
+
+static float prevData = 0;
+static float p = 10, q = 0.00001, r = 0.05, kGain = 0;
+
+float kalmanFilter(float inData)
+{
+ p = p + q;
+ kGain = p / (p + r);
+
+ inData = prevData + (kGain * (inData - prevData));
+ p = (1 - kGain) * p;
+
+ prevData = inData;
+ return inData;
+}
+
+//rssi处理函数
+//input: 从汇编读到的瞬时值 滑动卡尔曼滤波
+//return:输出的有效值
+int8_t slideKalmanFilter(int8_t now_rssi)
+{
+ int8_t rssi_after_filter = 0;
+ int16_t sum_rssi = 0;
+ int size = 2 * K + 1;
+ int8_t sorted_buf[size];
+
+ if (now_rssi == 0)
+ return 0;
+ if (!isBufFull && mid_value_buf_index < 2 * K + 2)
+ {
+ mid_value_buf[mid_value_buf_index++] = now_rssi;
+ isBufFull = (mid_value_buf_index == size) ? 1 : 0;
+ if (isBufFull)
+ mid_value_buf_index = 0;
+ return 0;
+ }
+ else
+ {
+ mid_value_buf[mid_value_buf_index++] = now_rssi;
+ if (mid_value_buf_index == size)
+ mid_value_buf_index = 0;
+ }
+
+ for (int i = mid_value_buf_index; i < mid_value_buf_index+size; i++)
+ rssi_after_filter = kalmanFilter(mid_value_buf[i%size]);
+
+#if RSSI_LOG
+ MyPrintf("(%02d,%02d)\r\n", now_rssi, rssi_after_filter);
+#endif
+
+ return rssi_after_filter;
+}
+
+
+//滤波算法选择
+int8_t rssiFilter(uint8_t now_rssi)
+{
+ if (rssi_arithmetic_choose == 0)
+ {
+ return slideMedianFilter(now_rssi);
+ }
+ else if (rssi_arithmetic_choose == 1)
+ {
+ return medianFilter(now_rssi);
+ }
+ else
+ {
+ return slideKalmanFilter(now_rssi);
+ }
+}
+
+
+#define mem_rssi 0xc0018
+#define mem_ui_state_map 0xc4652
+
+uint16_t btGetUiStateMap(void)
+{
+ return HR_REG_16BIT(mem_ui_state_map);
+}
+
+int8_t btGetRssi(void)
+{
+ int8_t data;
+ data = -(HR_REG_8BIT(mem_rssi));
+ if(btGetUiStateMap() & 0x0001)
+ {
+#if RSSI_LOG
+ MyPrintf("classic rssi:%02d\r\n",data);
+#endif
+ return data;
+ }
+ else if(btGetUiStateMap() & 0x0200)
+ {
+#if RSSI_LOG
+ MyPrintf("ble rssi:%02d\r\n",data);
+#endif
+ return data;
+ }
+ else
+ {
+#if RSSI_LOG
+ MyPrintf("no link rssi\r\n");
+#endif
+ return 0;
+ }
+}
+
+
+int8_t btGetFliterRssi(void)
+{
+ int8_t data;
+ data = rssiFilter(-HR_REG_8BIT(mem_rssi));
+ if(btGetUiStateMap() & 0x0001)
+ {
+#if RSSI_LOG
+ MyPrintf("classic rssi:%02d\r\n",data);
+#endif
+ return data;
+ }
+ else if(btGetUiStateMap() & 0x0200)
+ {
+#if RSSI_LOG
+ MyPrintf("ble rssi:%02d\r\n",data);
+#endif
+ return data;
+ }
+ else
+ {
+#if RSSI_LOG
+ MyPrintf("no link rssi\r\n");
+#endif
+ return 0;
+ }
+}
+
Index: user/main.c
===================================================================
--- /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/user/main.c (revision 923)
+++ /YC3121_demo/ModuleDemo/BlueTooth/BT_HFP_BLE_DEMO/user/main.c (working copy)
@@ -35,6 +35,7 @@
#include "app_bt_protocol.h"
#include "app_ble_protocol.h"
#include "app_ble_gatt.h"
+#include "app_rssi_filter.h"
#include "eeprom_config.h"
#include "app_eeprom_user_info.h"
@@ -60,8 +61,6 @@
/* Private function prototypes -----------------------------------------------*/
void UART_Configuration(void);
void btSetLpmMode(uint8_t mode);
-uint8_t btGetRssi(void);
-
void BT_IRQHandler()
@@ -149,6 +148,7 @@
while (1)
{
BT_Progress();
+// btGetFliterRssi();
}
}
@@ -490,33 +490,6 @@
-#define mem_rssi 0xc0018
-#define mem_ui_state_map 0xc4652
-
-uint16_t btGetUiStateMap(void)
-{
- return HR_REG_16BIT(mem_ui_state_map);
-}
-
-uint8_t btGetRssi(void)
-{
- uint8_t data;
- data = HR_REG_8BIT(mem_rssi);
- if(btGetUiStateMap() & 0x0001)
- {
- uartPrintArray("classic rssi",&data,1,TYPE_HEX);
- return data;
- }
- else if(btGetUiStateMap() & 0x0200)
- {
- uartPrintArray("ble rssi",&data,1,TYPE_HEX);
- return data;
- }
- else
- return 0;
-}
-
-
/**
* @brief 设置低功耗模式
*