grbl源码解析——圆弧插补

uint8_tgc_execute_line(char *line)函数，对G02，G03代码进行解析，目的是求出圆心坐标。

接着调用mc_arc函数进行后续数据处理。

// position == current xyz, target == target xyz,
// offset == offset from current xyz, axis_X defines circle plane in tool space, axis_linear is
// the direction of helical travel, radius == circle radius, isclockwise boolean. Used for vector transformation direction.
void mc_arc(float *target, plan_line_data_t *pl_data, float *position, float *offset, float radius,
  uint8_t axis_0, uint8_t axis_1, uint8_t axis_linear, uint8_t is_clockwise_arc)
{
  float center_axis0 = position[axis_0] + offset[axis_0];
  float center_axis1 = position[axis_1] + offset[axis_1];
  float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
  float r_axis1 = -offset[axis_1];
  float rt_axis0 = target[axis_0] - center_axis0;
  float rt_axis1 = target[axis_1] - center_axis1;

  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  if (is_clockwise_arc) { // Correct atan2 output per direction
    if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= 2*M_PI; }
  } else {
    if (angular_travel <= ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel += 2*M_PI; }
  }

这部分代码的作用是求解angular_travel的大小，即圆心指向起点向量与圆心指向终点向量间的夹角。根据数学知识，设向量a(x0,y0),b(x1,y1),向量a，b的夹角θ=atan2（x0y1-y0x1,x0x1+y0y1），angular_travel为向量r、向量rt的夹角。

  uint16_t segments = floor(fabs(0.5*angular_travel*radius)/sqrt(settings.arc_tolerance*(2*radius - settings.arc_tolerance)) );

mc_arc函数的原理就是将圆弧插补成足够多的小线段。arc_tolerance为圆弧上任意两点连接而成的小线段到圆弧的最大距离，该值是人为设定的。arc_tolerance越小，则圆弧微分成的小线段越多，插补精度也越高。由上图可知该段圆弧可以插补成segments个小线段。

float theta_per_segment = angular_travel/segments;//每个线段对应的圆心角
float linear_per_segment = (target[axis_linear] - position[axis_linear])/segments;// axis_linear，除了圆弧平面之外的第三个轴，即与圆弧平面垂直的轴
float cos_T = 2.0 - theta_per_segment*theta_per_segment;
float sin_T = theta_per_segment*0.16666667*(cos_T + 4.0);
cos_T *= 0.5;

三角函数的泰勒展开公式如下：

    float sin_Ti;
    float cos_Ti;
    float r_axisi;
    uint16_t i;
    uint8_t count = 0;

    for (i = 1; i<segments; i++) { // Increment (segments-1).

      if (count < N_ARC_CORRECTION) {
        // Apply vector rotation matrix. ~40 usec
        r_axisi = r_axis0*sin_T + r_axis1*cos_T;
        r_axis0 = r_axis0*cos_T - r_axis1*sin_T;
        r_axis1 = r_axisi;
        count++;
      } else {
        // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. ~375 usec
        // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
        cos_Ti = cos(i*theta_per_segment);
        sin_Ti = sin(i*theta_per_segment);
        r_axis0 = -offset[axis_0]*cos_Ti + offset[axis_1]*sin_Ti;
        r_axis1 = -offset[axis_0]*sin_Ti - offset[axis_1]*cos_Ti;
        count = 0;
      }

      // Update arc_target location
      position[axis_0] = center_axis0 + r_axis0;
      position[axis_1] = center_axis1 + r_axis1;
      position[axis_linear] += linear_per_segment;

      mc_line(position, pl_data);

      // Bail mid-circle on system abort. Runtime command check already performed by mc_line.
      if (sys.abort) { return; }
    }

圆的参数方程公式如下：

if (count < N_ARC_CORRECTION) 中的Pi坐标的求法如下：

else中的Pi坐标的求法如下：

按精度来说，else中的求法精度高，但是要进行sin、cos运算，速度会慢些。因此每隔N_ARC_CORRECTION次才采用else中的算法求解Pi位置，可以提高运算速度，同时也能消除前几次通过泰勒展开式求解导致的累计误差。当圆弧足够小时，segments可能小于N_ARC_CORRECTION，用泰勒展开式求解位置速度快，同时累计误差不会变的很大。
mc_arc的最后就是调用mc_line函数。后续将涉及到速度前瞻运算。