Skip to content

DEV-922 Upstream gen-2 default alignment into SensorLSM6DSV #282

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
jyong15 merged 7 commits into from
Aug 4, 2026
Merged

DEV-922 Upstream gen-2 default alignment into SensorLSM6DSV #282

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
08ddcdc
Update SensorLSM6DSV.java
JongChern Jul 31, 2026
fea5a79
DEV-922 Upstream gen-2 default alignment into SensorLSM6DSV
marknolan Aug 2, 2026
74119a1
DEV-922 Write mag driver-form alignment as a literal (Copilot review)
marknolan Aug 2, 2026
cef9e7e
DEV-922 Update API_00008 CAL expectations for the gen-2 default align…
claude Aug 4, 2026
49a2df6
Merge pull request #283 from ShimmerResearch/claude/adversarial-pr-re…
jyong15 Aug 4, 2026
7b83b81
DEV-922 Correct calibration comments after cross-repo verification
claude Aug 4, 2026
316df2d
DEV-922 Comment corrections from skeptic re-review
claude Aug 4, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
98 changes: 79 additions & 19 deletions ShimmerDriver/src/main/java/com/shimmerresearch/verisense/sensors/SensorLSM6DSV.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
import com.shimmerresearch.driverUtilities.ChannelDetails.CHANNEL_TYPE;
import com.shimmerresearch.sensors.AbstractSensor;
import com.shimmerresearch.sensors.ActionSetting;
import com.shimmerresearch.sensors.lisxmdl.SensorLIS2MDL;

/**
* Second-generation Verisense IMU: LSM6DSV (accelerometer + gyroscope) with the
Expand All @@ -38,10 +39,24 @@
* tagged FIFO ([TAG_CNT][X][Y][Z]) interleaving the three streams; the byte-level
* extraction + per-stream timestamping lives in
* {@code VerisenseDevice.parseDataBlockDataLsm6dsv(...)}. This class provides the
* channel definitions, configuration and calibration. Calibration matches the
* firmware/SDK nominal model: value = raw / sensitivity (identity alignment,
* zero offset) where sensitivity = 32768/(FS*9.80665) for accel, 32768/FS for
* gyro and 1/0.15 for mag.
* channel definitions, configuration and calibration.
* <p>
* 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.
* <p>
* 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 &gt;= 5, SR68 rev
* &gt;= 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
*/
Expand Down Expand Up @@ -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}};
Expand All @@ -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;
Expand Down
18 changes: 13 additions & 5 deletions .../test/java/com/shimmerresearch/verisense/API_00008_VerisenseLsm6dsvTaggedFifoParsing.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */
Expand Down
Loading