添加航向差速控制算法

coordinate_convert/c/studio_proj_c.h

@@ -24,6 +24,20 @@
 #define WGS84_F_C 1 / INVF_C                                    // 扁率
 #define WGS84_E2_C ((2 * WGS84_F_C) - (WGS84_F_C * WGS84_F_C))  // 第一偏心扁率的平方
 
+
+/* 中央经线计算规范说明 ------------------------------------------------------
+ * + 3度分带法 (适用于1:1万~1:50万地图)
+ *   - 理论:  3度带中央经线=3度带带号*3.0 , 带号= 3度带带号=(经度+1.5°)/3取整
+ *   - 带号计算: int zone_num = floor((经度 + 1.5°) / 3.0);
+ *   - 中央经线: double central = floor((经度 + 1.5°) / 3.0) * 3.0;
+ *
+ * + 6度分带法 (适用于1:1万以下小比例地图)
+ *   - 理论:  6度带中央经线=6度带带号*6.0 - 3.0 , 带号= 6度带带号=(经度+6°)/6取整
+ *   - 带号计算: zone_num = floor((经度 + 6°) / 6.0);
+ *   - 中央经线: double central = floor((经度 + 6°) / 6.0) * 6.0 - 3.0;
+ * ----------------------------------------------------------------*/
+
+
 /// 经纬度 转 高斯克吕格投影
 /// \param central 中央经线 (度)
 /// \param lon 经度 (度)

heading_diff_control/heading_diff_ctrl_c.c

@@ -0,0 +1,117 @@
+/**
+ ******************************************************************************
+ * @file           : heading_diff_ctrl_c.cpp
+ * @author         : wangyingjie
+ * @brief          : None
+ * @attention      : None
+ * @date           : 2025/5/23
+ ******************************************************************************
+ */
+
+#include "heading_diff_ctrl_c.h"
+
+
+//// 小船控制结构体
+//typedef struct
+//{
+//    studio_pid heading_pid;  // 方位角PID控制器
+//    studio_point_c target;   // 目标点
+//    double base_throttle;    // 基础油门
+//    double left_throttle;    // 左侧油门
+//    double right_throttle;   // 右侧油门
+//} diff_ctrl;
+
+// 初始化小船控制器
+void init_diff_ctrl(diff_ctrl *controller,
+                          const studio_point_c *target,
+                          double base_throttle,  // 输入范围0~1.0
+                          double Kp,
+                          double Ki,
+                          double Kd)
+{
+    init_pid(&controller->heading_pid, 0, 0, Kp, Ki, Kd, 1.0, 0.3);
+    controller->target = *target;
+    controller->base_throttle = base_throttle;
+    controller->left_throttle = base_throttle;
+    controller->right_throttle = base_throttle;
+}
+
+void reset_diff_ctrl_goal(diff_ctrl* controller, const studio_point_c* target)
+{
+    controller->target = *target;
+    set_goal(&controller->heading_pid, 0);
+}
+
+// 正确计算最短路径误差（返回-180~180°）
+double calculate_heading_error(double target_deg, double current_deg)
+{
+    double error = target_deg - current_deg;
+    error = fmod(error + 360.0, 360.0);  // 转0~360°
+    if (error > 180.0)
+    {
+        error -= 360.0;
+    }
+    return error;
+}
+
+double update_boat(diff_ctrl *controller, const studio_point_c *current_pos, double current_heading_deg)
+{
+    // 1. 计算目标方位角（确保函数正确）
+    const double target_deg = azimuth_angle_gcs_c(current_pos, &controller->target);
+
+    // 2. 计算最短路径误差
+    double error = calculate_heading_error(target_deg, current_heading_deg);
+    double steer = 0.0;
+
+    // 3. PID控制
+    if (fabs(error) > 1.0)  // 仅当误差超过1度时进行纠偏
+    {
+        set_goal(&controller->heading_pid, 0);
+        steer = compute_pid(&controller->heading_pid, error);
+    }
+    else
+    {
+
+    }
+
+    // 4. 差速控制（关键修正）
+    controller->left_throttle = controller->base_throttle - steer;
+    controller->right_throttle = controller->base_throttle + steer;
+
+    // 5. 限幅保护
+    controller->left_throttle = fmax(0.0, fmin(1.0, controller->left_throttle));
+    controller->right_throttle = fmax(0.0, fmin(1.0, controller->right_throttle));
+
+    return distance_squared_gcs_c(current_pos, &controller->target);
+}
+
+void simulate_movement(studio_point_c *current_pos, double *current_heading, double left_throttle, double right_throttle)
+{
+    const double dt = 0.1;         // 时间间隔（秒）
+    const double max_speed = 2;  // 最大速度（米/秒）
+    const double turn_rate = 40;  // 转向速率（度/秒 per 差速百分比）
+
+    // 保存原始位置用于计算位移
+    const studio_point_c original_pos = *current_pos;
+
+    // 1. 计算航向变化 ---------------------------------------------
+    double steer_diff = left_throttle - right_throttle;
+    *current_heading += turn_rate * steer_diff * dt;
+    *current_heading = fmod(*current_heading + 360.0, 360.0);
+
+    // 2. 计算位移量（米）------------------------------------------
+    double avg_throttle = (left_throttle + right_throttle) / 2.0;
+    double speed = max_speed * avg_throttle;
+
+    // 将航向角转换为弧度
+    double radian = *current_heading * AO_M_PI / 180.0;
+
+    // 计算东向和北向位移（单位：米）
+    const double delta_east = speed * sin(radian) * dt;   // 东向位移（+东/-西）
+    const double delta_north = speed * cos(radian) * dt;  // 北向位移（+北/-南）
+
+    // 3. 通过投影转换更新位置 -------------------------------------
+    *current_pos = lonlat_move_by(&original_pos, delta_east, delta_north);
+}
+
+

heading_diff_control/heading_diff_ctrl_c.h

@@ -0,0 +1,62 @@
+/**
+ ******************************************************************************
+ * @file           : heading_diff_ctrl_c.cpp
+ * @author         : wangyingjie
+ * @brief          : None
+ * @attention      : None
+ * @date           : 2025/5/23
+ ******************************************************************************
+ */
+
+#ifndef HEADING_DIFF_CTRL_H
+#define HEADING_DIFF_CTRL_H
+
+//#ifdef __cplusplus
+//extern "C"
+//{
+//#endif
+
+#include "control/pid/studio_pid_c.h"
+#include "geography/studio_proj_c.h"
+#include "geometry/studio_geo_algo_c.h"
+
+//#ifdef __cplusplus
+//}
+//#endif
+
+// 测试使用
+
+//#include "geometry/studio_geo_utils.h"
+//#include "geometry/studio_geo_algo.h"
+
+// 测试使用结束
+
+// 小船控制结构体
+typedef struct
+{
+    studio_pid heading_pid;  // 方位角PID控制器
+    studio_point_c target;   // 目标点
+    double base_throttle;    // 基础油门
+    double left_throttle;    // 左侧油门
+    double right_throttle;   // 右侧油门
+} diff_ctrl;
+
+// 初始化小船控制器
+void init_diff_ctrl(diff_ctrl *controller,
+                    const studio_point_c *target,
+                    double base_throttle,  // 输入范围0~1.0
+                    double Kp,
+                    double Ki,
+                    double Kd);
+
+void reset_diff_ctrl_goal(diff_ctrl *controller, const studio_point_c *target);
+
+// 正确计算最短路径误差（返回-180~180°）
+double calculate_heading_error(double target_deg, double current_deg);
+
+double update_boat(diff_ctrl *controller, const studio_point_c *current_pos, double current_heading_deg);
+
+void simulate_movement(studio_point_c *current_pos, double *current_heading, double left_throttle, double right_throttle);
+
+
+#endif  // HEADING_DIFF_CTRL_H

heading_diff_control/test_diff_ctrl.cpp

@@ -0,0 +1,142 @@
+/**
+ ******************************************************************************
+ * @file           : test_diff_ctrl.cpp
+ * @author         : wangyingjie
+ * @brief          : None
+ * @attention      : None
+ * @date           : 2025/5/23
+ ******************************************************************************
+ */
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "nzzn/diff_ctrl_c/heading_diff_ctrl_c.h"
+#include "control/pid/studio_pid_c.h"
+#include "geography/studio_proj_c.h"
+#include "geometry/studio_geo_algo_c.h"
+
+#ifdef __cplusplus
+}
+#endif
+
+// 测试使用
+
+#include "geometry/studio_geo_utils.h"
+#include "geometry/studio_geo_algo.h"
+
+// 测试使用结束
+
+int main()
+{
+    double central_1 = floor(120.07158158 / 3.0) * 3.0;
+    double central_2 = static_cast<int>(120.07158158 / 3) * 3;
+    double central_3 = floor((120.07158158 + 1.5) / 3.0) * 3.0;
+    printf("===================== %s =====================\n", __FILE__);
+
+    silly::geo::utils::init_gdal_env();
+    std::string path;
+#ifdef IS_LINUX
+    path = "/home/wyj/myself/2_data/2_geojson/multi_point/route_1.geojson";
+#else
+    path = "D:/9_data/2_readfile/geojson/multi_point/route_3.geojson";
+#endif
+    std::cout << "path: " << path << std::endl;
+
+    std::vector<studio_geo_coll> res_collections;
+    //studio_geo_coll temp_300;
+    //temp_300.m_type = enum_geometry_type::egtLineString;
+    //studio_geo_coll temp_100;
+    //temp_100.m_type = enum_geometry_type::egtLineString;
+
+    // 差速控制测试开始 ==================
+
+    studio_point_c beg_p = studio_point_init(120.07158158, 36.15932277);
+    studio_point_c end_p = studio_point_init(120.07193922, 36.15990152);
+    studio_point_c change_1_p = studio_point_init(120.07232455, 36.15964342);
+    studio_point_c change_2_p = studio_point_init(120.07215543, 36.15944710);
+
+    // 测试记录 实际删去 --------beg--------
+    studio_geo_coll beg_end_points;
+    beg_end_points.m_type = enum_geometry_type::egtMultiPoint;
+    beg_end_points.m_points.push_back(studio_point(beg_p.x, beg_p.y));
+    beg_end_points.m_points.push_back(studio_point(end_p.x, end_p.y));
+    beg_end_points.m_points.push_back(studio_point(change_1_p.x, change_1_p.y));
+    beg_end_points.m_points.push_back(studio_point(change_2_p.x, change_2_p.y));
+    //temp_300.m_line.push_back(studio_point(beg_p.x, beg_p.y));
+    //temp_100.m_line.push_back(studio_point(beg_p.x, beg_p.y));
+    // 测试记录 实际删去 --------end--------
+
+    int count = 800;  // 最大迭代次数
+
+    const int heads = 2;
+    double current_heading[heads] = {300.0, 100.0};
+    for (int i = 0; i < heads; ++i)
+    {
+        studio_geo_coll temp_line;
+        temp_line.m_type = enum_geometry_type::egtLineString;
+        temp_line.m_points.push_back(studio_point(beg_p.x, beg_p.y));
+
+        diff_ctrl ctrl;
+        init_diff_ctrl(&ctrl, &end_p, 0.5, 0.8, 0.01, 0.2);
+        studio_point_c current_pos = beg_p;
+        double current_heading_deg = current_heading[i];
+        for (int j = 0; j < count; ++j)
+        {
+            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);
+
+            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);
+
+            if (dist_sq < 4.0)
+            {
+                printf("到达目标！\n");
+                break;
+            }
+
+            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
+            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
+        }
+
+        // 前往先一个点
+        reset_diff_ctrl_goal(&ctrl, &change_1_p);
+        for (int j = 0; j < count; ++j)
+        {
+            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);
+            
+            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);
+
+            if (dist_sq < 4.0)
+            {
+                printf("到达目标！\n");
+                break;
+            }
+            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
+            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
+        }
+
+        // 前往第二个点
+        reset_diff_ctrl_goal(&ctrl, &change_2_p);
+        for (int j = 0; j < count; ++j)
+        {
+            double dist_sq = update_boat(&ctrl, &current_pos, current_heading_deg);
+
+            printf("Step %02d: 航向=%.1f° L=%.2f R=%.2f 距离²=%.1fm²\n", j + 1, current_heading_deg, ctrl.left_throttle, ctrl.right_throttle, dist_sq);
+            if (dist_sq < 4.0)
+            {
+                printf("到达目标！\n");
+                break;
+            }
+            simulate_movement(&current_pos, &current_heading_deg, ctrl.left_throttle, ctrl.right_throttle);
+            temp_line.m_line.push_back(studio_point(current_pos.x, current_pos.y));
+        }
+        res_collections.push_back(temp_line);
+    }
+
+    res_collections.push_back(beg_end_points);
+    silly::geo::utils::write_geo_coll(path, res_collections);
+    silly::geo::utils::destroy_gdal_env();
+
+    return 0;
+}

line_vacuate/studio_geo_algo_c.c

@@ -11,8 +11,9 @@
 #include "studio_geo_algo_c.h"
 #include "geography/studio_proj_c.h"
 
+/////////////////// 基础适量算法 ///////////////////
 
-double distance_squared(const studio_point_c *p1, const studio_point_c *p2)
+double distance_squared_c(const studio_point_c *p1, const studio_point_c *p2)
 {
     const double dx = p1->x - p2->x;
     const double dy = p1->y - p2->y;
@@ -50,7 +51,103 @@ double point_line_dist_square_c(const studio_point_c *p, const studio_point_c *s
     return dist_square;
 }
 
-// Douglas-Peucker算法，用于简化多边形或曲线
+
+double azimuth_angle_c(const studio_point_c *p_from, const studio_point_c *p_to)
+{
+    double delta_x = p_to->x - p_from->x;
+    double delta_y = p_to->y - p_from->y;
+
+    double theta_rad = atan2(delta_x, delta_y);
+    double theta_deg = theta_rad * (180.0 / AO_M_PI);
+
+    if (theta_deg < 0.0)
+    {
+        theta_deg += 360.0;
+    }
+    return theta_deg;
+}
+
+
+double distance_squared_gcs_c(const studio_point_c *p1, const studio_point_c *p2)
+{
+    // 经纬度转高斯投影
+    // double central = ((int) (p1->x) / 3) * 3;
+    double central = floor((p1->x + 1.5) / 3.0) * 3.0;
+    double gx_1, gy_1;
+    lonlat_to_gauss(central, p1->x, p1->y, &gx_1, &gy_1);
+    studio_point_c gs_p1 = studio_point_init(gx_1, gy_1);
+    double gx_2, gy_2;
+    lonlat_to_gauss(central, p2->x, p2->y, &gx_2, &gy_2);
+    studio_point_c gs_p2 = studio_point_init(gx_2, gy_2);
+    double dist_square = distance_squared_c(&gs_p1, &gs_p2);
+    return dist_square;
+}
+
+double point_line_dist_square_gcs_c(const studio_point_c *p, const studio_point_c *start, const studio_point_c *end)
+{
+    // 经纬度转高斯投影
+    // double central = ((int) (p->x) / 3) * 3;
+    double central = floor((p->x + 1.5) / 3.0) * 3.0;
+    double gx_p, gy_p;
+    lonlat_to_gauss(central, p->x, p->y, &gx_p, &gy_p);
+    studio_point_c gs_p = studio_point_init(gx_p, gy_p);
+    double gx_start, gy_start;
+    lonlat_to_gauss(central, start->x, start->y, &gx_start, &gy_start);
+    studio_point_c gs_start = studio_point_init(gx_start, gy_start);
+    double gx_end, gy_end;
+    lonlat_to_gauss(central, end->x, end->y, &gx_end, &gy_end);
+    studio_point_c gs_end = studio_point_init(gx_end, gy_end);
+    double dist_square = point_line_dist_square_c(&gs_p, &gs_start, &gs_end);
+    return dist_square;
+}
+
+double azimuth_angle_gcs_c(const studio_point_c *p_from, const studio_point_c *p_to)
+{
+    // 经纬度转高斯投影
+    // double central = ((int) (p_from->x) / 3) * 3;
+    double central = floor((p_from->x + 1.5) / 3.0) * 3.0;
+    double gx_from, gy_from;
+    lonlat_to_gauss(central, p_from->x, p_from->y, &gx_from, &gy_from);
+    studio_point_c gs_from = studio_point_init(gx_from, gy_from);
+    double gx_to, gy_to;
+    lonlat_to_gauss(central, p_to->x, p_to->y, &gx_to, &gy_to);
+    studio_point_c gs_to = studio_point_init(gx_to, gy_to);
+    double theta_deg = azimuth_angle_c(&gs_from, &gs_to);
+    return theta_deg;
+}
+
+studio_point_c lonlat_move_by(const studio_point_c *pt, double delta_x, double delta_y)
+{
+    // 中央经线
+    double central = floor((pt->x + 1.5) / 3.0) * 3.0;
+
+    // 转换为高斯平面坐标
+    double gx, gy;
+    lonlat_to_gauss(central, pt->x, pt->y, &gx, &gy);
+
+    // 应用平面坐标偏移（东向加delta_x，北向加delta_y）
+    const double new_gx = gx + delta_x; // 单位：米
+    const double new_gy = gy + delta_y;
+
+    // 转回经纬度
+    double new_lon, new_lat;
+    gauss_to_lonlat(central, new_gx, new_gy, &new_lon, &new_lat);
+
+    // 经度范围修正（-180~180）
+    new_lon = fmod(new_lon + 360.0, 360.0);  // 先转0~360
+    if (new_lon > 180.0)
+    {
+        new_lon -= 360.0;  // 再转-180~180
+    }
+    studio_point_c new_point = studio_point_init(new_lon, new_lat);
+
+    return new_point;
+}
+
+
+/////////////////// 矢量(线)抽稀算法 ///////////////////
+
+
 void douglas_peucker_c(const studio_line_c *points, size_t start, size_t end, double epsilon, unsigned int *indices,
                        unsigned int *index_count)
 {
@@ -204,7 +301,8 @@ bool line_vacuate_gcs_c(const studio_line_c *line, const int max_points, const d
     }
     // 经纬度转高斯克吕格投影
     studio_line_c gauss_line = studio_line_c_init();
-    double central = (int) (line->data[0].x) / 3 * 3;
+    // double central = ((int) (line->data[0].x) / 3) * 3;
+    double central = floor((line->data[0].x + 1.5) / 3.0) * 3.0;
     for (unsigned int i = 0; i < line->size; i++)
     {
         double gx, gy;
@@ -245,7 +343,8 @@ bool remove_outliers_c(const studio_line_c *line, double distance, studio_line_c
     double dist_square = distance * distance;
     // 经纬度转高斯克吕格投影
     studio_line_c gauss_line = studio_line_c_init();
-    double central = (int) (line->data[0].x) / 3 * 3;
+    // double central = ((int) (line->data[0].x) / 3) * 3;
+    double central = floor((line->data[0].x + 1.5) / 3.0) * 3.0;
     for (unsigned int i = 0; i < line->size; i++)
     {
         double gx, gy;
@@ -264,7 +363,7 @@ bool remove_outliers_c(const studio_line_c *line, double distance, studio_line_c
     studio_line_c_add_point(&gs_filtered_line, gauss_line.data[0]); // 保留第一个点
     for (unsigned int i = 1; i < gauss_line.size; i++) // 遍历相邻点对
     {
-        double d_sq = distance_squared(&gauss_line.data[i - 1], &gauss_line.data[i]);
+        double d_sq = distance_squared_c(&gauss_line.data[i - 1], &gauss_line.data[i]);
         if (d_sq <= dist_square)
         {
             studio_line_c_add_point(&gs_filtered_line, gauss_line.data[i]);

line_vacuate/studio_geo_algo_c.h

@@ -1,33 +1,73 @@
 /**
- ******************************************************************************
- * @file           : studio_geo_algo_c.h
- * @author         : wangyingjie
- * @brief          : C 语言版 几何算法库： DP算法
- * @attention      : None
- * @date           : 2025/5/11
- ******************************************************************************
- */
+******************************************************************************
+* @file           : studio_geo_algo_c.h
+* @author         : wangyingjie
+* @brief          : C 语言版几何算法库，包含常用的空间计算、线段抽稀、去除离群点等几何操作算法。
+*                   提供多种几何计算函数，如计算点之间的距离、方位角、点到线段的最短距离、以及线段的抽稀处理（Douglas-Peucker算法等）。
+*                   适用于笛卡尔坐标系和大地坐标系（经纬度）的应用场景。
+* @attention      : None
+* @date           : 2025/5/11
+******************************************************************************
+*/
 
 #ifndef STUDIO_GEO_ALGO_C_H
 #define STUDIO_GEO_ALGO_C_H
 
 #include "studio_geo_c.h"
 
-/////////////////// 矢量(线)抽稀算法 ///////////////////
+/////////////////// 基础适量算法 ///////////////////
 
-/// 计算两点之间的距离的平方
+/// 计算两点之间的距离的平方 (笛卡尔坐标系)
 /// @param p1
 /// @param p2
 /// @return
-double distance_squared(const studio_point_c *p1, const studio_point_c *p2);
+double distance_squared_c(const studio_point_c *p1, const studio_point_c *p2);
 
-/// 计算点到线段的距离的平方
+/// 计算点到线段的距离的平方 (笛卡尔坐标系)
 /// \param p 点
 /// \param start 线段起点
 /// \param end 线段终点
-/// \return 距离的平方
+/// \return 距离的平方 （单位 米）
 double point_line_dist_square_c(const studio_point_c *p, const studio_point_c *start, const studio_point_c *end);
 
+/// 计算两点之间的方位角 (笛卡尔坐标系)
+/// 方位角是从点 p_from 指向点 p_to 的向量与正北方向的顺时针夹角，单位为度
+/// @param p_from
+/// @param p_to
+/// @return
+double azimuth_angle_c(const studio_point_c *p_from, const studio_point_c *p_to);
+
+/// 计算两个经纬度之间的距离的平方 (大地坐标系)
+/// @param p1
+/// @param p2
+/// @return 距离的平方 （单位 米）
+double distance_squared_gcs_c(const studio_point_c *p1, const studio_point_c *p2);
+
+/// 计算经纬度点到经纬度线的距离的平方 (大地坐标系)
+/// \param p 点
+/// \param start 线段起点
+/// \param end 线段终点
+/// \return 距离的平方 （单位 米）
+double point_line_dist_square_gcs_c(const studio_point_c *p, const studio_point_c *start, const studio_point_c *end);
+
+/// 计算两点之间的方位角 (大地坐标系)
+/// 方位角是从点 p_from 指向点 p_to 的向量与正北方向的顺时针夹角，单位为度
+/// @param p_from
+/// @param p_to
+/// @return
+double azimuth_angle_gcs_c(const studio_point_c *p_from, const studio_point_c *p_to);
+
+/// <summary>
+/// 计算经纬度点移动一定距离后的新点
+/// </summary>
+/// <param name="pt"></param>
+/// <param name="delta_x">X方向移动距离，单位为米</param>
+/// <param name="delta_y">Y方向移动距离，单位为米</param>
+/// <returns></returns>
+studio_point_c lonlat_move_by(const studio_point_c *pt, double delta_x, double delta_y);
+
+/////////////////// 矢量(线)抽稀算法 ///////////////////
+
 /// Douglas-Peucker算法，线段压缩
 /// \param points 线段
 /// \param start 线段起点索引
@@ -35,8 +75,7 @@ double point_line_dist_square_c(const studio_point_c *p, const studio_point_c *s
 /// \param epsilon 抽稀容差,若某点到当前线段的最大垂直距离超过epsilon，则保留该点,单位与适量坐标系的单位一致
 /// \param indices 存储简化后的点索引
 /// \param index_count 存储简化后的点索引数量
-void douglas_peucker_c(const studio_line_c *points, size_t start, size_t end, double epsilon, unsigned int *indices,
-                       unsigned int *index_count);
+void douglas_peucker_c(const studio_line_c *points, size_t start, size_t end, double epsilon, unsigned int *indices, unsigned int *index_count);
 
 /// 矢量线段抽稀算法
 /// \param line 原始线段
@@ -52,9 +91,7 @@ bool line_vacuate_c(const studio_line_c *line, const int max_points, const doubl
 /// @param epsilon 抽稀容差,若某点到当前线段的最大垂直距离超过epsilon，这里单位是米
 /// @param vacuate_line 抽稀后的线段
 /// @return
-bool line_vacuate_gcs_c(const studio_line_c *line, const int max_points, const double epsilon,
-                        studio_line_c *vacuate_line);
-
+bool line_vacuate_gcs_c(const studio_line_c *line, const int max_points, const double epsilon, studio_line_c *vacuate_line);
 
 /////////////////// 移除线段中的离群点 ///////////////////
 
@@ -69,7 +106,6 @@ unsigned int hash(unsigned int value, unsigned int size);
 /// \param size 点索引数量
 void remove_duplicates(unsigned int **indices, unsigned int *size);
 
-
 /// 移除线段中的离群点
 /// @param line 原始线段
 /// @param distance 离群点距离
@@ -77,5 +113,4 @@ void remove_duplicates(unsigned int **indices, unsigned int *size);
 /// @return
 bool remove_outliers_c(const studio_line_c *line, double distance, studio_line_c *outliers_line);
 
-
 #endif  // STUDIO_GEO_ALGO_C_H

pid_c/main.cpp

@@ -1,29 +1,39 @@
 
 # include "my_lib.h"
-# include "pid_c.h"
+# include "studio_pid_c.h"
+
 
 
 int main()
 {
-    // 创建PID控制器实例
+    double ini = 2, goal = 50, p = 0.5, i = 0.001, d = 0.6;
     studio_pid pid;
 
-    // 初始化PID参数
-    double ini = 2, goal = 50, p = 0.5, i = 0.001, d = 0.6;
-    studio_pid_init(&pid, ini, goal, p, i, d);
+    // 初始化PID
+    init_pid(&pid, ini, goal, p, i, d, 100, 200);
 
-    // 模拟控制循环
+    // 第一阶段控制到 50
     double current_value = ini;
     for (int i = 0; i < 20; ++i)
     {
-        // 计算PID输出
-        double output = studio_pid_compute(&pid, current_value);
-        // 更新系统状态(这里简单地将输出加到当前值上)
-        current_value += output;
-        std::cout << "Step " << i + 1 << ": 目标=" << goal << ", 当前值=" << current_value << ", 输出=" << output << std::endl;
+        double output = compute_pid(&pid, current_value);
+        current_value += output; // 积分项和微分项会影响下一次计算
+        printf("Step %d: 目标=%.2f, 当前值=%.2f, 输出=%.2f\n", i + 1, get_goal(&pid), current_value, output);
+    }
+
+    printf("到达目标值，开始第二阶段控制\n");
+    // 改变目标值并重置PID状态
+    set_goal(&pid, 100);
+
+    // 第二阶段控制到 100
+    for (int i = 0; i < 20; ++i)
+    {
+        double output = compute_pid(&pid, current_value);
+        current_value += output; // 积分项和微分项会影响下一次计算
+        printf("Step %d: 目标=%.2f, 当前值=%.2f, 输出=%.2f\n", i + 1, get_goal(&pid), current_value, output);
     }
 
-	return 0; 
+    return 0;
 }
 
 

pid_c/pid_c.h

@@ -1,78 +0,0 @@
-#ifndef PID_C_H
-#define PID_C_H
-
-#include <stdint.h>
-
-// PID控制器结构体
-typedef struct
-{
-    double initval;     // 初始值
-    double goal;        // 目标值
-    double error;       // 当前误差
-    uint32_t pid_step;  // 步数计数器
-    double Kp;          // 比例常数
-    double Ki;          // 积分常数
-    double Kd;          // 微分常数
-    double prev_error;  // 前一个误差
-    double inte_error;  // 积分误差
-    double dif_error;   // 微分误差
-} studio_pid;
-
-// 初始化PID控制器
-void studio_pid_init(studio_pid *pid, double init_v, double goal_v, double p, double i, double d)
-{
-    pid->initval = init_v;
-    pid->goal = goal_v;
-    pid->Kp = p;
-    pid->Ki = i;
-    pid->Kd = d;
-    pid->error = goal_v - init_v;
-    pid->pid_step = 1;
-    pid->prev_error = 0;
-    pid->inte_error = 0;
-    pid->dif_error = 0;
-}
-
-// 计算PID输出
-double studio_pid_compute(studio_pid *pid, double current_value)
-{
-    // 计算当前误差
-    double cte = pid->goal - current_value;
-
-    // 更新误差项
-    if (pid->pid_step == 1)
-    {
-        pid->prev_error = cte;
-    }
-
-    pid->inte_error += cte;
-    pid->dif_error = cte - pid->prev_error;
-    pid->prev_error = cte;
-
-    ++pid->pid_step;
-
-    // 计算并返回PID输出
-    return pid->Kp * cte + pid->Ki * pid->inte_error + pid->Kd * pid->dif_error;
-}
-
-// 获取目标值
-double studio_pid_get_goal(const studio_pid *pid)
-{
-    return pid->goal;
-}
-
-// 设置目标值
-void studio_pid_set_goal(studio_pid *pid, double goal_v)
-{
-    pid->goal = goal_v;
-}
-
-// 设置PID参数
-void studio_pid_set_params(studio_pid *pid, double p, double i, double d)
-{
-    pid->Kp = p;
-    pid->Ki = i;
-    pid->Kd = d;
-}
-
-#endif

pid_c/studio_pid_c.c

@@ -0,0 +1,102 @@
+/**
+  ******************************************************************************
+  * @file           : studio_pid_c.c
+  * @author         : wangyingjie
+  * @brief          : None
+  * @attention      : None
+  * @date           : 2025/5/23
+  ******************************************************************************
+  */
+
+#include "studio_pid_c.h"
+
+
+// 初始化PID结构体
+void init_pid(studio_pid *pid, double init_v, double goal_v, double p, double i, double d,
+              double integral_limit_val, double output_limit_val)
+{
+    pid->initval = init_v;
+    pid->goal = goal_v;
+    pid->Kp = p;
+    pid->Ki = i;
+    pid->Kd = d;
+    pid->error = goal_v - init_v;
+    pid->prev_error = pid->error;
+    pid->inte_error = 0.0;
+    pid->dif_error = 0.0;
+    pid->integral_limit = integral_limit_val;
+    pid->output_limit = output_limit_val;
+}
+
+// 计算PID输出
+double compute_pid(studio_pid *pid, double current_value)
+{
+    // 计算当前误差 CTE
+    double cte = pid->goal - current_value;
+
+    // 更新积分和微分项
+    pid->inte_error += cte; // 积分项
+    pid->dif_error = cte - pid->prev_error; // 微分项
+    pid->prev_error = cte;
+
+    // 应用积分限幅
+    if (pid->inte_error > pid->integral_limit)
+    {
+        pid->inte_error = pid->integral_limit;
+    } else if (pid->inte_error < -pid->integral_limit)
+    {
+        pid->inte_error = -pid->integral_limit;
+    }
+    // 计算PID输出
+    double output = pid->Kp * cte + pid->Ki * pid->inte_error + pid->Kd * pid->dif_error;
+    // 应用输出限幅
+    if (output > pid->output_limit)
+    {
+        output = pid->output_limit;
+    } else if (output < -pid->output_limit)
+    {
+        output = -pid->output_limit;
+    }
+    return output;
+}
+
+// 设置PID参数
+void set_pid_params(studio_pid *pid, double p, double i, double d)
+{
+    pid->Kp = p;
+    pid->Ki = i;
+    pid->Kd = d;
+}
+
+// 设置积分限幅
+void set_integral_limit(studio_pid *pid, double limit)
+{
+    pid->integral_limit = limit;
+}
+
+// 设置输出限幅
+void set_output_limit(studio_pid *pid, double limit)
+{
+    pid->output_limit = limit;
+}
+
+// 获取目标值
+double get_goal(const studio_pid *pid)
+{
+    return pid->goal;
+}
+
+// 重置PID状态
+void reset_pid(studio_pid *pid)
+{
+    pid->prev_error = pid->goal - pid->initval;
+    pid->inte_error = 0.0;
+    pid->dif_error = 0.0;
+}
+
+// 设置目标值并重置PID状态
+void set_goal(studio_pid *pid, double goal_v)
+{
+    pid->goal = goal_v;
+    reset_pid(pid);
+}

pid_c/studio_pid_c.h

@@ -0,0 +1,56 @@
+/**
+******************************************************************************
+  * @file           : studio_pid_c.h
+  * @author         : wangyingjie
+  * @brief          : None
+  * @attention      : None
+  * @date           : 2025/5/23
+  ******************************************************************************
+  */
+#ifndef STUDIO_PID_C_H
+#define STUDIO_PID_C_H
+
+#include <stdint.h>
+
+typedef struct
+{
+    double initval; // 初始值
+    double goal; // 目标值
+    double error; // 当前误差（目标值 - 当前值）
+    double Kp; // 比例常数
+    double Ki; // 积分常数
+    double Kd; // 微分常数
+    double prev_error; // 前一个误差
+    double inte_error; // 积分误差
+    double dif_error; // 微分误差
+    double integral_limit; // 积分项限幅值
+    double output_limit; // 输出限幅值
+} studio_pid;
+
+// 初始化PID结构体
+void init_pid(studio_pid *pid, double init_v, double goal_v, double p, double i, double d,
+              double integral_limit_val, double output_limit_val);
+
+// 计算PID输出
+double compute_pid(studio_pid *pid, double current_value);
+
+// 设置PID参数
+void set_pid_params(studio_pid *pid, double p, double i, double d);
+
+// 设置积分限幅
+void set_integral_limit(studio_pid *pid, double limit);
+
+// 设置输出限幅
+void set_output_limit(studio_pid *pid, double limit);
+
+// 获取目标值
+double get_goal(const studio_pid *pid);
+
+// 重置PID状态
+void reset_pid(studio_pid *pid);
+
+// 设置目标值并重置PID状态
+void set_goal(studio_pid *pid, double goal_v);
+
+
+#endif