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; }