WhyCan Forum(哇酷开发者社区)

#include <rtthread.h>

struct calibration {
    int x[5];
    int y[5];
    int xfb[5];
    int yfb[5];
    int a[7];
};
struct calibration cal;
int perform_calibration(struct calibration *cal) {
    int j;
    float n, x, y, x2, y2, xy, z, zx, zy;
    float det, a, b, c, e, f, i;
    float scaling = 65536.0;
// Get sums for matrix
    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]);
    }
// Get determinant of matrix -- check if determinant is too small
    det = n*(x2*y2 - xy*xy) + x*(xy*y - x*y2) + y*(x*xy - y*x2);
    if(det < 0.1 && det > -0.1) {
        rt_kprintf("ts_calibrate: determinant is too small -- %f\n",det);
        return 0;
    }
// Get elements of inverse matrix
    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;
// Get sums for x calibration
    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]);
    }
// Now multiply out to get the calibration for framebuffer x coord
    cal->a[0] = (int)((a*z + b*zx + c*zy)*(scaling));
    cal->a[1] = (int)((b*z + e*zx + f*zy)*(scaling));
    cal->a[2] = (int)((c*z + f*zx + i*zy)*(scaling));
   // rt_kprintf("%f %f %f\n",(a*z + b*zx + c*zy),(b*z + e*zx + f*zy),(c*z + f*zx + i*zy));
// Get sums for y calibration
    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]);
    }
// Now multiply out to get the calibration for framebuffer y coord
    cal->a[3] = (int)((a*z + b*zx + c*zy)*(scaling));
    cal->a[4] = (int)((b*z + e*zx + f*zy)*(scaling));
    cal->a[5] = (int)((c*z + f*zx + i*zy)*(scaling));
    //rt_kprintf("%f %f %f\n",(a*z + b*zx + c*zy),(b*z + e*zx + f*zy),(c*z + f*zx + i*zy));
// If we got here, we're OK, so assign scaling to a[6] and return
    cal->a[6] = (int)scaling;
    /*校准参数*/
    rt_kprintf("%d %d %d %d %d %d %d\n",cal->a[0],cal->a[1],cal->a[2],cal->a[3],
cal->a[4],cal->a[5],(int)cal->a[6]);

    /*测试下效果 x=2062 y=2109*/
    rt_kprintf("x %d\n", (cal->a[0] + cal->a[1]*2062 + cal->a[2]*2109 ) / cal->a[6]);
    rt_kprintf("y %d\n", (cal->a[3] + cal->a[4]*2062 + cal->a[5]*2109 ) / cal->a[6]);

    
    return 1;
    
}
int tscal(void)
{
    /*TP 5点采集值*/
    cal.x[0] = 677;
    cal.y[0] = 1032;
    cal.x[1] = 3431;
    cal.y[1] = 1040;
    cal.x[2] = 3432;
    cal.y[2] = 3209;
    cal.x[3] = 672;
    cal.y[3] = 3172;
    cal.x[4] = 2062;
    cal.y[4] = 2109;
    /*期望对应到哪些点*/
    cal.xfb[0] = 100;
    cal.yfb[0] = 100;
    cal.xfb[1] = 700;
    cal.yfb[1] = 100;
    cal.xfb[2] = 700;
    cal.yfb[2] = 380;
    cal.xfb[3] = 100;
    cal.yfb[3] = 380;
    cal.xfb[4] = 400;
    cal.yfb[4] = 240;
    perform_calibration(&cal);
    return 0;
}

MSH_CMD_EXPORT(tscal,tscal);