Merge branch 'main' into new-bat

.github/FUNDING.yml

@@ -0,0 +1,3 @@
+# These are supported funding model platforms
+
+github: SlimeVR

lib/ICM42688/ICM42688.h

@@ -0,0 +1,192 @@
+/* 01/14/2022 Copyright Tlera Corporation
+
+    Created by Kris Winer
+
+  This sketch uses SDA/SCL on pins 21/20 (ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board.
+  The ICM42688 is a combo sensor with embedded accel and gyro, here used as 6 DoF in a 9 DoF absolute orientation solution.
+
+  Library may be used freely and without limit with attribution.
+
+*/
+
+#ifndef ICM42688_h
+#define ICM42688_h
+
+/* ICM42688 registers
+https://media.digikey.com/pdf/Data%20Sheets/TDK%20PDFs/ICM-42688-P_DS_Rev1.2.pdf
+*/
+// User Bank 0
+#define ICM42688_DEVICE_CONFIG             0x11
+#define ICM42688_DRIVE_CONFIG              0x13
+#define ICM42688_INT_CONFIG                0x14
+#define ICM42688_FIFO_CONFIG               0x16
+#define ICM42688_TEMP_DATA1                0x1D
+#define ICM42688_TEMP_DATA0                0x1E
+#define ICM42688_ACCEL_DATA_X1             0x1F
+#define ICM42688_ACCEL_DATA_X0             0x20
+#define ICM42688_ACCEL_DATA_Y1             0x21
+#define ICM42688_ACCEL_DATA_Y0             0x22
+#define ICM42688_ACCEL_DATA_Z1             0x23
+#define ICM42688_ACCEL_DATA_Z0             0x24
+#define ICM42688_GYRO_DATA_X1              0x25
+#define ICM42688_GYRO_DATA_X0              0x26
+#define ICM42688_GYRO_DATA_Y1              0x27
+#define ICM42688_GYRO_DATA_Y0              0x28
+#define ICM42688_GYRO_DATA_Z1              0x29
+#define ICM42688_GYRO_DATA_Z0              0x2A
+#define ICM42688_TMST_FSYNCH               0x2B
+#define ICM42688_TMST_FSYNCL               0x2C
+#define ICM42688_INT_STATUS                0x2D
+#define ICM42688_FIFO_COUNTH               0x2E
+#define ICM42688_FIFO_COUNTL               0x2F
+#define ICM42688_FIFO_DATA                 0x30
+#define ICM42688_APEX_DATA0                0x31
+#define ICM42688_APEX_DATA1                0x32
+#define ICM42688_APEX_DATA2                0x33
+#define ICM42688_APEX_DATA3                0x34
+#define ICM42688_APEX_DATA4                0x35
+#define ICM42688_APEX_DATA5                0x36
+#define ICM42688_INT_STATUS2               0x37
+#define ICM42688_INT_STATUS3               0x38
+#define ICM42688_SIGNAL_PATH_RESET         0x4B
+#define ICM42688_INTF_CONFIG0              0x4C
+#define ICM42688_INTF_CONFIG1              0x4D
+#define ICM42688_PWR_MGMT0                 0x4E
+#define ICM42688_GYRO_CONFIG0              0x4F
+#define ICM42688_ACCEL_CONFIG0             0x50
+#define ICM42688_GYRO_CONFIG1              0x51
+#define ICM42688_GYRO_ACCEL_CONFIG0        0x52
+#define ICM42688_ACCEL_CONFIG1             0x53
+#define ICM42688_TMST_CONFIG               0x54
+#define ICM42688_APEX_CONFIG0              0x56
+#define ICM42688_SMD_CONFIG                0x57
+#define ICM42688_FIFO_CONFIG1              0x5F
+#define ICM42688_FIFO_CONFIG2              0x60
+#define ICM42688_FIFO_CONFIG3              0x61
+#define ICM42688_FSYNC_CONFIG              0x62
+#define ICM42688_INT_CONFIG0               0x63
+#define ICM42688_INT_CONFIG1               0x64
+#define ICM42688_INT_SOURCE0               0x65
+#define ICM42688_INT_SOURCE1               0x66
+#define ICM42688_INT_SOURCE3               0x68
+#define ICM42688_INT_SOURCE4               0x69
+#define ICM42688_FIFO_LOST_PKT0            0x6C
+#define ICM42688_FIFO_LOST_PKT1            0x6D
+#define ICM42688_SELF_TEST_CONFIG          0x70
+#define ICM42688_WHO_AM_I                  0x75 // should return 0x47
+#define ICM42688_REG_BANK_SEL              0x76
+
+// User Bank 1
+#define ICM42688_SENSOR_CONFIG0            0x03
+#define ICM42688_GYRO_CONFIG_STATIC2       0x0B
+#define ICM42688_GYRO_CONFIG_STATIC3       0x0C
+#define ICM42688_GYRO_CONFIG_STATIC4       0x0D
+#define ICM42688_GYRO_CONFIG_STATIC5       0x0E
+#define ICM42688_GYRO_CONFIG_STATIC6       0x0F
+#define ICM42688_GYRO_CONFIG_STATIC7       0x10
+#define ICM42688_GYRO_CONFIG_STATIC8       0x11
+#define ICM42688_GYRO_CONFIG_STATIC9       0x12
+#define ICM42688_GYRO_CONFIG_STATIC10      0x13
+#define ICM42688_XG_ST_DATA                0x5F
+#define ICM42688_YG_ST_DATA                0x60
+#define ICM42688_ZG_ST_DATA                0x61
+#define ICM42688_TMSTAL0                   0x63
+#define ICM42688_TMSTAL1                   0x64
+#define ICM42688_TMSTAL2                   0x62
+#define ICM42688_INTF_CONFIG4              0x7A
+#define ICM42688_INTF_CONFIG5              0x7B
+#define ICM42688_INTF_CONFIG6              0x7C
+
+// User Bank 2
+#define ICM42688_ACCEL_CONFIG_STATIC2      0x03
+#define ICM42688_ACCEL_CONFIG_STATIC3      0x04
+#define ICM42688_ACCEL_CONFIG_STATIC4      0x05
+#define ICM42688_XA_ST_DATA                0x3B
+#define ICM42688_YA_ST_DATA                0x3C
+#define ICM42688_ZA_ST_DATA                0x3D
+
+// User Bank 4
+#define ICM42688_APEX_CONFIG1              0x40
+#define ICM42688_APEX_CONFIG2              0x41
+#define ICM42688_APEX_CONFIG3              0x42
+#define ICM42688_APEX_CONFIG4              0x43
+#define ICM42688_APEX_CONFIG5              0x44
+#define ICM42688_APEX_CONFIG6              0x45
+#define ICM42688_APEX_CONFIG7              0x46
+#define ICM42688_APEX_CONFIG8              0x47
+#define ICM42688_APEX_CONFIG9              0x48
+#define ICM42688_ACCEL_WOM_X_THR           0x4A
+#define ICM42688_ACCEL_WOM_Y_THR           0x4B
+#define ICM42688_ACCEL_WOM_Z_THR           0x4C
+#define ICM42688_INT_SOURCE6               0x4D
+#define ICM42688_INT_SOURCE7               0x4E
+#define ICM42688_INT_SOURCE8               0x4F
+#define ICM42688_INT_SOURCE9               0x50
+#define ICM42688_INT_SOURCE10              0x51
+#define ICM42688_OFFSET_USER0              0x77
+#define ICM42688_OFFSET_USER1              0x78
+#define ICM42688_OFFSET_USER2              0x79
+#define ICM42688_OFFSET_USER3              0x7A
+#define ICM42688_OFFSET_USER4              0x7B
+#define ICM42688_OFFSET_USER5              0x7C
+#define ICM42688_OFFSET_USER6              0x7D
+#define ICM42688_OFFSET_USER7              0x7E
+#define ICM42688_OFFSET_USER8              0x7F
+
+#define ICM42688_ADDRESS           0x68   // Address of ICM42688 accel/gyro when ADO = 0
+
+#define AFS_2G  0x03
+#define AFS_4G  0x02
+#define AFS_8G  0x01
+#define AFS_16G 0x00 // default
+
+#define GFS_2000DPS   0x00   // default
+#define GFS_1000DPS   0x01
+#define GFS_500DPS    0x02
+#define GFS_250DPS    0x03
+#define GFS_125DPS    0x04
+#define GFS_62_50DPS  0x05
+#define GFS_31_25DPS  0x06
+#define GFS_15_625DPS 0x07
+
+// Low Noise mode
+#define AODR_32kHz    0x01   
+#define AODR_16kHz    0x02
+#define AODR_8kHz     0x03
+#define AODR_4kHz     0x04
+#define AODR_2kHz     0x05
+#define AODR_1kHz     0x06  // default
+//Low Noise or Low Power modes
+#define AODR_500Hz    0x0F
+#define AODR_200Hz    0x07
+#define AODR_100Hz    0x08
+#define AODR_50Hz     0x09
+#define AODR_25Hz     0x0A
+#define AODR_12_5Hz   0x0B
+// Low Power mode
+#define AODR_6_25Hz   0x0C  
+#define AODR_3_125Hz  0x0D
+#define AODR_1_5625Hz 0x0E
+
+#define GODR_32kHz  0x01   
+#define GODR_16kHz  0x02
+#define GODR_8kHz   0x03
+#define GODR_4kHz   0x04
+#define GODR_2kHz   0x05
+#define GODR_1kHz   0x06 // default
+#define GODR_500Hz  0x0F
+#define GODR_200Hz  0x07
+#define GODR_100Hz  0x08
+#define GODR_50Hz   0x09
+#define GODR_25Hz   0x0A
+#define GODR_12_5Hz 0x0B
+
+#define aMode_OFF 0x01
+#define aMode_LP  0x02
+#define aMode_LN  0x03
+
+#define gMode_OFF 0x00
+#define gMode_SBY 0x01
+#define gMode_LN  0x03
+
+#endif

lib/ICM42688/MMC5983MA.h

@@ -0,0 +1,63 @@
+/* 06/14/2020 Copyright Tlera Corporation
+
+    Created by Kris Winer
+
+  This sketch uses SDA/SCL on pins 21/20 (Ladybug default), respectively, and it uses the Ladybug STM32L432 Breakout Board.
+  The MMC5983MA is a low power magnetometer, here used as 3 DoF in a 9 DoF absolute orientation solution.
+
+  Library may be used freely and without limit with attribution.
+
+*/
+
+#ifndef MMC5983MA_h
+#define MMC5983MA_h
+
+//Register map for MMC5983MA'
+//http://www.memsic.com/userfiles/files/DataSheets/Magnetic-Sensors-Datasheets/MMC5983MA_Datasheet.pdf
+#define MMC5983MA_XOUT_0        0x00
+#define MMC5983MA_XOUT_1        0x01
+#define MMC5983MA_YOUT_0        0x02
+#define MMC5983MA_YOUT_1        0x03
+#define MMC5983MA_ZOUT_0        0x04
+#define MMC5983MA_ZOUT_1        0x05
+#define MMC5983MA_XYZOUT_2      0x06
+#define MMC5983MA_TOUT          0x07
+#define MMC5983MA_STATUS        0x08
+#define MMC5983MA_CONTROL_0     0x09
+#define MMC5983MA_CONTROL_1     0x0A
+#define MMC5983MA_CONTROL_2     0x0B
+#define MMC5983MA_CONTROL_3     0x0C
+#define MMC5983MA_PRODUCT_ID    0x2F
+
+#define MMC5983MA_ADDRESS       0x30
+
+// Sample rates
+#define MODR_ONESHOT   0x00
+#define MODR_1Hz       0x01
+#define MODR_10Hz      0x02
+#define MODR_20Hz      0x03
+#define MODR_50Hz      0x04
+#define MODR_100Hz     0x05
+#define MODR_200Hz     0x06 // BW = 0x01 only
+#define MODR_1000Hz    0x07 // BW = 0x11 only
+
+//Bandwidths
+#define MBW_100Hz 0x00  // 8 ms measurement time
+#define MBW_200Hz 0x01  // 4 ms
+#define MBW_400Hz 0x02  // 2 ms
+#define MBW_800Hz 0x03  // 0.5 ms
+
+// Set/Reset as a function of measurements
+#define MSET_1     0x00 // Set/Reset each data measurement
+#define MSET_25    0x01 // each 25 data measurements
+#define MSET_75    0x02
+#define MSET_100   0x03
+#define MSET_250   0x04
+#define MSET_500   0x05
+#define MSET_1000  0x06
+#define MSET_2000  0x07
+
+#define MMC5983MA_mRes (1.0f / 16384.0f) // mag sensitivity if using 18 bit data
+#define MMC5983MA_offset 131072.0f // mag range unsigned to signed
+
+#endif

lib/bmi160/BMI160.cpp

@@ -121,16 +121,10 @@ void BMI160::setMagDeviceAddress(uint8_t addr) {
     setRegister(BMI160_RA_MAG_IF_0_DEVADDR, addr << 1); // 0 bit of address is reserved and needs to be shifted
 }
 
-bool BMI160::setMagRegister(uint8_t addr, uint8_t value) {
+void BMI160::setMagRegister(uint8_t addr, uint8_t value) {
     setRegister(BMI160_RA_MAG_IF_4_WRITE_VALUE, value);
     setRegister(BMI160_RA_MAG_IF_3_WRITE_RA, addr);
     delay(3);
-    I2CdevMod::readByte(devAddr, BMI160_RA_ERR, buffer);
-    if (buffer[0] & BMI160_ERR_MASK_I2C_FAIL) {
-        printf("BMI160: mag register proxy write error\n");
-        return false;
-    }
-    return true;
 }
 
 /** Get Device ID.

lib/bmi160/BMI160.h

@@ -52,7 +52,7 @@ THE SOFTWARE.
 #define BMI160_RA_MAG_CONF             0x44
 #define BMI160_RA_MAG_IF_0_DEVADDR     0x4B
 #define BMI160_RA_MAG_IF_1_MODE        0x4C
-#define BMI160_RA_MAG_IF_2_READ_RA    0x4D
+#define BMI160_RA_MAG_IF_2_READ_RA     0x4D
 #define BMI160_RA_MAG_IF_3_WRITE_RA    0x4E
 #define BMI160_RA_MAG_IF_4_WRITE_VALUE 0x4F
 #define BMI160_RA_IF_CONF              0x6B
@@ -343,11 +343,12 @@ typedef enum {
 #define BMI160_FIFO_A_LEN 6
 
 #define BMI160_RA_ERR                        0x02
-#define BMI160_ERR_MASK_MAG_DRDY_ERR         0x10000000
-#define BMI160_ERR_MASK_DROP_CMD_ERR         0x01000000
-#define BMI160_ERR_MASK_I2C_FAIL             0x00100000
-#define BMI160_ERR_MASK_ERROR_CODE           0x00011110
-#define BMI160_ERR_MASK_CHIP_NOT_OPERABLE    0x00000001
+#define BMI160_ERR_MASK_MAG_DRDY_ERR         0b10000000
+#define BMI160_ERR_MASK_DROP_CMD_ERR         0b01000000
+// datasheet is unclear - reserved or i2c_fail_err?
+// #define BMI160_ERR_MASK_I2C_FAIL             0b00100000
+#define BMI160_ERR_MASK_ERROR_CODE           0b00011110
+#define BMI160_ERR_MASK_CHIP_NOT_OPERABLE    0b00000001
 
 /**
  * Interrupt Latch Mode options
@@ -755,7 +756,7 @@ class BMI160 {
         void waitForMagDrdy();
         bool getSensorTime(uint32_t *v_sensor_time_u32);
         void setMagDeviceAddress(uint8_t addr);
-        bool setMagRegister(uint8_t addr, uint8_t value);
+        void setMagRegister(uint8_t addr, uint8_t value);
 
         bool getErrReg(uint8_t* out);
     private:

lib/bno080/BNO080.cpp

@@ -159,6 +159,7 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I
 	if (receivePacket() == true)
 	{
 		if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
+		{
 			if (_printDebug == true)
 			{
 				_debugPort->print(F("SW Version Major: 0x"));
@@ -176,6 +177,7 @@ boolean BNO080::beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_I
 				_debugPort->println(SW_Version_Patch, HEX);
 			}
 			return (true);
+		}
 	}
 
 	return (false); //Something went wrong
@@ -1036,17 +1038,20 @@ bool BNO080::readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t words
 // See 5.2.1 on BNO08X datasheet.
 // Wait For both of these packets specifically up to a max time and exit
 // after both packets are read or max waiting time is reached.
-void BNO080::waitForCompletedReset(void)
+void BNO080::waitForCompletedReset(uint32_t timeout)
 {
 	uint32_t tInitialResetTimeMS = millis();
 	bool tResetCompleteReceived = false;
 	bool tUnsolicitedResponseReceived = false;
 	shtpHeader[2] = 0; // Make sure we aren't reading old data.
 	shtpData[0] = 0;
 
-	// Wait max 5s for the two packets. OR Until we get both reset responses.
-	while (millis() - tInitialResetTimeMS < 5000 &&
+	// Wait requested timeout for the two packets. OR Until we get both reset responses.
+	while (millis() - tInitialResetTimeMS < timeout &&
 	       (!tResetCompleteReceived || !tUnsolicitedResponseReceived)) {
+		#ifdef ESP8266
+			ESP.wdtFeed();
+		#endif
 		receivePacket();
 		if (shtpHeader[2] == 1 && shtpData[0] == 0x01) {
 			tResetCompleteReceived = true;
@@ -1059,17 +1064,18 @@ void BNO080::waitForCompletedReset(void)
 
 //Send command to reset IC
 //Read all advertisement packets from sensor
-//The sensor has been seen to reset twice if we attempt too much too quickly.
-//This seems to work reliably.
 void BNO080::softReset(void)
 {
+	// after power-on sensor is resetting by itself
+	// let's handle that POTENTIAL reset with shorter timeout
+	// in case sensor was not resetted - like after issuing RESET command
+	waitForCompletedReset(1000);
 	shtpData[0] = 1; //Reset
 
 	//Attempt to start communication with sensor
 	sendPacket(CHANNEL_EXECUTABLE, 1); //Transmit packet on channel 1, 1 byte
-
-	waitForCompletedReset();
-	waitForCompletedReset();
+	// now that reset should occur for sure, so let's try with longer timeout
+	waitForCompletedReset(5000);
 }
 
 //Set the operating mode to "On"

lib/bno080/BNO080.h

@@ -156,7 +156,7 @@ class BNO080
 	void enableDebugging(Stream &debugPort = Serial); //Turn on debug printing. If user doesn't specify then Serial will be used.
 
 	void softReset();	  //Try to reset the IMU via software
-	void waitForCompletedReset();
+	void waitForCompletedReset(uint32_t timeout);
 	uint8_t resetReason(); //Query the IMU for the reason it last reset
 	void modeOn();	  //Use the executable channel to turn the BNO on
 	void modeSleep();	  //Use the executable channel to put the BNO to sleep