int smb380_read_accel_avg(int num_avg, smb380acc_t *min, smb380acc_t *max,
smb380acc_t *avg)
{
long x_avg = 0, y_avg = 0, z_avg = 0;
int comres = 0;
int i;
smb380acc_t accel; /* read accel data */
x_avg = 0;
y_avg = 0;
z_avg = 0;
max->x = -512;
max->y = -512;
max->z = -512;
min->x = 512;
min->y = 512;
min->z = 512;
for (i = 0; i < num_avg; i++) {
comres += smb380_read_accel_xyz(&accel); /* read 10 acceleration data triples */
if (accel.x > max->x)
max->x = accel.x;
if (accel.x < min->x)
min->x = accel.x;
if (accel.y > max->y)
max->y = accel.y;
if (accel.y < min->y)
min->y = accel.y;
if (accel.z > max->z)
max->z = accel.z;
if (accel.z < min->z)
min->z = accel.z;
x_avg += accel.x;
y_avg += accel.y;
z_avg += accel.z;
smb380_pause(10);
}
avg->x = x_avg /= num_avg; /* calculate averages, min and max values */
avg->y = y_avg /= num_avg;
avg->z = z_avg /= num_avg;
return comres;
}
int smb380_calibrate(smb380acc_t orientation, int *tries)
{
unsigned short offset_x, offset_y, offset_z;
unsigned short old_offset_x, old_offset_y, old_offset_z;
int need_calibration=0, min_max_ok=0;
int ltries;
smb380acc_t min,max,avg;
smb380_set_range(SMB380_RANGE_2G);
smb380_set_bandwidth(SMB380_BW_25HZ);
smb380_set_ee_w(1);
smb380_get_offset(0, &offset_x);
smb380_get_offset(1, &offset_y);
smb380_get_offset(2, &offset_z);
old_offset_x = offset_x;
old_offset_y = offset_y;
old_offset_z = offset_z;
ltries = *tries;
do {
smb380_read_accel_avg(10, &min, &max, &avg); /* read acceleration data min, max, avg */
min_max_ok = smb380_verify_min_max(min, max, avg);
/* check if calibration is needed */
if (min_max_ok)
need_calibration = smb380_calc_new_offset(orientation, avg, &offset_x, &offset_y, &offset_z);
if (*tries==0) /*number of maximum tries reached? */
break;
if (need_calibration) {
/* when needed calibration is updated in image */
(*tries)--;
smb380_set_offset(0, offset_x);
smb380_set_offset(1, offset_y);
smb380_set_offset(2, offset_z);
smb380_pause(20);
}
} while (need_calibration || !min_max_ok);
if (*tries>0 && *tries < ltries) {
if (old_offset_x!= offset_x)
smb380_set_offset_eeprom(0, offset_x);
if (old_offset_y!= offset_y)
smb380_set_offset_eeprom(1,offset_y);
if (old_offset_z!= offset_z)
smb380_set_offset_eeprom(2, offset_z);
}
smb380_set_ee_w(0);
smb380_pause(20);
*tries = ltries - *tries;
return !need_calibration;
}
