圆弧路径规划C重构

参数

• currentPos: 命名元组 Coord(x=-155.0, y=0.0, z=0.4, e=0.0)
• targetPos: [0.0, -155.0, 0.3]
• offset： [155.0, 0.0] 圆心坐标
• clockwise: True 顺时针，Fasle 逆时针
• mm_per_arc_segment： 默认值1.0，支持配置文件动态获取
• 输入和输出，和python代码的实现保持一致。

C代码实现

• 第一版v1.0

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2

#define M_PI 3.14159265358979323846


// Function to plan the arc and return the generated coordinates
double** planArc(double currentPos[3], double targetPos[3], double offset[2], int clockwise, double mm_per_arc_segment) {
    // Radius vector from center to current location
    double r_P = -offset[0];
    double r_Q = -offset[1];

    // Determine angular travel
    double center_P = currentPos[X_AXIS] - r_P;
    double center_Q = currentPos[Y_AXIS] - r_Q;
    double rt_X = targetPos[X_AXIS] - center_P;
    double rt_Y = targetPos[Y_AXIS] - center_Q;
    double angular_travel = atan2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);

    if (angular_travel < 0.0) {
        angular_travel += 2.0 * M_PI;
    }
    if (clockwise) {
        angular_travel -= 2.0 * M_PI;
    }

    if (angular_travel == 0.0 &&
        currentPos[X_AXIS] == targetPos[X_AXIS] &&
        currentPos[Y_AXIS] == targetPos[Y_AXIS]) {
        // Make a circle if the angular rotation is 0 and the target is the current position
        angular_travel = 2.0 * M_PI;
    }

    // Determine number of segments
    double linear_travel = targetPos[Z_AXIS] - currentPos[Z_AXIS];
    double radius = hypot(r_P, r_Q);
    double flat_mm = radius * angular_travel;
    double mm_of_travel = (linear_travel != 0.0) ? hypot(flat_mm, linear_travel) : fabs(flat_mm);

    int segments = (int)fmax(1.0, floor(mm_of_travel / mm_per_arc_segment));
    printf("==== Numbers: %d\n", segments);

    //*segment_count = segments;  // Store the number of segments in the output parameter

    // Allocate memory for coordinates
    double** coords = (double**)malloc((segments + 1) * sizeof(double*));
    for (int i = 0; i <= segments; i++) {
        coords[i] = (double*)malloc(3 * sizeof(double));
    }

    // Generate coordinates
    double theta_per_segment = angular_travel / segments;
    double linear_per_segment = linear_travel / segments;

    for (int i = 1; i < segments; i++) {
        double dist_Z = i * linear_per_segment;
        double cos_Ti = cos(i * theta_per_segment);
        double sin_Ti = sin(i * theta_per_segment);
        double temp_r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti;
        double temp_r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;

        coords[i][X_AXIS] = center_P + temp_r_P;
        coords[i][Y_AXIS] = center_Q + temp_r_Q;
        coords[i][Z_AXIS] = currentPos[Z_AXIS] + dist_Z;
    }

    // Add the target position as the last coordinate
    coords[segments][X_AXIS] = targetPos[X_AXIS];
    coords[segments][Y_AXIS] = targetPos[Y_AXIS];
    coords[segments][Z_AXIS] = targetPos[Z_AXIS];

    return coords;  // Return the array of coordinates
}

int main() {
    double currentPos[3] = {-155.0, 0.0, 0.4};
    double targetPos[3] = {0.0, -155.0, 0.3};
    double offset[2] = {155.0, 0.0};
    int clockwise = 0;
    double mm_per_arc_segment = 1.0;
    int segment_count = 243;  // 测试参数值

    // Plan the arc
    double** coords = planArc(currentPos, targetPos, offset, clockwise, mm_per_arc_segment);

    // Print the result
    for (int i = 1; i <= segment_count; i++) {
        printf("Segment %d: X = %.13f, Y = %.13f, Z = %.13f\n", i, coords[i][X_AXIS], coords[i][Y_AXIS], coords[i][Z_AXIS]);
    }

    // Free memory
    for (int i = 0; i <= segment_count; i++) {
        free(coords[i]);
    }
    free(coords);

    return 0;
}

• 第二版v2.0

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

#include "compiler.h"

#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2

#define M_PI 3.14159265358979323846


typedef struct {
    double** coords;
    int rows;
} arc_result;

// chailinnan add ...
// Function to plan the arc and return the generated coordinates
arc_result __visible
    planArc(double currentPos[3], double targetPos[3], double offset[2], int clockwise, double mm_per_arc_segment) {
    // Radius vector from center to current location
    double r_P = -offset[0];
    double r_Q = -offset[1];

    // Determine angular travel
    double center_P = currentPos[X_AXIS] - r_P;
    double center_Q = currentPos[Y_AXIS] - r_Q;
    double rt_X = targetPos[X_AXIS] - center_P;
    double rt_Y = targetPos[Y_AXIS] - center_Q;
    double angular_travel = atan2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y);

    if (angular_travel < 0.0) {
        angular_travel += 2.0 * M_PI;
    }
    if (clockwise) {
        angular_travel -= 2.0 * M_PI;
    }

    if (angular_travel == 0.0 &&
        currentPos[X_AXIS] == targetPos[X_AXIS] &&
        currentPos[Y_AXIS] == targetPos[Y_AXIS]) {
        // Make a circle if the angular rotation is 0 and the target is the current position
        angular_travel = 2.0 * M_PI;
    }

    // Determine number of segments
    double linear_travel = targetPos[Z_AXIS] - currentPos[Z_AXIS];
    double radius = hypot(r_P, r_Q);
    double flat_mm = radius * angular_travel;
    double mm_of_travel = (linear_travel != 0.0) ? hypot(flat_mm, linear_travel) : fabs(flat_mm);

    int segments = (int)fmax(1.0, floor(mm_of_travel / mm_per_arc_segment));
    // printf("==== Numbers: %d\n", segments);
    //*segment_count = segments;  // Store the number of segments in the output parameter

    arc_result result;
    result.rows = segments;

    // Allocate memory for coordinates
    result.coords = (double**)malloc((segments + 1) * sizeof(double*));
    for (int i = 0; i <= segments; i++) {
        result.coords[i] = (double*)malloc(3 * sizeof(double));
    }

    // Generate coordinates
    double theta_per_segment = angular_travel / segments;
    double linear_per_segment = linear_travel / segments;

    for (int i = 1; i < segments; i++) {
        double dist_Z = i * linear_per_segment;
        double cos_Ti = cos(i * theta_per_segment);
        double sin_Ti = sin(i * theta_per_segment);
        double temp_r_P = -offset[0] * cos_Ti + offset[1] * sin_Ti;
        double temp_r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;

        result.coords[i][X_AXIS] = center_P + temp_r_P;
        result.coords[i][Y_AXIS] = center_Q + temp_r_Q;
        result.coords[i][Z_AXIS] = currentPos[Z_AXIS] + dist_Z;
    }

    // Add the target position as the last coordinate
    result.coords[segments][X_AXIS] = targetPos[X_AXIS];
    result.coords[segments][Y_AXIS] = targetPos[Y_AXIS];
    result.coords[segments][Z_AXIS] = targetPos[Z_AXIS];

    return result;
}



// free arc_result memory
void __visible free_arc_result(arc_result result) {
    for (int i = 0; i <= result.rows; i++) {
        free(result.coords[i]);
    }
    free(result.coords);
}

• __visible 关键词，表示编译后对外部可见，支持外部调用

编译

• **init.py **里面需要把添加的c代码模块，定义进去，然后进行编译
• /home/pi/klippy-env/bin/python init.py
  • 对添加的c代码进行编译

python 调用

from cffi import FFI

ffi = FFI()
ffi.cdef("""
    double* planArc(...);  // C函数的声明
    void free(void*);  // C中释放内存的函数
""")

C = ffi.dlopen("your_c_library.so")

# 使用ffi.gc绑定返回的指针和C的free函数
coords = ffi.gc(C.planArc(...), C.free)

# 使用coords，Python会在coords不再被引用时自动调用C.free

• python 调用示例，释放内存资源

ffi_main, ffi_lib = chelper.get_ffi()
coords = ffi_main.gc(ffi_lib.planArc(...),
                     ffi_lib.free)

• 具体调用实现

测试

import chelper
from gcode import Coord

def test_v2():
    currentPos = Coord(x=-155.0, y=0.0, z=0.4, e=0.0)

    ffi_main, ffi_lib = chelper.get_ffi()
    arc_result = ffi_main.gc(ffi_lib.planArc(currentPos, [0.0, -155.0, 0.3], [155.0, 0.0],
                                             False, 1.0),
                             ffi_lib.free_arc_result)

    print(arc_result.coords)
    print(arc_result.rows)

    rows = arc_result.rows
    coords = arc_result.coords

    coords_list = [coords[i] for i in range(1, rows + 1)]

    numeric_values = [[ptr[0], ptr[1], ptr[2]] for ptr in coords_list]  # 读取每个指针指向的 double 值
    print(numeric_values)
    print(len(numeric_values))