Skip to content

Commit ccaf2c0

Browse files
scaryghostscaryghost
committed
api v3.7.0 commit
1 parent 0a8f237 commit ccaf2c0

File tree

9 files changed

+470
-35
lines changed

9 files changed

+470
-35
lines changed

‎library/build.gradle‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@ android {
3131
defaultConfig {
3232
minSdkVersion 18
3333
targetSdkVersion 28
34-
versionCode 59
35-
versionName "3.6.2"
34+
versionCode 60
35+
versionName "3.7.0"
3636
}
3737
buildTypes {
3838
release {

‎library/src/main/java/com/mbientlab/metawear/impl/SensorFusionBoschImpl.java‎

Lines changed: 150 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,11 @@
4141

4242
import java.nio.ByteBuffer;
4343
import java.nio.ByteOrder;
44+
import java.util.Arrays;
4445
import java.util.Calendar;
4546

47+
import bolts.CancellationToken;
48+
import bolts.Capture;
4649
import bolts.Task;
4750

4851
import static com.mbientlab.metawear.impl.Constant.Module.SENSOR_FUSION;
@@ -74,10 +77,11 @@ static String createUri(DataTypeBase dataType) {
7477

7578
private static final long serialVersionUID = -7041546136871081754L;
7679

80+
private static final byte CALIBRATION_STATE_REV = 1, CALIBRATION_DATA_REV = 2;
7781
private static final byte ENABLE = 1, MODE = 2, OUTPUT_ENABLE = 3,
7882
CORRECTED_ACC = 4, CORRECTED_ROT = 5, CORRECTED_MAG = 6,
7983
QUATERNION = 7, EULER_ANGLES = 8, GRAVITY_VECTOR = 9, LINEAR_ACC = 0xa,
80-
CALIB_STATUS = 0xb;
84+
CALIB_STATUS = 0xb, ACC_CALIB_DATA = 0xc, GYRO_CALIB_DATA = 0xd, MAG_CALIB_DATA = 0xe;
8185
private final static String QUATERNION_PRODUCER= "com.mbientlab.metawear.impl.SensorFusionBoschImpl.QUATERNION_PRODUCER",
8286
EULER_ANGLES_PRODUCER= "com.mbientlab.metawear.impl.SensorFusionBoschImpl.EULER_ANGLES_PRODUCER",
8387
GRAVITY_PRODUCER= "com.mbientlab.metawear.impl.SensorFusionBoschImpl.GRAVITY_PRODUCER",
@@ -381,6 +385,7 @@ public void stop() {
381385
private byte dataEnableMask;
382386

383387
private transient TimedTask<byte[]> readRegisterTask;
388+
private transient AsyncDataProducer correctedAccProducer, correctedAngVelProducer, correctedMagProducer, quaterionProducer, eulerAnglesProducer, gravityProducer, linearAccProducer;
384389

385390
SensorFusionBoschImpl(MetaWearBoardPrivate mwPrivate) {
386391
super(mwPrivate);
@@ -399,7 +404,14 @@ protected void init() {
399404
readRegisterTask = new TimedTask<>();
400405

401406
mwPrivate.addResponseHandler(new Pair<>(SENSOR_FUSION.id, Util.setRead(MODE)), response -> readRegisterTask.setResult(response));
402-
mwPrivate.addResponseHandler(new Pair<>(SENSOR_FUSION.id, Util.setRead(CALIB_STATUS)), response -> readRegisterTask.setResult(response));
407+
if (mwPrivate.lookupModuleInfo(SENSOR_FUSION).revision >= CALIBRATION_STATE_REV) {
408+
mwPrivate.addResponseHandler(new Pair<>(SENSOR_FUSION.id, Util.setRead(CALIB_STATUS)), response -> readRegisterTask.setResult(response));
409+
}
410+
if (mwPrivate.lookupModuleInfo(SENSOR_FUSION).revision >= CALIBRATION_DATA_REV) {
411+
mwPrivate.addResponseHandler(new Pair<>(SENSOR_FUSION.id, Util.setRead(ACC_CALIB_DATA)), response -> readRegisterTask.setResult(response));
412+
mwPrivate.addResponseHandler(new Pair<>(SENSOR_FUSION.id, Util.setRead(GYRO_CALIB_DATA)), response -> readRegisterTask.setResult(response));
413+
mwPrivate.addResponseHandler(new Pair<>(SENSOR_FUSION.id, Util.setRead(MAG_CALIB_DATA)), response -> readRegisterTask.setResult(response));
414+
}
403415
}
404416

405417
@Override
@@ -542,37 +554,58 @@ public void commit() {
542554

543555
@Override
544556
public AsyncDataProducer correctedAcceleration() {
545-
return new SensorFusionAsyncDataProducer(CORRECTED_ACC_PRODUCER, (byte) 0x01);
557+
if (correctedAccProducer == null) {
558+
correctedAccProducer = new SensorFusionAsyncDataProducer(CORRECTED_ACC_PRODUCER, (byte) 0x01);
559+
}
560+
return correctedAccProducer;
546561
}
547562

548563
@Override
549564
public AsyncDataProducer correctedAngularVelocity() {
550-
return new SensorFusionAsyncDataProducer(CORRECTED_ROT_PRODUCER, (byte) 0x02);
565+
if (correctedAngVelProducer == null) {
566+
correctedAngVelProducer = new SensorFusionAsyncDataProducer(CORRECTED_ROT_PRODUCER, (byte) 0x02);
567+
}
568+
return correctedAngVelProducer;
551569
}
552570

553571
@Override
554572
public AsyncDataProducer correctedMagneticField() {
555-
return new SensorFusionAsyncDataProducer(CORRECTED_MAG_PRODUCER, (byte) 0x04);
573+
if (correctedMagProducer == null) {
574+
correctedMagProducer = new SensorFusionAsyncDataProducer(CORRECTED_MAG_PRODUCER, (byte) 0x04);
575+
}
576+
return correctedMagProducer;
556577
}
557578

558579
@Override
559580
public AsyncDataProducer quaternion() {
560-
return new SensorFusionAsyncDataProducer(QUATERNION_PRODUCER, (byte) 0x08);
581+
if (quaterionProducer == null) {
582+
quaterionProducer = new SensorFusionAsyncDataProducer(QUATERNION_PRODUCER, (byte) 0x08);
583+
}
584+
return quaterionProducer;
561585
}
562586

563587
@Override
564588
public AsyncDataProducer eulerAngles() {
565-
return new SensorFusionAsyncDataProducer(EULER_ANGLES_PRODUCER, (byte) 0x10);
589+
if (eulerAnglesProducer == null) {
590+
eulerAnglesProducer = new SensorFusionAsyncDataProducer(EULER_ANGLES_PRODUCER, (byte) 0x10);
591+
}
592+
return eulerAnglesProducer;
566593
}
567594

568595
@Override
569596
public AsyncDataProducer gravity() {
570-
return new SensorFusionAsyncDataProducer(GRAVITY_PRODUCER, (byte) 0x20);
597+
if (gravityProducer == null) {
598+
gravityProducer = new SensorFusionAsyncDataProducer(GRAVITY_PRODUCER, (byte) 0x20);
599+
}
600+
return gravityProducer;
571601
}
572602

573603
@Override
574604
public AsyncDataProducer linearAcceleration() {
575-
return new SensorFusionAsyncDataProducer(LINEAR_ACC_PRODUCER, (byte) 0x40);
605+
if (linearAccProducer == null) {
606+
linearAccProducer = new SensorFusionAsyncDataProducer(LINEAR_ACC_PRODUCER, (byte) 0x40);
607+
}
608+
return linearAccProducer;
576609
}
577610

578611
@Override
@@ -659,15 +692,113 @@ public Task<Void> pullConfigAsync() {
659692

660693
@Override
661694
public Task<CalibrationState> readCalibrationStateAsync() {
662-
return readRegisterTask.execute("Did not receive sensor fusion calibration status within %dms", Constant.RESPONSE_TIMEOUT,
663-
() -> mwPrivate.sendCommand(new byte[] {SENSOR_FUSION.id, Util.setRead(CALIB_STATUS)})
664-
).onSuccessTask(task -> {
665-
CalibrationAccuracy values[] = CalibrationAccuracy.values();
666-
return Task.forResult(new CalibrationState(
667-
values[task.getResult()[2]],
668-
values[task.getResult()[3]],
669-
values[task.getResult()[4]]
670-
));
671-
});
695+
if (mwPrivate.lookupModuleInfo(SENSOR_FUSION).revision >= CALIBRATION_STATE_REV) {
696+
return readRegisterTask.execute("Did not receive sensor fusion calibration status within %dms", Constant.RESPONSE_TIMEOUT,
697+
() -> mwPrivate.sendCommand(new byte[] {SENSOR_FUSION.id, Util.setRead(CALIB_STATUS)})
698+
).onSuccessTask(task -> {
699+
CalibrationAccuracy values[] = CalibrationAccuracy.values();
700+
return Task.forResult(new CalibrationState(
701+
values[task.getResult()[2]],
702+
values[task.getResult()[3]],
703+
values[task.getResult()[4]]
704+
));
705+
});
706+
}
707+
return Task.forError(new UnsupportedOperationException("Minimum firmware v1.4.2 required to use this function"));
708+
}
709+
710+
@Override
711+
public Task<CalibrationData> calibrate(CancellationToken ct, long pollingPeriod, CalibrationStateUpdateHandler updateHandler) {
712+
if (mwPrivate.lookupModuleInfo(SENSOR_FUSION).revision >= CALIBRATION_DATA_REV) {
713+
final Capture<Boolean> terminate = new Capture<>(false);
714+
final Capture<byte[]> acc = new Capture<>(null),
715+
gyro = new Capture<>(null),
716+
mag = new Capture<>(null);
717+
718+
return Task.forResult(null).continueWhile(() -> !terminate.get(), ignored -> ct.isCancellationRequested() ? readCalibrationStateAsync().onSuccessTask(task -> {
719+
if (updateHandler != null) {
720+
updateHandler.receivedUpdate(task.getResult());
721+
}
722+
723+
switch (mode) {
724+
case NDOF:
725+
terminate.set(task.getResult().accelerometer == CalibrationAccuracy.HIGH_ACCURACY &&
726+
task.getResult().gyroscope == CalibrationAccuracy.HIGH_ACCURACY &&
727+
task.getResult().magnetometer == CalibrationAccuracy.HIGH_ACCURACY);
728+
break;
729+
case IMU_PLUS:
730+
terminate.set(task.getResult().accelerometer == CalibrationAccuracy.HIGH_ACCURACY &&
731+
task.getResult().gyroscope == CalibrationAccuracy.HIGH_ACCURACY);
732+
break;
733+
case COMPASS:
734+
terminate.set(task.getResult().accelerometer == CalibrationAccuracy.HIGH_ACCURACY &&
735+
task.getResult().magnetometer == CalibrationAccuracy.HIGH_ACCURACY);
736+
break;
737+
case M4G:
738+
terminate.set(task.getResult().accelerometer == CalibrationAccuracy.HIGH_ACCURACY &&
739+
task.getResult().magnetometer == CalibrationAccuracy.HIGH_ACCURACY);
740+
break;
741+
}
742+
743+
return !terminate.get() ? Task.delay(pollingPeriod) : Task.<Void>forResult(null);
744+
}) : Task.cancelled()
745+
).onSuccessTask(ignored -> readRegisterTask.execute("Did not receive accelerometer calibration data within %dms", Constant.RESPONSE_TIMEOUT,
746+
() -> mwPrivate.sendCommand(new byte[] {SENSOR_FUSION.id, Util.setRead(ACC_CALIB_DATA)}))
747+
).onSuccessTask(task -> {
748+
byte[] result = task.getResult();
749+
acc.set(Arrays.copyOfRange(result, 2, result.length));
750+
751+
return mode == Mode.IMU_PLUS || mode == Mode.NDOF ? readRegisterTask.execute("Did not receive gyroscope calibration data within %dms", Constant.RESPONSE_TIMEOUT,
752+
() -> mwPrivate.sendCommand(new byte[] {SENSOR_FUSION.id, Util.setRead(GYRO_CALIB_DATA)})
753+
) : Task.forResult(null);
754+
}).onSuccessTask(task -> {
755+
if (task.getResult() != null) {
756+
byte[] result = task.getResult();
757+
gyro.set(Arrays.copyOfRange(result, 2, result.length));
758+
}
759+
760+
return mode != Mode.IMU_PLUS ? readRegisterTask.execute("Did not receive magnetometer calibration data within %dms", Constant.RESPONSE_TIMEOUT,
761+
() -> mwPrivate.sendCommand(new byte[] {SENSOR_FUSION.id, Util.setRead(MAG_CALIB_DATA)})
762+
) : Task.forResult(null);
763+
}).onSuccessTask(task -> {
764+
if (task.getResult() != null) {
765+
byte[] result = task.getResult();
766+
mag.set(Arrays.copyOfRange(result, 2, result.length));
767+
}
768+
769+
return Task.forResult(new CalibrationData(acc.get(), gyro.get(), mag.get()));
770+
});
771+
}
772+
return Task.forError(new UnsupportedOperationException("Minimum firmware v1.4.4 required to use this function"));
773+
}
774+
775+
@Override
776+
public Task<CalibrationData> calibrate(CancellationToken ct, CalibrationStateUpdateHandler updateHandler) {
777+
return calibrate(ct, 1000, updateHandler);
778+
}
779+
780+
@Override
781+
public Task<CalibrationData> calibrate(CancellationToken ct, long pollingPeriod) {
782+
return calibrate(ct, pollingPeriod, null);
783+
}
784+
785+
@Override
786+
public Task<CalibrationData> calibrate(CancellationToken ct) {
787+
return calibrate(ct, 1000, null);
788+
}
789+
790+
@Override
791+
public void writeCalibrationData(CalibrationData data) {
792+
if (mwPrivate.lookupModuleInfo(SENSOR_FUSION).revision >= CALIBRATION_STATE_REV) {
793+
mwPrivate.sendCommand(SENSOR_FUSION, ACC_CALIB_DATA, data.accelerometer);
794+
795+
if (mode == Mode.IMU_PLUS || mode == Mode.NDOF) {
796+
mwPrivate.sendCommand(SENSOR_FUSION, GYRO_CALIB_DATA, data.gyroscope);
797+
}
798+
799+
if (mode != Mode.IMU_PLUS) {
800+
mwPrivate.sendCommand(SENSOR_FUSION, MAG_CALIB_DATA, data.magnetometer);
801+
}
802+
}
672803
}
673804
}

‎library/src/main/java/com/mbientlab/metawear/impl/StreamedDataConsumer.java‎

Lines changed: 11 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -179,14 +179,20 @@ private static Tuple3<Calendar, Integer, Long> fillTimestamp(MetaWearBoardPrivat
179179
if (accounter != null) {
180180
DataProcessorConfig config = accounter.editor.configObj;
181181
if (config instanceof DataProcessorConfig.Accounter) {
182-
LoggingImpl logging = (LoggingImpl) mwPrivate.getModules().get(Logging.class);
183182
int size = ((DataProcessorConfig.Accounter) config).length;
184-
185-
byte[] padded= new byte[8];
183+
byte[] padded = new byte[8];
186184
System.arraycopy(response, offset, padded, 0, size);
187-
long tick= ByteBuffer.wrap(padded).order(ByteOrder.LITTLE_ENDIAN).getLong(0);
185+
long tick = ByteBuffer.wrap(padded).order(ByteOrder.LITTLE_ENDIAN).getLong(0);
188186

189-
return new Tuple3<>(logging.computeTimestamp((byte) -1, tick), size + offset, tick);
187+
switch(((DataProcessorConfig.Accounter) config).type) {
188+
case COUNT: {
189+
return new Tuple3<>(Calendar.getInstance(), size + offset, tick);
190+
}
191+
case TIME: {
192+
LoggingImpl logging = (LoggingImpl) mwPrivate.getModules().get(Logging.class);
193+
return new Tuple3<>(logging.computeTimestamp((byte) -1, tick), size + offset, tick);
194+
}
195+
}
190196
}
191197
}
192198
return new Tuple3<>(Calendar.getInstance(), offset, 0L);

‎library/src/main/java/com/mbientlab/metawear/impl/Util.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,7 @@ public static int closestIndex(float[] values, float key) {
5050
}
5151

5252
public static String arrayToHexString(byte[] value) {
53-
if (value.length == 0) {
53+
if (value == null || value.length == 0) {
5454
return "[]";
5555
}
5656

0 commit comments

Comments
 (0)