1 /** 2 * Self Test application for Invensense's MPU6050/MPU6500/MPU9150. 3 */ 4 5 #include <unistd.h> 6 #include <dirent.h> 7 #include <fcntl.h> 8 #include <stdio.h> 9 #include <errno.h> 10 #include <sys/stat.h> 11 #include <stdlib.h> 12 #include <features.h> 13 #include <dirent.h> 14 #include <string.h> 15 #include <poll.h> 16 #include <stddef.h> 17 #include <linux/input.h> 18 #include <time.h> 19 #include <linux/time.h> 20 21 #include "invensense.h" 22 #include "ml_math_func.h" 23 #include "storage_manager.h" 24 #include "ml_stored_data.h" 25 #include "ml_sysfs_helper.h" 26 27 #ifndef ABS 28 #define ABS(x)(((x) >= 0) ? (x) : -(x)) 29 #endif 30 31 //#define DEBUG_PRINT /* Uncomment to print Gyro & Accel read from Driver */ 32 33 #define MAX_SYSFS_NAME_LEN (100) 34 #define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) 35 36 /** Change this key if the data being stored by this file changes */ 37 #define INV_DB_SAVE_KEY 53395 38 39 #define FALSE 0 40 #define TRUE 1 41 42 #define GYRO_PASS_STATUS_BIT 0x01 43 #define ACCEL_PASS_STATUS_BIT 0x02 44 #define COMPASS_PASS_STATUS_BIT 0x04 45 46 typedef union { 47 long l; 48 int i; 49 } bias_dtype; 50 51 char *sysfs_names_ptr; 52 53 struct sysfs_attrbs { 54 char *enable; 55 char *power_state; 56 char *dmp_on; 57 char *dmp_int_on; 58 char *self_test; 59 char *temperature; 60 char *gyro_enable; 61 char *gyro_x_bias; 62 char *gyro_y_bias; 63 char *gyro_z_bias; 64 char *accel_enable; 65 char *accel_x_bias; 66 char *accel_y_bias; 67 char *accel_z_bias; 68 char *compass_enable; 69 } mpu; 70 71 struct inv_db_save_t { 72 /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ 73 long compass_bias[3]; 74 /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ 75 long gyro_bias[3]; 76 /** Temperature when *gyro_bias was stored. */ 77 long gyro_temp; 78 /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ 79 long accel_bias[3]; 80 /** Temperature when accel bias was stored. */ 81 long accel_temp; 82 long gyro_temp_slope[3]; 83 /** Sensor Accuracy */ 84 int gyro_accuracy; 85 int accel_accuracy; 86 int compass_accuracy; 87 }; 88 89 static struct inv_db_save_t save_data; 90 91 /** This function receives the data that was stored in non-volatile memory 92 between power off */ 93 static inv_error_t inv_db_load_func(const unsigned char *data) 94 { 95 memcpy(&save_data, data, sizeof(save_data)); 96 return INV_SUCCESS; 97 } 98 99 /** This function returns the data to be stored in non-volatile memory between 100 power off */ 101 static inv_error_t inv_db_save_func(unsigned char *data) 102 { 103 memcpy(data, &save_data, sizeof(save_data)); 104 return INV_SUCCESS; 105 } 106 107 /** read a sysfs entry that represents an integer */ 108 int read_sysfs_int(char *filename, int *var) 109 { 110 int res=0; 111 FILE *fp; 112 113 fp = fopen(filename, "r"); 114 if (fp != NULL) { 115 fscanf(fp, "%d\n", var); 116 fclose(fp); 117 } else { 118 MPL_LOGE("inv_self_test: ERR open file to read"); 119 res= -1; 120 } 121 return res; 122 } 123 124 /** write a sysfs entry that represents an integer */ 125 int write_sysfs_int(char *filename, int data) 126 { 127 int res=0; 128 FILE *fp; 129 130 fp = fopen(filename, "w"); 131 if (fp!=NULL) { 132 fprintf(fp, "%d\n", data); 133 fclose(fp); 134 } else { 135 MPL_LOGE("inv_self_test: ERR open file to write"); 136 res= -1; 137 } 138 return res; 139 } 140 141 int inv_init_sysfs_attributes(void) 142 { 143 unsigned char i = 0; 144 char sysfs_path[MAX_SYSFS_NAME_LEN]; 145 char *sptr; 146 char **dptr; 147 148 sysfs_names_ptr = 149 (char*)malloc(sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); 150 sptr = sysfs_names_ptr; 151 if (sptr != NULL) { 152 dptr = (char**)&mpu; 153 do { 154 *dptr++ = sptr; 155 sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); 156 } while (++i < MAX_SYSFS_ATTRB); 157 } else { 158 MPL_LOGE("inv_self_test: couldn't alloc mem for sysfs paths"); 159 return -1; 160 } 161 162 // get proper (in absolute/relative) IIO path & build MPU's sysfs paths 163 inv_get_sysfs_path(sysfs_path); 164 165 sprintf(mpu.enable, "%s%s", sysfs_path, "/buffer/enable"); 166 sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); 167 sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on"); 168 sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test"); 169 sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); 170 171 sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); 172 sprintf(mpu.gyro_x_bias, "%s%s", sysfs_path, "/in_anglvel_x_calibbias"); 173 sprintf(mpu.gyro_y_bias, "%s%s", sysfs_path, "/in_anglvel_y_calibbias"); 174 sprintf(mpu.gyro_z_bias, "%s%s", sysfs_path, "/in_anglvel_z_calibbias"); 175 176 sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); 177 sprintf(mpu.accel_x_bias, "%s%s", sysfs_path, "/in_accel_x_calibbias"); 178 sprintf(mpu.accel_y_bias, "%s%s", sysfs_path, "/in_accel_y_calibbias"); 179 sprintf(mpu.accel_z_bias, "%s%s", sysfs_path, "/in_accel_z_calibbias"); 180 181 sprintf(mpu.compass_enable, "%s%s", sysfs_path, "/compass_enable"); 182 183 #if 0 184 // test print sysfs paths 185 dptr = (char**)&mpu; 186 for (i = 0; i < MAX_SYSFS_ATTRB; i++) { 187 MPL_LOGE("inv_self_test: sysfs path: %s", *dptr++); 188 } 189 #endif 190 return 0; 191 } 192 193 /******************************************************************************* 194 * M a i n S e l f T e s t 195 ******************************************************************************/ 196 int main(int argc, char **argv) 197 { 198 FILE *fptr; 199 int self_test_status = 0; 200 inv_error_t result; 201 bias_dtype gyro_bias[3]; 202 bias_dtype accel_bias[3]; 203 int axis = 0; 204 size_t packet_sz; 205 int axis_sign = 1; 206 unsigned char *buffer; 207 long timestamp; 208 int temperature = 0; 209 bool compass_present = TRUE; 210 211 result = inv_init_sysfs_attributes(); 212 if (result) 213 return -1; 214 215 inv_init_storage_manager(); 216 217 // Clear out data. 218 memset(&save_data, 0, sizeof(save_data)); 219 memset(gyro_bias, 0, sizeof(gyro_bias)); 220 memset(accel_bias, 0, sizeof(accel_bias)); 221 222 // Register packet to be saved. 223 result = inv_register_load_store( 224 inv_db_load_func, inv_db_save_func, 225 sizeof(save_data), INV_DB_SAVE_KEY); 226 227 // Power ON MPUxxxx chip 228 if (write_sysfs_int(mpu.power_state, 1) < 0) { 229 printf("Self-Test:ERR-Failed to set power state=1\n"); 230 } else { 231 // Note: Driver turns on power automatically when self-test invoked 232 } 233 234 // Disable Master enable 235 if (write_sysfs_int(mpu.enable, 0) < 0) { 236 printf("Self-Test:ERR-Failed to disable master enable\n"); 237 } 238 239 // Disable DMP 240 if (write_sysfs_int(mpu.dmp_on, 0) < 0) { 241 printf("Self-Test:ERR-Failed to disable DMP\n"); 242 } 243 244 // Enable Accel 245 if (write_sysfs_int(mpu.accel_enable, 1) < 0) { 246 printf("Self-Test:ERR-Failed to enable accel\n"); 247 } 248 249 // Enable Gyro 250 if (write_sysfs_int(mpu.gyro_enable, 1) < 0) { 251 printf("Self-Test:ERR-Failed to enable gyro\n"); 252 } 253 254 // Enable Compass 255 if (write_sysfs_int(mpu.compass_enable, 1) < 0) { 256 #ifdef DEBUG_PRINT 257 printf("Self-Test:ERR-Failed to enable compass\n"); 258 #endif 259 compass_present= FALSE; 260 } 261 262 fptr = fopen(mpu.self_test, "r"); 263 if (!fptr) { 264 printf("Self-Test:ERR-Couldn't invoke self-test\n"); 265 result = -1; 266 goto free_sysfs_storage; 267 } 268 269 // Invoke self-test 270 fscanf(fptr, "%d", &self_test_status); 271 if (compass_present == TRUE) { 272 printf("Self-Test:Self test result- " 273 "Gyro passed= %x, Accel passed= %x, Compass passed= %x\n", 274 (self_test_status & GYRO_PASS_STATUS_BIT), 275 (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1, 276 (self_test_status & COMPASS_PASS_STATUS_BIT) >> 2); 277 } else { 278 printf("Self-Test:Self test result- " 279 "Gyro passed= %x, Accel passed= %x\n", 280 (self_test_status & GYRO_PASS_STATUS_BIT), 281 (self_test_status & ACCEL_PASS_STATUS_BIT) >> 1); 282 } 283 fclose(fptr); 284 285 if (self_test_status & GYRO_PASS_STATUS_BIT) { 286 // Read Gyro Bias 287 if (read_sysfs_int(mpu.gyro_x_bias, &gyro_bias[0].i) < 0 || 288 read_sysfs_int(mpu.gyro_y_bias, &gyro_bias[1].i) < 0 || 289 read_sysfs_int(mpu.gyro_z_bias, &gyro_bias[2].i) < 0) { 290 memset(gyro_bias, 0, sizeof(gyro_bias)); 291 printf("Self-Test:Failed to read Gyro bias\n"); 292 } else { 293 save_data.gyro_accuracy = 3; 294 #ifdef DEBUG_PRINT 295 printf("Self-Test:Gyro bias[0..2]= [%d %d %d]\n", 296 gyro_bias[0].i, gyro_bias[1].i, gyro_bias[2].i); 297 #endif 298 } 299 } else { 300 printf("Self-Test:Failed Gyro self-test\n"); 301 } 302 303 if (self_test_status & ACCEL_PASS_STATUS_BIT) { 304 // Read Accel Bias 305 if (read_sysfs_int(mpu.accel_x_bias, &accel_bias[0].i) < 0 || 306 read_sysfs_int(mpu.accel_y_bias, &accel_bias[1].i) < 0 || 307 read_sysfs_int(mpu.accel_z_bias, &accel_bias[2].i) < 0) { 308 memset(accel_bias,0, sizeof(accel_bias)); 309 printf("Self-Test:Failed to read Accel bias\n"); 310 } else { 311 save_data.accel_accuracy = 3; 312 #ifdef DEBUG_PRINT 313 printf("Self-Test:Accel bias[0..2]= [%d %d %d]\n", 314 accel_bias[0].i, accel_bias[1].i, accel_bias[2].i); 315 #endif 316 } 317 } else { 318 printf("Self-Test:Failed Accel self-test\n"); 319 } 320 321 if (!(self_test_status & (GYRO_PASS_STATUS_BIT | ACCEL_PASS_STATUS_BIT))) { 322 printf("Self-Test:Failed Gyro and Accel self-test, " 323 "nothing left to do\n"); 324 result = -1; 325 goto free_sysfs_storage; 326 } 327 328 // Read temperature 329 fptr= fopen(mpu.temperature, "r"); 330 if (fptr != NULL) { 331 fscanf(fptr,"%d %ld", &temperature, ×tamp); 332 fclose(fptr); 333 } else { 334 printf("Self-Test:ERR-Couldn't read temperature\n"); 335 } 336 337 // When we read gyro bias, the bias is in raw units scaled by 1000. 338 // We store the bias in raw units scaled by 2^16 339 save_data.gyro_bias[0] = (long)(gyro_bias[0].l * 65536.f / 8000.f); 340 save_data.gyro_bias[1] = (long)(gyro_bias[1].l * 65536.f / 8000.f); 341 save_data.gyro_bias[2] = (long)(gyro_bias[2].l * 65536.f / 8000.f); 342 343 // Save temperature @ time stored. 344 // Temperature is in degrees Celsius scaled by 2^16 345 save_data.gyro_temp = temperature * (1L << 16); 346 save_data.accel_temp = save_data.gyro_temp; 347 348 // When we read accel bias, the bias is in raw units scaled by 1000. 349 // and it contains the gravity vector. 350 351 // Find the orientation of the device, by looking for gravity 352 if (ABS(accel_bias[1].l) > ABS(accel_bias[0].l)) { 353 axis = 1; 354 } 355 if (ABS(accel_bias[2].l) > ABS(accel_bias[axis].l)) { 356 axis = 2; 357 } 358 if (accel_bias[axis].l < 0) { 359 axis_sign = -1; 360 } 361 362 // Remove gravity, gravity in raw units should be 16384 for a 2g setting. 363 // We read data scaled by 1000, so 364 accel_bias[axis].l -= axis_sign * 4096L * 1000L; 365 366 // Convert scaling from raw units scaled by 1000 to raw scaled by 2^16 367 save_data.accel_bias[0] = (long)(accel_bias[0].l * 65536.f / 1000.f * 4.f); 368 save_data.accel_bias[1] = (long)(accel_bias[1].l * 65536.f / 1000.f * 4.f); 369 save_data.accel_bias[2] = (long)(accel_bias[2].l * 65536.f / 1000.f * 4.f); 370 371 #if 1 372 printf("Self-Test:Saved Accel bias[0..2]= [%ld %ld %ld]\n", 373 save_data.accel_bias[0], save_data.accel_bias[1], 374 save_data.accel_bias[2]); 375 printf("Self-Test:Saved Gyro bias[0..2]= [%ld %ld %ld]\n", 376 save_data.gyro_bias[0], save_data.gyro_bias[1], 377 save_data.gyro_bias[2]); 378 printf("Self-Test:Gyro temperature @ time stored %ld\n", 379 save_data.gyro_temp); 380 printf("Self-Test:Accel temperature @ time stored %ld\n", 381 save_data.accel_temp); 382 #endif 383 384 // Get size of packet to store. 385 inv_get_mpl_state_size(&packet_sz); 386 387 // Create place to store data 388 buffer = (unsigned char *)malloc(packet_sz + 10); 389 if (buffer == NULL) { 390 printf("Self-Test:Can't allocate buffer\n"); 391 result = -1; 392 goto free_sysfs_storage; 393 } 394 395 // Store the data 396 result = inv_save_mpl_states(buffer, packet_sz); 397 if (result) { 398 result = -1; 399 } else { 400 fptr= fopen(MLCAL_FILE, "wb+"); 401 if (fptr != NULL) { 402 fwrite(buffer, 1, packet_sz, fptr); 403 fclose(fptr); 404 } else { 405 printf("Self-Test:ERR- Can't open calibration file to write - %s\n", 406 MLCAL_FILE); 407 result = -1; 408 } 409 } 410 411 free(buffer); 412 413 free_sysfs_storage: 414 free(sysfs_names_ptr); 415 return result; 416 } 417 418
