+ * Sensitivities: 32768/(FS*9.80665) LSB per m/s^2 for accel, the ST angular-rate + * spec of 4.375 mdps/LSB at +-125 dps for gyro, and 667 LSB/Gauss for the LIS2MDL + * mag - the last taken from {@link SensorLIS2MDL} rather than duplicated here, so + * calibrated magnetometer output is in GAUSS, consistent with every other Shimmer + * magnetometer and with the per-unit calibration the device stores. + *
+ * Default alignment is the real sensor->ASM frame map (accel/gyro share the chip + * mounting; the LIS2MDL frame is left-handed), matching the web SDK's + * CALIBRATION_SENSORS_GEN2 and what verisense-device-console writes to the device. + * No hardware-revision gate is needed here: this sensor class is only instantiated + * for firmware payload design v13 and above (see + * {@code VerisenseDevice.sensorAndConfigMapsCreate()}), i.e. second-generation + * firmware, and every second-generation mounting (SR61 rev >= 5, SR68 rev + * >= 9) shares this frame. Note the FW/HW pairing itself is not enforced + * anywhere: a first-generation board flashed with gen-2 firmware would get this + * frame too. Offsets are zero. * * @author Mark Nolan */ @@ -230,10 +245,45 @@ public static final class DatabaseConfigHandle { CompatibilityInfoForMaps.listOfCompatibleVersionInfoLSM6DSV); // ----------------- Calibration Start ----------------------- - // Identity alignment + zero offset so calibrated = raw / sensitivity, matching - // the firmware/SDK nominal model (and the validated standalone decoder). public static final double[][] DEFAULT_OFFSET_VECTOR_LSM6DSV = {{0},{0},{0}}; - public static final double[][] DEFAULT_ALIGNMENT_MATRIX_LSM6DSV = {{1,0,0},{0,1,0},{0,0,1}}; + + // Default alignment, stored first in APPLIED form - the sensor->ASM map, + // physical = applied . (raw - bias) / sens - so the literals below read exactly + // as verisense-device-console displays them and as the web SDK declares them + // (calibrationDefaults.ts, CALIBRATION_SENSORS_GEN2). Those two and this class + // must stay in agreement; they are the same physical mounting. + // + // WARNING for future per-unit calibration work: the device stores alignment in + // this same APPLIED form (the firmware seeds it in asm_calibration.c and the + // console writes it back), but the existing load paths + // (CalibDetailsKinematic.parseCalParamByteArray and the file parser's + // CalibrationFileManager) copy bytes into the AM slot WITHOUT inverting, and + // UtilCalibration applies AM^-1. Neither path is used for this sensor today; + // if gen-2 per-unit calibration is ever enabled, the loaded alignment must be + // inverted to driver form at load or calibration will be applied backwards. + // The trap is bidirectional: generateCalParamByteArray() likewise writes the + // driver-form AM out uninverted, so writing calibration TO a gen-2 device + // would send the inverse of the applied form the firmware and console expect. + /** Applied sensor->ASM alignment for the LSM6DSV accel + gyro; det +1 (a proper rotation). */ + public static final double[][] APPLIED_ALIGNMENT_LSM6DSV_ACCEL_GYRO = {{0,1,0},{0,0,1},{1,0,0}}; + /** Applied sensor->ASM alignment for the LIS2MDL mag; its frame is left-handed, so det -1 (a reflection). */ + public static final double[][] APPLIED_ALIGNMENT_LIS2MDL_MAG = {{1,0,0},{0,0,1},{0,1,0}}; + + // The driver stores the opposite convention: CalibDetailsKinematic holds AM and + // UtilCalibration computes AM^-1 . SM^-1 . (data - OV), so the matrices handed to + // the calibration blocks below are the INVERSES of the applied form. A true + // inverse, not a transpose: the two coincide only for orthogonal + // signed-permutation defaults like these, and would differ for a rig-measured + // matrix with cross-axis terms. + public static final double[][] DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO = + UtilCalibration.matrixInverse3x3(APPLIED_ALIGNMENT_LSM6DSV_ACCEL_GYRO); + /* The mag applied matrix is its own inverse (an involution), but deriving it via + * matrixInverse3x3 would stamp -0.0 into the zero entries (its determinant is -1), + * which survives serialization and fails Arrays.deepEquals against 0.0 - so the + * driver-form matrix is written out as a literal. ASM_PC_00032 guards that it + * really is the inverse of the applied form. (The accel/gyro inverse above is + * det +1 and computes canonical 0.0 entries, so it stays derived.) */ + public static final double[][] DEFAULT_ALIGNMENT_LIS2MDL_MAG = {{1,0,0},{0,0,1},{0,1,0}}; // Accel sensitivity (LSB per m/s^2) = 32768/(FS_g*9.80665) public static final double[][] SENS_ACCEL_2G = {{1670.703,0,0},{0,1670.703,0},{0,0,1670.703}}; @@ -251,40 +301,50 @@ public static final class DatabaseConfigHandle { public static final double[][] SENS_GYRO_1000DPS = {{28.571428571,0,0},{0,28.571428571,0},{0,0,28.571428571}}; public static final double[][] SENS_GYRO_2000DPS = {{14.285714286,0,0},{0,14.285714286,0},{0,0,14.285714286}}; - // Mag sensitivity (LSB per uT) = 1/0.15 - public static final double[][] SENS_MAG = {{6.666667,0,0},{0,6.666667,0},{0,0,6.666667}}; + // Mag sensitivity: taken from the LIS2MDL's own sensor class rather than + // re-declared here, because it is a property of the chip (1.5 mGauss/LSB = + // 2000/3 = 666.67 LSB/Gauss; 667 is the established Shimmer rounding) and not + // of the LSM6DSV sensor hub the samples arrive through. 667 LSB/Gauss, matching + // the firmware calibration seed (SC_LIS2MDL_MAG_SENS), the web SDK catalog, and + // every other Shimmer magnetometer (LSM303DLHC etc. are all LSB/Gauss). + // + // This was previously a local copy of 6.666667 = 1/0.15 LSB/uT, which made + // gen-2 magnetometer output 100x larger than every other Shimmer device for the + // same field, and 100x out of step with the per-unit calibration the device + // stores. Reference the shared constant so the two cannot diverge again. + public static final double[][] SENS_MAG = SensorLIS2MDL.DefaultSensitivityMatrixMagShimmer3r; public CalibDetailsKinematic calibDetailsAccel2g = new CalibDetailsKinematic( LSM6DSV_ACCEL_RANGE.RANGE_2G.configValue, LSM6DSV_ACCEL_RANGE.RANGE_2G.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_ACCEL_2G, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_ACCEL_2G, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsAccel4g = new CalibDetailsKinematic( LSM6DSV_ACCEL_RANGE.RANGE_4G.configValue, LSM6DSV_ACCEL_RANGE.RANGE_4G.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_ACCEL_4G, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_ACCEL_4G, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsAccel8g = new CalibDetailsKinematic( LSM6DSV_ACCEL_RANGE.RANGE_8G.configValue, LSM6DSV_ACCEL_RANGE.RANGE_8G.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_ACCEL_8G, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_ACCEL_8G, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsAccel16g = new CalibDetailsKinematic( LSM6DSV_ACCEL_RANGE.RANGE_16G.configValue, LSM6DSV_ACCEL_RANGE.RANGE_16G.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_ACCEL_16G, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_ACCEL_16G, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsGyro125dps = new CalibDetailsKinematic( LSM6DSV_GYRO_RANGE.RANGE_125DPS.configValue, LSM6DSV_GYRO_RANGE.RANGE_125DPS.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_GYRO_125DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_GYRO_125DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsGyro250dps = new CalibDetailsKinematic( LSM6DSV_GYRO_RANGE.RANGE_250DPS.configValue, LSM6DSV_GYRO_RANGE.RANGE_250DPS.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_GYRO_250DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_GYRO_250DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsGyro500dps = new CalibDetailsKinematic( LSM6DSV_GYRO_RANGE.RANGE_500DPS.configValue, LSM6DSV_GYRO_RANGE.RANGE_500DPS.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_GYRO_500DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_GYRO_500DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsGyro1000dps = new CalibDetailsKinematic( LSM6DSV_GYRO_RANGE.RANGE_1000DPS.configValue, LSM6DSV_GYRO_RANGE.RANGE_1000DPS.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_GYRO_1000DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_GYRO_1000DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsGyro2000dps = new CalibDetailsKinematic( LSM6DSV_GYRO_RANGE.RANGE_2000DPS.configValue, LSM6DSV_GYRO_RANGE.RANGE_2000DPS.label, - DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_GYRO_2000DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); + DEFAULT_ALIGNMENT_LSM6DSV_ACCEL_GYRO, SENS_GYRO_2000DPS, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic calibDetailsMag = new CalibDetailsKinematic( - 0, "Default", DEFAULT_ALIGNMENT_MATRIX_LSM6DSV, SENS_MAG, DEFAULT_OFFSET_VECTOR_LSM6DSV); + 0, "Default", DEFAULT_ALIGNMENT_LIS2MDL_MAG, SENS_MAG, DEFAULT_OFFSET_VECTOR_LSM6DSV); public CalibDetailsKinematic mCurrentCalibDetailsAccel = calibDetailsAccel4g; public CalibDetailsKinematic mCurrentCalibDetailsGyro = calibDetailsGyro500dps; diff --git a/ShimmerDriver/src/test/java/com/shimmerresearch/verisense/API_00008_VerisenseLsm6dsvTaggedFifoParsing.java b/ShimmerDriver/src/test/java/com/shimmerresearch/verisense/API_00008_VerisenseLsm6dsvTaggedFifoParsing.java index 0bce8705..75fe05de 100644 --- a/ShimmerDriver/src/test/java/com/shimmerresearch/verisense/API_00008_VerisenseLsm6dsvTaggedFifoParsing.java +++ b/ShimmerDriver/src/test/java/com/shimmerresearch/verisense/API_00008_VerisenseLsm6dsvTaggedFifoParsing.java @@ -143,11 +143,19 @@ public void test001_accelGyroMagInterleaved() throws Exception { // LITERAL expected values (the gyro sensitivity was ~12.8% wrong before the // DEV-793 round-2 review fix; deriving the expectation from the class // constants would defeat the lock). Defaults: accel +/-4 g = 835.3517 - // LSB/(m/s^2); gyro +/-500 dps = 57.142857 LSB/dps (ST 17.50 mdps/LSB). - assertEquals(100 / 835.3517, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_ACC_X), 0.0001); - assertEquals(-200 / 835.3517, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_ACC_Y), 0.0001); - assertEquals(10 / 57.142857143, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_GYRO_X), 0.0001); - assertEquals(-20 / 57.142857143, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_GYRO_Y), 0.0001); + // LSB/(m/s^2); gyro +/-500 dps = 57.142857 LSB/dps (ST 17.50 mdps/LSB); + // mag 667 LSB/Gauss (LIS2MDL 1.5 mGauss/LSB, legacy rounding of 666.67). + // Since DEV-922 the defaults also apply the gen-2 sensor->ASM alignment: + // accel/gyro calibrated X/Y/Z come from raw Y/Z/X, mag X/Y/Z from raw X/Z/Y. + assertEquals(-200 / 835.3517, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_ACC_X), 0.0001); + assertEquals(300 / 835.3517, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_ACC_Y), 0.0001); + assertEquals(100 / 835.3517, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_ACC_Z), 0.0001); + assertEquals(-20 / 57.142857143, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_GYRO_X), 0.0001); + assertEquals(30 / 57.142857143, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_GYRO_Y), 0.0001); + assertEquals(10 / 57.142857143, cal(aligned0, SensorLSM6DSV.ObjectClusterSensorName.LSM6DSV_GYRO_Z), 0.0001); + assertEquals(-150 / 667.0, cal(magOjc, SensorLSM6DSV.ObjectClusterSensorName.LIS2MDL_MAG_X), 0.0001); + assertEquals(150 / 667.0, cal(magOjc, SensorLSM6DSV.ObjectClusterSensorName.LIS2MDL_MAG_Y), 0.0001); + assertEquals(5 / 667.0, cal(magOjc, SensorLSM6DSV.ObjectClusterSensorName.LIS2MDL_MAG_Z), 0.0001); } /** Gyro-only: gyro acts as the aligned reference stream. */