Merge branch 'feature/angle_tank_filter' into 'master'

drivers/gpio/gpio_control.c

@@ -439,11 +439,11 @@ void tank0204_pwm_value(int pin,int value){
 		if(pin == 27){
           device_shoting_check(27,30); 
         }else if(pin == 5) {
-            softPwmWrite(7, 30);
-            my_zlog_info("pwm:7,0");
+            softPwmWrite(7, 35);
+            my_zlog_info("pwm:7,1");
         }else if(pin == 7) {
-            softPwmWrite(5, 30);
-            my_zlog_info("pwm:5,0");
+            softPwmWrite(5, 35);
+            my_zlog_info("pwm:5,1");
         }else {
             softPwmWrite(pin, 30);
             my_zlog_info("pwm:%d",pin);
@@ -530,7 +530,7 @@ void dog0501_pwm_value(int pin,int value) {	//软件陪我们控制调速
     if(value==1) {
 		if(pin == 27){
             softPwmWrite(27, 40);
-            softPwmWrite(7, 80);
+            softPwmWrite(7, 100);
         } else {
             softPwmWrite(pin, 35);
             my_zlog_info("pwm:%d",pin);

drivers/sensors/tank_angle.c

@@ -2,6 +2,8 @@
 #include "common.h"
 #include "ads1115.h"
 
+
+
 double tank_angle(){
     double angle=0;
     float angle_shot=ads1115_read_channel(2);
@@ -13,8 +15,6 @@ double tank_angle(){
     return angle;
 }
 
-
-
 int angle_limit(){
     if(ANGLE_LIMIT_INDEX == 1 ){
         int tank_limit_angele = tank_angle();
@@ -26,46 +26,19 @@ int angle_limit(){
 }
 
 
-/*
-  角度 EMA 滤波
-  - filter : 上次滤波值（度）
-  - raw      : 当前原始角度（度）
-  - alpha    : 衰减因子（0..1），越大越跟随原始值（推荐 0.05~0.5）
-  - deadband : 死区（度），小于该值认为是抖动
-*/
-// EN滤波结构体
-// typedef struct {
-//     double alpha;      // 滤波系数 (0~1)
-//     double filtered;   // 上一次滤波角度
-// } ENFilter;
-
-// static ENFilter filter = {.alpha = 0.5, .filtered = 0.05};
-
-// // 支持360°→0°跳变的滤波函数
-// double en_filter_update(double new_angle)
-// {
-//     double diff = new_angle - filter.filtered;
+float low_pass_filter_360(float prev_angle, float new_angle, float alpha) {
+    // 规整到 [0, 360)
+    prev_angle = fmod(prev_angle, 360.0f);
+    if (prev_angle < 0) prev_angle += 360.0f;
+    new_angle = fmod(new_angle, 360.0f);
+    if (new_angle < 0) new_angle += 360.0f;
     
-//     // 处理角度环绕（支持359→0或0→359平滑过渡）
-//     if (diff > 180.0)
-//         diff -= 360.0;
-//     else if (diff < -180.0)
-//         diff += 360.0;
-
-//     // 一阶低通滤波
-//     filter.filtered += filter.alpha * diff;
-
-//     // 结果保持在[0, 360)
-//     if (filter.filtered < 0)
-//         filter.filtered += 360.0;
-//     else if (filter.filtered >= 360.0)
-//         filter.filtered -= 360.0;
-
-//     return filter.filtered;
-// }
-
-// //上次的角度滤波后的值
-// double return_en_filter_last_angle(){
-//     return filter.filtered;
-// }
+    // 计算最短路径角度差
+    float diff = new_angle - prev_angle;
+    if (diff > 180.0f) diff -= 360.0f;
+    else if (diff < -180.0f) diff += 360.0f;
     
+    // 应用滤波
+    float filtered = prev_angle + alpha * diff;
+    return fmod(filtered + 360.0f, 360.0f); // 确保输出在 [0, 360)
+}
\ No newline at end of file

drivers/sensors/tank_angle.h

@@ -11,6 +11,8 @@ double tank_angle();
 
 int angle_limit();
 
+float low_pass_filter_360(float prev_angle, float new_angle, float alpha);
+
 // //角度滤波
 // double en_filter_update(double new_angle);
 

modules/mqtt/mqtt_infor_handle.c

@@ -76,17 +76,23 @@ void angle_mqtt_send() {
     static int angle_i=0;
     cJSON *root = cJSON_CreateObject();
     char TOPIC_send_angle[26];
-    double rounded_angle = tank_angle();
+    static double prev_angle=0;
+    static double rounded_angle;
+    rounded_angle =low_pass_filter_360(prev_angle,tank_angle(),0.7);
     sprintf(TOPIC_send_angle,"dev_rtinfo/%s",mqtt_topic_pure_number());
     cJSON_AddStringToObject(root, "type","tank_angle");
     cJSON_AddNumberToObject(root, "angle",rounded_angle);
     char *payload = cJSON_PrintUnformatted(root);
     angle_i++;
-    if(angle_i>=20){
+    if(angle_i>=10){
         my_zlog_debug("%s",payload);
+        prev_angle=tank_angle();
         angle_i=0;
     }
-    
+    if(rounded_angle == 0){
+        cJSON_Delete(root);
+        return;
+    }
     for(int i=0;i<g_mqtt_cam_config_t->mqtt_count;i++){
         mosquitto_publish(g_clients_t[i].mosq, NULL, TOPIC_send_angle, strlen(payload), payload, 0, false); 
     }