WhyCan Forum(哇酷开发者社区)

struct tscal_t {
	int x[5], xfb[5];
	int y[5], yfb[5];
	int a[7];
	int index;
};

static int perform_calibration(struct tscal_t * cal)
{
	float n, x, y, x2, y2, xy, z, zx, zy;
	float det, a, b, c, e, f, i;
	float scaling = 65536.0;
	int j;

	n = x = y = x2 = y2 = xy = 0;
	for(j = 0; j < 5; j++)
	{
		n += 1.0;
		x += (float)cal->x[j];
		y += (float)cal->y[j];
		x2 += (float)(cal->x[j]*cal->x[j]);
		y2 += (float)(cal->y[j]*cal->y[j]);
		xy += (float)(cal->x[j]*cal->y[j]);
	}

	det = n * (x2 * y2 - xy * xy) + x * (xy * y - x * y2) + y * (x * xy - y * x2);
	if(det < 0.1 && det > -0.1)
		return 0;

	a = (x2 * y2 - xy * xy) / det;
	b = (xy * y - x * y2) / det;
	c = (x * xy - y * x2) / det;
	e = (n * y2 - y * y) / det;
	f = (x * y - n * xy) / det;
	i = (n * x2 - x * x) / det;

	z = zx = zy = 0;
	for(j = 0; j < 5; j++)
	{
		z += (float)cal->xfb[j];
		zx += (float)(cal->xfb[j] * cal->x[j]);
		zy += (float)(cal->xfb[j] * cal->y[j]);
	}
	cal->a[0] = (int)((b * z + e * zx + f * zy) * (scaling));
	cal->a[1] = (int)((c * z + f * zx + i * zy) * (scaling));
	cal->a[2] = (int)((a * z + b * zx + c * zy) * (scaling));

	z = zx = zy = 0;
	for(j = 0; j < 5; j++)
	{
		z += (float)cal->yfb[j];
		zx += (float)(cal->yfb[j] * cal->x[j]);
		zy += (float)(cal->yfb[j] * cal->y[j]);
	}
	cal->a[3] = (int)((b * z + e * zx + f * zy) * (scaling));
	cal->a[4] = (int)((c * z + f * zx + i * zy) * (scaling));
	cal->a[5] = (int)((a * z + b * zx + c * zy) * (scaling));
	cal->a[6] = (int)scaling;

	return 1;
}

	"ts-ns2009@0": {
		"i2c-bus": "i2c-v3s.0",
		"slave-address": 72,
		"median-filter-length": 5,
		"mean-filter-length": 5,
		"calibration": [14052, 21, -2411064, -67, 8461, -1219628, 65536],
		"poll-interval-ms": 10
	},

[1, 0, 0, 0, 1, 0, 1]

	*x = (filter->cal[2] + filter->cal[0] * tx + filter->cal[1] * ty) / filter->cal[6];
	*y = (filter->cal[5] + filter->cal[3] * tx + filter->cal[4] * ty) / filter->cal[6];