Lines Matching refs:sensors
61 static struct inv_sensor_cal_t sensors;
91 memcpy(raw, sensors.compass.raw, sizeof(sensors.compass.raw));
98 // copy in the saved accuracy in the actual sensors accuracy
99 sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy;
100 sensors.accel.accuracy = inv_data_builder.save.accel_accuracy;
101 sensors.compass.accuracy = inv_data_builder.save.compass_accuracy;
103 if (sensors.accel.accuracy == 3) {
106 if (sensors.compass.accuracy == 3) {
155 memset(&sensors, 0, sizeof(sensors));
178 return sensors.gyro.sensitivity;
188 return sensors.accel.sensitivity;
198 return sensors.compass.sensitivity;
255 set_sensor_orientation_and_scale(&sensors.gyro, orientation,
271 sensors.gyro.sample_rate_us = sample_rate_us;
272 sensors.gyro.sample_rate_ms = sample_rate_us / 1000;
273 if (sensors.gyro.bandwidth == 0) {
274 sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us);
290 sensors.accel.sample_rate_us = sample_rate_us;
291 sensors.accel.sample_rate_ms = sample_rate_us / 1000;
292 if (sensors.accel.bandwidth == 0) {
293 sensors.accel.bandwidth = (int)(1000000L / sample_rate_us);
337 *ts = sensors.quat.timestamp;
339 if (sensors.quat.timestamp_prev != sensors.quat.timestamp)
343 *ts = sensors.gyro.timestamp;
345 if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp)
349 *ts = sensors.accel.timestamp;
351 if (sensors.accel.timestamp_prev != sensors.accel.timestamp)
355 *ts = sensors.compass.timestamp;
357 if (sensors.compass.timestamp_prev != sensors.compass.timestamp)
379 if ((sensors.quat.status & (INV_QUAT_9AXIS | INV_SENSOR_ON)) == (INV_QUAT_9AXIS | INV_SENSOR_ON)) {
381 *ts = sensors.quat.timestamp;
383 if (sensors.quat.timestamp_prev != sensors.quat.timestamp)
388 if ((sensors.compass.status & INV_SENSOR_ON) == 0) {
393 td[0] = sample_rate_us - sensors.quat.sample_rate_us;
394 td[1] = sample_rate_us - sensors.compass.sample_rate_us;
395 if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) {
400 } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
405 td[2] = sample_rate_us - sensors.accel.sample_rate_us;
406 if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) {
418 if ((sensors.gyro.status & INV_SENSOR_ON) == 0) {
422 td[0] = sample_rate_us - sensors.gyro.sample_rate_us;
443 if ((sensors.compass.status & INV_SENSOR_ON) == 0) {
446 if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
451 td[0] = sample_rate_us - sensors.accel.sample_rate_us;
452 td[1] = sample_rate_us - sensors.compass.sample_rate_us;
469 if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) {
472 } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) {
477 td[0] = sample_rate_us - sensors.quat.sample_rate_us;
478 td[1] = sample_rate_us - sensors.accel.sample_rate_us;
479 if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) {
487 if ((sensors.gyro.status & INV_SENSOR_ON) == 0) {
491 td[0] = sample_rate_us - sensors.gyro.sample_rate_us;
511 sensors.compass.sample_rate_us = sample_rate_us;
512 sensors.compass.sample_rate_ms = sample_rate_us / 1000;
513 if (sensors.compass.bandwidth == 0) {
514 sensors.compass.bandwidth = (int)(1000000L / sample_rate_us);
520 *sample_rate_ms = sensors.gyro.sample_rate_ms;
525 *sample_rate_ms = sensors.accel.sample_rate_ms;
530 *sample_rate_ms = sensors.compass.sample_rate_ms;
545 sensors.quat.sample_rate_us = sample_rate_us;
546 sensors.quat.sample_rate_ms = sample_rate_us / 1000;
554 sensors.gyro.bandwidth = bandwidth_hz;
562 sensors.accel.bandwidth = bandwidth_hz;
570 sensors.compass.bandwidth = bandwidth_hz;
578 return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON;
586 return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON;
594 return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON;
597 /** Get last timestamp across all 3 sensors that are on.
598 * This find out which timestamp has the largest value for sensors that are on.
604 if (sensors.accel.status & INV_SENSOR_ON) {
605 timestamp = sensors.accel.timestamp;
607 if (sensors.gyro.status & INV_SENSOR_ON) {
608 if (timestamp < sensors.gyro.timestamp) {
609 timestamp = sensors.gyro.timestamp;
613 if (sensors.compass.status & INV_SENSOR_ON) {
614 if (timestamp < sensors.compass.timestamp) {
615 timestamp = sensors.compass.timestamp;
618 if (sensors.temp.status & INV_SENSOR_ON) {
619 if (timestamp < sensors.temp.timestamp) {
620 timestamp = sensors.temp.timestamp;
623 if (sensors.quat.status & INV_SENSOR_ON) {
624 if (timestamp < sensors.quat.timestamp) {
625 timestamp = sensors.quat.timestamp;
652 set_sensor_orientation_and_scale(&sensors.accel, orientation,
675 set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity);
730 inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
732 sensors.compass.accuracy = accuracy;
773 sensors.accel.accuracy = accuracy;
795 inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias);
844 inv_apply_calibration(&sensors.gyro,
848 sensors.gyro.accuracy = accuracy;
852 if (sensors.temp.calibrated[0])
853 inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0];
909 inv_convert_to_body_with_scale(sensors.gyro.orientation, 46850825L,
987 inv_convert_to_body_with_scale(sensors.accel.orientation, 536870912L,
1019 sensors.accel.raw[0] = (short)accel[0];
1020 sensors.accel.raw[1] = (short)accel[1];
1021 sensors.accel.raw[2] = (short)accel[2];
1022 sensors.accel.status |= INV_RAW_DATA;
1023 inv_apply_calibration(&sensors.accel, inv_data_builder.save_accel_mpl.accel_bias);
1025 sensors.accel.calibrated[0] = accel[0];
1026 sensors.accel.calibrated[1] = accel[1];
1027 sensors.accel.calibrated[2] = accel[2];
1028 sensors.accel.status |= INV_CALIBRATED;
1029 sensors.accel.accuracy = status & 3;
1032 sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON;
1033 sensors.accel.timestamp_prev = sensors.accel.timestamp;
1034 sensors.accel.timestamp = timestamp;
1057 memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short));
1058 sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
1059 sensors.gyro.timestamp_prev = sensors.gyro.timestamp;
1060 sensors.gyro.timestamp = timestamp;
1061 inv_apply_calibration(&sensors.gyro, inv_data_builder.save_mpl.gyro_bias);
1093 sensors.compass.raw[0] = (short)data[0];
1094 sensors.compass.raw[1] = (short)data[1];
1095 sensors.compass.raw[2] = (short)data[2];
1096 inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias);
1097 sensors.compass.status |= INV_RAW_DATA;
1099 sensors.compass.calibrated[0] = compass[0];
1100 sensors.compass.calibrated[1] = compass[1];
1101 sensors.compass.calibrated[2] = compass[2];
1102 sensors.compass.status |= INV_CALIBRATED;
1103 sensors.compass.accuracy = status & 3;
1106 sensors.compass.timestamp_prev = sensors.compass.timestamp;
1107 sensors.compass.timestamp = timestamp;
1108 sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON;
1129 sensors.temp.calibrated[0] = temp;
1130 sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
1131 sensors.temp.timestamp_prev = sensors.temp.timestamp;
1132 sensors.temp.timestamp = timestamp;
1167 memcpy(sensors.quat.raw, resultQuat, sizeof(sensors.quat.raw));
1169 memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw));
1171 sensors.quat.timestamp_prev = sensors.quat.timestamp;
1172 sensors.quat.timestamp = timestamp;
1173 sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON;
1174 sensors.quat.status |= (INV_QUAT_3AXIS & status);
1175 sensors.quat.status |= (INV_QUAT_6AXIS & status);
1176 sensors.quat.status |= (INV_QUAT_9AXIS & status);
1177 sensors.quat.status |= (INV_BIAS_APPLIED & status);
1178 sensors.quat.status |= (INV_DMP_BIAS_APPLIED & status);
1179 sensors.quat.status |= (INV_QUAT_3ELEMENT & status);
1180 MPL_LOGV("quat.status: %d", sensors.quat.status);
1181 if (sensors.quat.status & (INV_QUAT_9AXIS | INV_QUAT_6AXIS)) {
1185 if (sensors.quat.status & INV_QUAT_9AXIS) {
1194 sensors.pressure.status |= INV_NEW_DATA;
1209 sensors.accel.status = 0;
1223 sensors.compass.status = 0;
1237 sensors.quat.status = 0;
1251 sensors.gyro.status = 0;
1259 sensors.temp.status = 0;
1369 if (sensors.gyro.status & INV_NEW_DATA)
1371 if (sensors.accel.status & INV_NEW_DATA)
1373 if (sensors.compass.status & INV_NEW_DATA)
1375 if (sensors.temp.status & INV_NEW_DATA)
1377 if (sensors.quat.status & INV_NEW_DATA)
1379 if (sensors.pressure.status & INV_NEW_DATA)
1386 result = inv_data_builder.process[kk].func(&sensors);
1404 if (sensors.gyro.status & INV_NEW_DATA) {
1405 sensors.gyro.status |= INV_CONTIGUOUS;
1406 current_time = sensors.gyro.timestamp;
1408 if (sensors.accel.status & INV_NEW_DATA) {
1409 sensors.accel.status |= INV_CONTIGUOUS;
1410 current_time = MAX(current_time, sensors.accel.timestamp);
1412 if (sensors.compass.status & INV_NEW_DATA) {
1413 sensors.compass.status |= INV_CONTIGUOUS;
1414 current_time = MAX(current_time, sensors.compass.timestamp);
1416 if (sensors.temp.status & INV_NEW_DATA) {
1417 sensors.temp.status |= INV_CONTIGUOUS;
1418 current_time = MAX(current_time, sensors.temp.timestamp);
1420 if (sensors.quat.status & INV_NEW_DATA) {
1421 sensors.quat.status |= INV_CONTIGUOUS;
1422 current_time = MAX(current_time, sensors.quat.timestamp);
1426 /* See if sensors are still on. These should be turned off by inv_*_was_turned_off()
1429 if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000)
1431 if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000)
1433 if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000)
1436 if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000)
1441 sensors.gyro.status &= ~INV_NEW_DATA;
1442 sensors.accel.status &= ~INV_NEW_DATA;
1443 sensors.compass.status &= ~INV_NEW_DATA;
1444 sensors.temp.status &= ~INV_NEW_DATA;
1445 sensors.quat.status &= ~INV_NEW_DATA;
1446 sensors.pressure.status &= ~INV_NEW_DATA;
1457 memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated));
1460 *timestamp = sensors.accel.timestamp;
1463 *accuracy = sensors.accel.accuracy;
1474 memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
1476 *timestamp = sensors.gyro.timestamp;
1479 *accuracy = sensors.gyro.accuracy;
1490 memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled));
1492 *timestamp = sensors.gyro.timestamp;
1504 memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated));
1514 memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated));
1516 *timestamp = sensors.compass.timestamp;
1522 *accuracy = sensors.compass.accuracy;
1533 memcpy(data, sensors.compass.raw_scaled, sizeof(sensors.compass.raw_scaled));
1535 *timestamp = sensors.compass.timestamp;
1548 data[0] = sensors.temp.calibrated[0];
1550 *timestamp = sensors.temp.timestamp;
1552 *accuracy = sensors.temp.accuracy;
1560 return sensors.gyro.accuracy;
1570 return sensors.compass.accuracy;
1578 return sensors.accel.accuracy;
1583 *orient = sensors.gyro.orientation;
1589 *orient = sensors.accel.orientation;
1603 matrix[i] = sensors.soft_iron.matrix_d[i];
1614 sensors.soft_iron.matrix_d[i] = matrix[i];
1616 sensors.soft_iron.matrix_f[i] = inv_q30_to_float(matrix[i]);
1625 matrix[i] = sensors.soft_iron.matrix_f[i];
1635 sensors.soft_iron.matrix_f[i] = matrix[i];
1637 sensors.soft_iron.matrix_d[i] = (long )(matrix[i]*ROT_MATRIX_SCALE_LONG);
1647 data[i] = sensors.soft_iron.trans[i];
1656 data[i] = sensors.soft_iron.raw[i];
1665 sensors.soft_iron.raw[i] = data[i];
1667 if (sensors.soft_iron.enable == 1) {
1668 mlMatrixVectorMult(sensors.soft_iron.matrix_d, data, sensors.soft_iron.trans);
1671 sensors.soft_iron.trans[i] = data[i];
1682 sensors.soft_iron.matrix_f[i] = 0.0f;
1685 memset(&sensors.soft_iron.matrix_d,0,sizeof(sensors.soft_iron.matrix_d));
1689 sensors.soft_iron.matrix_f[i*4] = 1.0;
1691 sensors.soft_iron.matrix_d[i*4] = ROT_MATRIX_SCALE_LONG;
1700 sensors.soft_iron.enable = 1;
1706 sensors.soft_iron.enable = 0;
