होम स्थिति सेंसर कैलिब्रेशन नोट

//       
static TSumSensorsData 	g_sens_data[2];
static int32_t   	g_sens_data_sum_cnt[2];
static uint8_t		g_sens_data_num;

//  - ,     
IS_INTERRUPT void on_dma_raw_ready_calibrate_step1()
{
	SensorRawBuffer *raw = sensor_get_raw_buffer();
	g_sens_data[g_sens_data_num].acc_x += swap_i16(raw->accell_x_unswap);
	g_sens_data[g_sens_data_num].acc_y += swap_i16(raw->accell_y_unswap);
	g_sens_data[g_sens_data_num].acc_z += swap_i16(raw->accell_z_unswap);
	g_sens_data[g_sens_data_num].gyro_x += swap_i16(raw->gyro_x_unswap);
	g_sens_data[g_sens_data_num].gyro_y += swap_i16(raw->gyro_y_unswap);
	g_sens_data[g_sens_data_num].gyro_z += swap_i16(raw->gyro_z_unswap);
	g_sens_data[g_sens_data_num].mag_x += raw->mag_x_raw * g_mag_calibrate.kx;
	g_sens_data[g_sens_data_num].mag_y += raw->mag_y_raw * g_mag_calibrate.ky;
	g_sens_data[g_sens_data_num].mag_z += raw->mag_z_raw * g_mag_calibrate.kz;
	g_sens_data_sum_cnt[g_sens_data_num]++;
}

//   main
void sensors_calibrate_program(FlashROM *flash_ptr)
{
	double calibrate_result_error[3];
	TVector16 calibrate_result[3];
	int32_t radius[ACCEL_NO_MOTION_DETECT_COUNT];
	uint8_t raw_is_deleted[ACCEL_NO_MOTION_DETECT_COUNT];
	TVector16 raw[3][ACCEL_NO_MOTION_DETECT_COUNT];
	
        . . .

	//  
	g_sens_data_sum_cnt[0] = 0;
	g_sens_data_num = 0;
	int16_t prev_avg_x = 0;
	int16_t prev_avg_y = 0;
	int16_t prev_avg_z = 0;
	int8_t low_motion_cnt = 0;

	while(low_motion_cnt < ACCEL_NO_MOTION_DETECT_COUNT)
	{
		if (g_sens_data_sum_cnt[g_sens_data_num] >= ACCEL_NO_MOTION_DETECT_SAMPLES)
		{
			uint8_t new_data_num = (g_sens_data_num + 1) & 1;
			g_sens_data[new_data_num].acc_x = 0;
			g_sens_data[new_data_num].acc_y = 0;
			g_sens_data[new_data_num].acc_z = 0;
			g_sens_data[new_data_num].gyro_x = 0;
			g_sens_data[new_data_num].gyro_y = 0;
			g_sens_data[new_data_num].gyro_z = 0;
			g_sens_data[new_data_num].mag_x = 0;
			g_sens_data[new_data_num].mag_y = 0;
			g_sens_data[new_data_num].mag_z = 0;
			g_sens_data_sum_cnt[new_data_num] = 0;

			uint8_t old_data_num = g_sens_data_num;
			g_sens_data_num = new_data_num; //           
			// ( -    ,   )

			//     -  
			int16_t avg_x = g_sens_data[old_data_num].acc_x / g_sens_data_sum_cnt[old_data_num];
			int16_t avg_y = g_sens_data[old_data_num].acc_y / g_sens_data_sum_cnt[old_data_num];
			int16_t avg_z = g_sens_data[old_data_num].acc_z / g_sens_data_sum_cnt[old_data_num];

			//      
			int16_t dx = avg_x - prev_avg_x;
			int16_t dy = avg_y - prev_avg_y;
			int16_t dz = avg_z - prev_avg_z;
			prev_avg_x = avg_x;
			prev_avg_y = avg_y;
			prev_avg_z = avg_z;

			//     
			if ((abs_i16(dx) <= ACCEL_NO_MOTION_DETECT_AVG_VALUE)&&(abs_i16(dy) <= ACCEL_NO_MOTION_DETECT_AVG_VALUE)&&(abs_i16(dz) <= ACCEL_NO_MOTION_DETECT_AVG_VALUE))
			{
				//    
				raw[RAW_ACC][low_motion_cnt].x = avg_x;
				raw[RAW_ACC][low_motion_cnt].y = avg_y;
				raw[RAW_ACC][low_motion_cnt].z = avg_z;
				raw[RAW_GYRO][low_motion_cnt].x = g_sens_data[old_data_num].gyro_x / g_sens_data_sum_cnt[old_data_num];
				raw[RAW_GYRO][low_motion_cnt].y = g_sens_data[old_data_num].gyro_y / g_sens_data_sum_cnt[old_data_num];
				raw[RAW_GYRO][low_motion_cnt].z = g_sens_data[old_data_num].gyro_z / g_sens_data_sum_cnt[old_data_num];
				raw[RAW_MAG][low_motion_cnt].x = g_sens_data[old_data_num].mag_x / g_sens_data_sum_cnt[old_data_num];
				raw[RAW_MAG][low_motion_cnt].y = g_sens_data[old_data_num].mag_y / g_sens_data_sum_cnt[old_data_num];
				raw[RAW_MAG][low_motion_cnt].z = g_sens_data[old_data_num].mag_z / g_sens_data_sum_cnt[old_data_num];

				low_motion_cnt++;

				//   
				beep();

				//     2   ,     -   
				//  -  
				//      
				delay_ms(2000);
			}
		}
	}
. . .
}

Public Function get_err(A As Double, B As Double, C As Double, R As Double) As Double
Dim x, y, z As Double
Dim sigma As Double
Dim row_n As Long
get_err = 0
For row_n = 1 To 15
    x = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 1).Value
    y = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 2).Value
    z = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 3).Value
    get_err = get_err + abs( (A - x) ^ 2 + (B - y) ^ 2 + (C - z) ^ 2 - R ^ 2 )
Next
End Function

. . .
A = 0
B = 0
C = 0

Do While True
   min_sigma = 0
    For ai = -1 To 1
        For bi = -1 To 1
            For ci = -1 To 1
                sigma = get_err(A + ai, B + bi, C + ci, 16384)
                If sigma < min_sigma Or min_sigma = 0 Then
                    ai_min = ai
                    bi_min = bi
                    ci_min = ci
                    min_sigma = sigma
                End If
            Next
        Next
    Next
    
    If ai_min = 0 And bi_min = 0 And ci_min = 0 Then
        Exit Do
    End If
    
    A = A + ai_min
    B = B + bi_min
    C = C + ci_min
Loop
. . .

Public Function get_err(A As Double, B As Double, C As Double, R As Double) As Double
Dim x, y, z As Double
Dim sigma As Double
Dim row_n As Long
get_err = 0
For row_n = 1 To 15
    x = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 1).Value
    y = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 2).Value
    z = Application.ActiveWorkbook.ActiveSheet.Cells(row_n, 3).Value
    get_err = get_err + abs( (A - x) ^ 2 + (B - y) ^ 2 + (C - z) ^ 2 - R ^ 2 )
Next
End Function
. . .
A = 0
B = 0
C = 0
step = 1000
Do While True
   min_sigma = 0
    For ai = -1 To 1
        For bi = -1 To 1
            For ci = -1 To 1
                sigma = get_err(A + ai * step, B + bi * step, C + ci * step, 16384)
                If sigma < min_sigma Or min_sigma = 0 Then
                    ai_min = ai
                    bi_min = bi
                    ci_min = ci
                    min_sigma = sigma
                End If
            Next
        Next
    Next
    If ai_min = 0 And bi_min = 0 And ci_min = 0 Then        
        step = step / 10
        If step < 0.01 Then
            Exit Do
        End If
    Else
    A = A + ai_min * step
    B = B + bi_min * step
    C = C + ci_min * step
    End If
Loop
. . .