Lines Matching refs:calData
146 * @param calData
153 inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len)
169 (unsigned short)calData[6] * 256 + calData[7]) / 65536.f;
172 newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]);
176 newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]);
180 newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]);
250 * @param calData
257 inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len)
275 (unsigned short)calData[6] * 256 + calData[7]) / 65536.f;
278 newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]);
282 newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]);
286 newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]);
327 accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]);
333 accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]);
339 accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]);
381 * @param calData
388 inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len)
408 inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
409 inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
410 inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
411 inv_obj.temp_ptrs[i] += (int)calData[ptr++];
416 inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
417 inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
418 inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
419 inv_obj.temp_valid_data[i] += (int)calData[ptr++];
427 tmp += 16777216LL * (long long)calData[ptr++];
428 tmp += 65536LL * (long long)calData[ptr++];
429 tmp += 256LL * (long long)calData[ptr++];
430 tmp += (long long)calData[ptr++];
443 calData[ptr++];
444 tmp += 65536LL * (long long)calData[ptr++];
445 tmp += 256LL * (long long)calData[ptr++];
446 tmp += (long long)calData[ptr++];
458 tmp += 16777216LL * (long long)calData[ptr++];
459 tmp += 65536LL * (long long)calData[ptr++];
460 tmp += 256LL * (long long)calData[ptr++];
461 tmp += (long long)calData[ptr++];
473 tmp += 16777216LL * (long long)calData[ptr++];
474 tmp += 65536LL * (long long)calData[ptr++];
475 tmp += 256LL * (long long)calData[ptr++];
476 tmp += (long long)calData[ptr++];
489 t += 16777216UL * ((uint32_t) calData[ptr++]);
490 t += 65536UL * ((uint32_t) calData[ptr++]);
491 t += 256u * ((uint32_t) calData[ptr++]);
492 t += (uint32_t) calData[ptr++];
535 * @param calData
542 inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len)
566 inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
567 inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
568 inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
569 inv_obj.temp_ptrs[i] += (int)calData[ptr++];
574 inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
575 inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
576 inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
577 inv_obj.temp_valid_data[i] += (int)calData[ptr++];
585 tmp += 16777216LL * (long long)calData[ptr++];
586 tmp += 65536LL * (long long)calData[ptr++];
587 tmp += 256LL * (long long)calData[ptr++];
588 tmp += (long long)calData[ptr++];
601 tmp += 16777216LL * (long long)calData[ptr++];
602 tmp += 65536LL * (long long)calData[ptr++];
603 tmp += 256LL * (long long)calData[ptr++];
604 tmp += (long long)calData[ptr++];
616 tmp += 16777216LL * (long long)calData[ptr++];
617 tmp += 65536LL * (long long)calData[ptr++];
618 tmp += 256LL * (long long)calData[ptr++];
619 tmp += (long long)calData[ptr++];
631 tmp += 16777216LL * (long long)calData[ptr++];
632 tmp += 65536LL * (long long)calData[ptr++];
633 tmp += 256LL * (long long)calData[ptr++];
634 tmp += (long long)calData[ptr++];
647 t += 16777216UL * ((uint32_t) calData[ptr++]);
648 t += 65536UL * ((uint32_t) calData[ptr++]);
649 t += 256u * ((uint32_t) calData[ptr++]);
650 t += (uint32_t) calData[ptr++];
660 inv_obj.got_compass_bias = (int)calData[ptr++];
661 inv_obj.got_init_compass_bias = (int)calData[ptr++];
662 inv_obj.compass_state = (int)calData[ptr++];
666 t += 16777216UL * ((uint32_t) calData[ptr++]);
667 t += 65536UL * ((uint32_t) calData[ptr++]);
668 t += 256u * ((uint32_t) calData[ptr++]);
669 t += (uint32_t) calData[ptr++];
676 t += 16777216UL * ((uint32_t) calData[ptr++]);
677 t += 65536UL * ((uint32_t) calData[ptr++]);
678 t += 256u * ((uint32_t) calData[ptr++]);
679 t += (uint32_t) calData[ptr++];
686 t += 16777216UL * ((uint32_t) calData[ptr++]);
687 t += 65536UL * ((uint32_t) calData[ptr++]);
688 t += 256u * ((uint32_t) calData[ptr++]);
689 t += (uint32_t) calData[ptr++];
695 t += 16777216UL * ((uint32_t) calData[ptr++]);
696 t += 65536UL * ((uint32_t) calData[ptr++]);
697 t += 256u * ((uint32_t) calData[ptr++]);
698 t += (uint32_t) calData[ptr++];
704 dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
705 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
706 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
707 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
708 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
709 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
710 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
711 dToLL.ll += (unsigned long long)calData[ptr++];
718 dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
719 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
720 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
721 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
722 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
723 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
724 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
725 dToLL.ll += (unsigned long long)calData[ptr++];
765 * @param calData
772 inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len)
797 inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]);
798 inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]);
799 inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]);
800 inv_obj.temp_ptrs[i] += (int)calData[ptr++];
805 inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]);
806 inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]);
807 inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]);
808 inv_obj.temp_valid_data[i] += (int)calData[ptr++];
816 tmp += 16777216LL * (long long)calData[ptr++];
817 tmp += 65536LL * (long long)calData[ptr++];
818 tmp += 256LL * (long long)calData[ptr++];
819 tmp += (long long)calData[ptr++];
832 tmp += 16777216LL * (long long)calData[ptr++];
833 tmp += 65536LL * (long long)calData[ptr++];
834 tmp += 256LL * (long long)calData[ptr++];
835 tmp += (long long)calData[ptr++];
847 tmp += 16777216LL * (long long)calData[ptr++];
848 tmp += 65536LL * (long long)calData[ptr++];
849 tmp += 256LL * (long long)calData[ptr++];
850 tmp += (long long)calData[ptr++];
862 calData[ptr++];
863 tmp += 65536LL * (long long)calData[ptr++];
864 tmp += 256LL * (long long)calData[ptr++];
865 tmp += (long long)calData[ptr++];
878 t += 16777216UL * ((uint32_t) calData[ptr++]);
879 t += 65536UL * ((uint32_t) calData[ptr++]);
880 t += 256u * ((uint32_t) calData[ptr++]);
881 t += (uint32_t) calData[ptr++];
893 inv_obj.got_compass_bias = (int)calData[ptr++];
895 inv_obj.got_init_compass_bias = (int)calData[ptr++];
897 inv_obj.compass_state = (int)calData[ptr++];
902 t += 16777216UL * ((uint32_t) calData[ptr++]);
903 t += 65536UL * ((uint32_t) calData[ptr++]);
904 t += 256u * ((uint32_t) calData[ptr++]);
905 t += (uint32_t) calData[ptr++];
912 t += 16777216UL * ((uint32_t) calData[ptr++]);
913 t += 65536UL * ((uint32_t) calData[ptr++]);
914 t += 256u * ((uint32_t) calData[ptr++]);
915 t += (uint32_t) calData[ptr++];
922 t += 16777216UL * ((uint32_t) calData[ptr++]);
923 t += 65536UL * ((uint32_t) calData[ptr++]);
924 t += 256u * ((uint32_t) calData[ptr++]);
925 t += (uint32_t) calData[ptr++];
931 t += 16777216UL * ((uint32_t) calData[ptr++]);
932 t += 65536UL * ((uint32_t) calData[ptr++]);
933 t += 256u * ((uint32_t) calData[ptr++]);
934 t += (uint32_t) calData[ptr++];
939 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
940 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
941 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
942 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
943 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
944 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
945 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
946 dToLL.ll += (unsigned long long)calData[ptr++];
952 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
953 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
954 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
955 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
956 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
957 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
958 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
959 dToLL.ll += (unsigned long long)calData[ptr++];
967 t += 16777216UL * ((uint32_t) calData[ptr++]);
968 t += 65536UL * ((uint32_t) calData[ptr++]);
969 t += 256u * ((uint32_t) calData[ptr++]);
970 t += (uint32_t) calData[ptr++];
975 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
976 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
977 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
978 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
979 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
980 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
981 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
982 dToLL.ll += (unsigned long long)calData[ptr++];
988 dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]);
989 dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]);
990 dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]);
991 dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]);
992 dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]);
993 dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]);
994 dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]);
995 dToLL.ll += (unsigned long long)calData[ptr++];
1002 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++];
1003 inv_obj.compass_offsets[0] = calData[ptr++];
1004 inv_obj.compass_offsets[1] = calData[ptr++];
1005 inv_obj.compass_offsets[2] = calData[ptr++];
1007 inv_obj.compass_accuracy = calData[ptr++];
1050 * @param calData
1057 inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len)
1079 temp = (unsigned short)calData[ptr++] * (1L << 8);
1080 temp += calData[ptr++];
1087 tmp += (int32_t) calData[ptr++] * (1L << 24);
1088 tmp += (int32_t) calData[ptr++] * (1L << 16);
1089 tmp += (int32_t) calData[ptr++] * (1L << 8);
1090 tmp += (int32_t) calData[ptr++];
1123 tmp += (int32_t) calData[ptr++] * (1L << 24);
1124 tmp += (int32_t) calData[ptr++] * (1L << 16);
1125 tmp += (int32_t) calData[ptr++] * (1L << 8);
1126 tmp += (int32_t) calData[ptr++];
1156 * @param calData
1161 inv_error_t inv_load_cal(unsigned char *calData)
1185 len += 16777216L * ((int)calData[0]);
1186 len += 65536L * ((int)calData[1]);
1187 len += 256 * ((int)calData[2]);
1188 len += (int)calData[3];
1190 calType = ((int)calData[4]) * 256 + ((int)calData[5]);
1201 chk += 16777216L * ((uint32_t) calData[ptr++]);
1202 chk += 65536L * ((uint32_t) calData[ptr++]);
1203 chk += 256 * ((uint32_t) calData[ptr++]);
1204 chk += (uint32_t) calData[ptr++];
1206 cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN,
1214 return loaders[calType] (calData, len);
1224 * @param calData
1231 inv_error_t inv_store_cal(unsigned char *calData, int length)
1252 calData[0] = (unsigned char)((length >> 24) & 0xff);
1253 calData[1] = (unsigned char)((length >> 16) & 0xff);
1254 calData[2] = (unsigned char)((length >> 8) & 0xff);
1255 calData[3] = (unsigned char)(length & 0xff);
1259 calData[4] = 0;
1260 calData[5] = 4;
1261 STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]);
1267 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
1268 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
1269 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
1270 calData[ptr++] = (unsigned char)(tmp & 0xff);
1276 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
1277 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
1278 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
1279 calData[ptr++] = (unsigned char)(tmp & 0xff);
1288 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
1289 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
1290 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
1291 calData[ptr++] = (unsigned char)(tmp & 0xff);
1303 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
1304 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
1305 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
1306 calData[ptr++] = (unsigned char)(tmp & 0xff);
1317 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
1318 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
1319 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
1320 calData[ptr++] = (unsigned char)(tmp & 0xff);
1331 calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff);
1332 calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff);
1333 calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff);
1334 calData[ptr++] = (unsigned char)(tmp & 0xff);
1345 calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
1346 calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
1347 calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
1348 calData[ptr++] = (unsigned char)(t & 0xff);
1353 calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias);
1355 calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias);
1358 calData[ptr++] = SF_UNCALIBRATED;
1360 calData[ptr++] = SF_STARTUP_SETTLE;
1366 calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
1367 calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
1368 calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
1369 calData[ptr++] = (unsigned char)(t & 0xff);
1375 calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
1376 calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
1377 calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
1378 calData[ptr++] = (unsigned char)(t & 0xff);
1384 calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
1385 calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
1386 calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
1387 calData[ptr++] = (unsigned char)(t & 0xff);
1392 calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
1393 calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
1394 calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
1395 calData[ptr++] = (unsigned char)(t & 0xff);
1400 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
1401 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
1402 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
1403 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
1404 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
1405 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
1406 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
1407 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
1413 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
1414 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
1415 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
1416 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
1417 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
1418 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
1419 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
1420 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
1426 calData[ptr++] = (unsigned char)((t >> 24) & 0xff);
1427 calData[ptr++] = (unsigned char)((t >> 16) & 0xff);
1428 calData[ptr++] = (unsigned char)((t >> 8) & 0xff);
1429 calData[ptr++] = (unsigned char)(t & 0xff);
1434 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
1435 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
1436 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
1437 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
1438 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
1439 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
1440 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
1441 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
1447 calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff);
1448 calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff);
1449 calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff);
1450 calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff);
1451 calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff);
1452 calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff);
1453 calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff);
1454 calData[ptr++] = (unsigned char)(dToLL.ll & 0xff);
1460 calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID];
1461 calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0];
1462 calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1];
1463 calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2];
1466 calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy);
1469 chk = inv_checksum(calData + INV_CAL_HDR_LEN,
1471 calData[ptr++] = (unsigned char)((chk >> 24) & 0xff);
1472 calData[ptr++] = (unsigned char)((chk >> 16) & 0xff);
1473 calData[ptr++] = (unsigned char)((chk >> 8) & 0xff);
1474 calData[ptr++] = (unsigned char)(chk & 0xff);
1492 unsigned char *calData;
1509 calData = (unsigned char *)inv_malloc(length);
1510 if (!calData) {
1515 result = inv_serial_read_cal(calData, length);
1522 result = inv_load_cal(calData);
1533 inv_free(calData);
1549 unsigned char *calData;
1557 calData = (unsigned char *)inv_malloc(length);
1558 if (!calData) {
1563 result = inv_store_cal(calData, length);
1570 result = inv_serial_write_cal(calData, length);
1580 inv_free(calData);
