github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'SlimeVR-Tracker-ESP/main' into main

Browse Source
This commit is contained in:
LETBBI
2022-03-15 22:06:41 +09:00
parent 550c59f457 08cc9aca76
commit a9320194cd
46 changed files with 1957 additions and 977 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@@ -1,6 +1,3 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch
.vscode/*
build/

7
.vscode/extensions.json vendored
View File

@@ -1,7 +0,0 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

19
.vscode/settings.json vendored
View File

@@ -1,19 +0,0 @@
{
"files.associations": {
"cmath": "cpp",
"cstdlib": "cpp",
"array": "cpp",
"deque": "cpp",
"list": "cpp",
"string": "cpp",
"unordered_map": "cpp",
"vector": "cpp",
"string_view": "cpp",
"initializer_list": "cpp",
"ranges": "cpp",
"*.tcc": "cpp",
"memory": "cpp",
"random": "cpp",
"algorithm": "cpp"
}
}

275
ci/build.py
View File

@@ -1,118 +1,201 @@
import json
import os
import re
import shutil
import subprocess
from textwrap import dedent
from enum import Enum
from typing import List
COLOR_ESC = '\033['
COLOR_RESET = f'{COLOR_ESC}0m'
COLOR_GREEN = f'{COLOR_ESC}32m'
COLOR_RED = f'{COLOR_ESC}31m'
COLOR_CYAN = f'{COLOR_ESC}36m'
COLOR_GRAY = f'{COLOR_ESC}30;1m'
def build_for_platform(platform: str) -> int:
class Board(Enum):
SLIMEVR = "BOARD_SLIMEVR"
WROOM32 = "BOARD_WROOM32"
class DeviceConfiguration:
def __init__(self, platform: str, board: Board, platformio_board: str) -> None:
self.platform = platform
self.board = board
self.platformio_board = platformio_board
def get_platformio_section(self) -> str:
return f"""
[env:{self.platformio_board}]
platform = {self.platform}
board = {self.platformio_board}
"""
def filename(self) -> str:
return f"{self.platformio_board}.bin"
def build_header(self) -> str:
sda = ""
scl = ""
imu_int = ""
imu_int2 = ""
battery_level = ""
leds = True
if self.board == Board.SLIMEVR:
sda = "4"
scl = "5"
imu_int = "10"
imu_int2 = "13"
battery_level = "17"
elif self.board == Board.WROOM32:
sda = "21"
scl = "22"
imu_int = "23"
imu_int2 = "25"
battery_level = "36"
else:
raise Exception(f"Unknown board: {self.board.value}")
return f"""
#define IMU IMU_BNO085
#define SECOND_IMU IMU
#define BOARD {self.board.value}
#define BATTERY_MONITOR BAT_EXTERNAL
#define PIN_IMU_SDA {sda}
#define PIN_IMU_SCL {scl}
#define PIN_IMU_INT {imu_int}
#define PIN_IMU_INT_2 {imu_int2}
#define PIN_BATTERY_LEVEL {battery_level}
#define ENABLE_LEDS {leds.__str__().lower()}
#define BATTERY_SHIELD_RESISTANCE 180
#define IMU_ROTATION DEG_90
#define SECOND_IMU_ROTATION DEG_90
"""
def __str__(self) -> str:
return f"{self.platform}@{self.board.value}"
def get_matrix() -> List[DeviceConfiguration]:
matrix: List[DeviceConfiguration] = []
configFile = open("./ci/devices.json", "r")
config = json.load(configFile)
for deviceConfig in config:
matrix.append(DeviceConfiguration(
deviceConfig["platform"], Board[deviceConfig["board"]], deviceConfig["platformio_board"]))
return matrix
def prepare() -> None:
print(f"🡢 {COLOR_CYAN}Preparation{COLOR_RESET}")
print(f" 🡢 {COLOR_GRAY}Backing up src/defines.h{COLOR_RESET}")
shutil.copy("src/defines.h", "src/defines.h.bak")
board = "BOARD_"
if platform == "esp12e":
board += "SLIMEVR"
elif platform == "d1_mini_lite":
board += "WEMOSD1MINI"
elif platform == "esp01":
board += "ESP01"
elif platform == "esp32dev":
board += "WROOM32"
else:
raise Exception("Unknown platform")
print(f" 🡢 {COLOR_GRAY}Backing up platformio.ini{COLOR_RESET}")
shutil.copy("./platformio.ini", "platformio.ini.bak")
with open("src/defines.h", "r") as f:
lines = f.read().rstrip().lstrip()
lines = re.sub("#define BOARD .*", f"#define BOARD {board}", lines)
print(f" 🡢 {COLOR_GRAY}Copying over build/platformio.ini{COLOR_RESET}")
shutil.copy("./ci/platformio.ini", "platformio.ini")
with open("src/defines.h", "wt") as f:
f.write(lines)
if os.path.exists("./build"):
print(f" 🡢 {COLOR_GRAY}Removing existing build folder...{COLOR_RESET}")
shutil.rmtree("./build")
print("Building for platform: " + platform)
ret_code = os.system(f"platformio run -e {platform}")
print(f" 🡢 {COLOR_GRAY}Creating build folder...{COLOR_RESET}")
os.mkdir("./build")
if ret_code == 0:
print("Build succeeded")
print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
shutil.copy(f".pio/build/{platform}/firmware.bin",
f"build/{platform}.bin")
else:
print("Build failed")
def cleanup() -> None:
print(f"🡢 {COLOR_CYAN}Cleanup{COLOR_RESET}")
print(f" 🡢 {COLOR_GRAY}Restoring src/defines.h...{COLOR_RESET}")
shutil.copy("src/defines.h.bak", "src/defines.h")
print(f" 🡢 {COLOR_GRAY}Removing src/defines.h.bak...{COLOR_RESET}")
os.remove("src/defines.h.bak")
return ret_code
print(f" 🡢 {COLOR_GRAY}Restoring platformio.ini...{COLOR_RESET}")
shutil.copy("platformio.ini.bak", "platformio.ini")
print(f" 🡢 {COLOR_GRAY}Removing platformio.ini.bak...{COLOR_RESET}")
os.remove("platformio.ini.bak")
print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
def build() -> int:
print(f"🡢 {COLOR_CYAN}Build{COLOR_RESET}")
failed_builds: List[str] = []
code = 0
matrix = get_matrix()
with open("./platformio.ini", "a") as f1:
for device in matrix:
f1.write(device.get_platformio_section())
for device in matrix:
print(f" 🡢 {COLOR_CYAN}Building for {device.platform}{COLOR_RESET}")
status = build_for_device(device)
if status == False:
failed_builds.append(device.platformio_board)
if len(failed_builds) > 0:
print(f" 🡢 {COLOR_RED}Failed!{COLOR_RESET}")
for failed_build in failed_builds:
print(f" 🡢 {COLOR_RED}{failed_build}{COLOR_RESET}")
code = 1
else:
print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
return code
def build_for_device(device: DeviceConfiguration) -> bool:
success = True
print(f"::group::Build {device}")
with open("src/defines.h", "wt") as f:
f.write(device.build_header())
code = os.system(
f"platformio run -e {device.platformio_board}")
if code == 0:
shutil.copy(f".pio/build/{device.platformio_board}/firmware.bin",
f"build/{device.filename()}")
print(f" 🡢 {COLOR_GREEN}Success!{COLOR_RESET}")
else:
success = False
print(f" 🡢 {COLOR_RED}Failed!{COLOR_RESET}")
print(f"::endgroup::")
return success
def main() -> None:
shutil.copy("platformio.ini", "platformio.ini.bak")
prepare()
code = build()
cleanup()
if os.path.exists("./build"):
shutil.rmtree("./build")
os.mkdir("./build")
with open("platformio.ini", "r") as f:
lines = f.read().rstrip().lstrip()
matches = re.search(
"\[env:.*\]\nplatform = .*\nboard = .*", lines)
if matches:
# remove lines
lines = lines.replace(matches.group(0), "")
# add new lines
lines += dedent("""
; ESP32 Dev Board
[env:esp32dev]
platform = espressif32
board = esp32dev
; SlimeVR PCB
[env:esp12e]
platform = espressif8266
board = esp12e
; ESP01 (512kB flash)
[env:esp01]
platform = espressif8266
board = esp01
; WEMOS D1 mini lite (1MB flash)
[env:d1_mini_lite]
platform = espressif8266
board = d1_mini_lite
""")
with open("platformio.ini", "wt") as f:
f.write(lines)
# TODO: Build a matrix of all platforms and all IMUs
esp32_ret_code = build_for_platform("esp32dev")
esp12e_ret_code = build_for_platform("esp12e")
esp01_ret_code = build_for_platform("esp01")
d1_mini_lite_ret_code = build_for_platform("d1_mini_lite")
shutil.copy("platformio.ini.bak", "platformio.ini")
os.remove("platformio.ini.bak")
print("\n\n\n\n")
if esp32_ret_code != 0:
print("Error occurred while building for ESP32 Dev Board")
if esp12e_ret_code != 0:
print("Error occurred while building for SlimeVR PCB")
if esp01_ret_code != 0:
print("Error occurred while building for ESP01")
if d1_mini_lite_ret_code != 0:
print("Error occurred while building for WEMOS D1 mini lite")
if esp32_ret_code != 0 or esp12e_ret_code != 0 or esp01_ret_code != 0 or d1_mini_lite_ret_code != 0:
print("One or more builds failed")
os._exit(1)
os._exit(code)
if __name__ == "__main__":

12
ci/devices.json Normal file
View File

@@ -0,0 +1,12 @@
[
{
"platform": "espressif8266",
"platformio_board": "esp12e",
"board": "SLIMEVR"
},
{
"platform": "espressif32",
"platformio_board": "esp32dev",
"board": "WROOM32"
}
]

8
ci/platformio.ini Normal file
View File

@@ -0,0 +1,8 @@
[env]
lib_deps=
https://github.com/SlimeVR/CmdParser.git
monitor_speed = 115200
framework = arduino
build_flags =
-O2
build_unflags = -Os

25
lib/bno080/BNO080.cpp
View File

@@ -1105,6 +1105,31 @@ uint8_t BNO080::resetReason()
return (0);
}
//Get the last error if it exists
BNO080Error BNO080::readError()
{
sendCommand(COMMAND_ERRORS);
BNO080Error err;
err.error_source = 255;
//Now we wait for response
if (receivePacket() == true)
{
if (shtpData[0] == SHTP_REPORT_COMMAND_RESPONSE && shtpData[2] == COMMAND_ERRORS)
{
err.severity = shtpData[5];
err.error_sequence_number = shtpData[6];
err.error_source = shtpData[7];
err.error = shtpData[8];
err.error_module = shtpData[9];
err.error_code = shtpData[10];
}
}
return err;
}
//Given a register value and a Q point, convert to float
//See https://en.wikipedia.org/wiki/Q_(number_format)
float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint)

11
lib/bno080/BNO080.h
View File

@@ -139,6 +139,15 @@ const byte CHANNEL_GYRO = 5;
#define MAX_PACKET_SIZE 128 //Packets can be up to 32k but we don't have that much RAM.
#define MAX_METADATA_SIZE 9 //This is in words. There can be many but we mostly only care about the first 9 (Qs, range, etc)
struct BNO080Error {
uint8_t severity;
uint8_t error_sequence_number;
uint8_t error_source;
uint8_t error;
uint8_t error_module;
uint8_t error_code;
};
class BNO080
{
public:
@@ -277,6 +286,8 @@ public:
void frsReadRequest(uint16_t recordID, uint16_t readOffset, uint16_t blockSize);
bool readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t wordsToRead);
BNO080Error readError();
//Global Variables
uint8_t shtpHeader[4]; //Each packet has a header of 4 bytes
uint8_t shtpData[MAX_PACKET_SIZE];

385
lib/magneto/magneto1.4.cpp
View File

@@ -3,157 +3,162 @@
//Magneto 1.3 4/24/2020
void CalculateCalibration(float *buf, int sampleCount, float BAinv[4][3])
{
double* D;
double J, hmb;
int i, j, index;
double maxval, norm, btqb, hm, norm1, norm2, norm3;
double x, y, z, x2, nxsrej, xs, xave;
double* raw; //raw obs
double xs = 0, xave = 0;
//
// calculate mean (norm) and standard deviation for possible outlier rejection
//
xs = 0;
xave = 0;
for (i = 0; i < sampleCount; i++)
for (int i = 0; i < sampleCount; i++)
{
x = buf[i * 3 + 0];
y = buf[i * 3 + 1];
z = buf[i * 3 + 2];
x2 = x * x + y * y + z * z;
double x = buf[i * 3 + 0];
double y = buf[i * 3 + 1];
double z = buf[i * 3 + 2];
double x2 = x * x + y * y + z * z;
xs += x2;
xave += sqrt(x2);
}
xave = xave / sampleCount; //mean vector length
xs = sqrt(xs / sampleCount - (xave * xave)); //std. dev.
// summarize statistics, give user opportunity to reject outlying measurements
nxsrej = 0;
// third time through!
// allocate array space for accepted measurements
D = (double*)malloc(10 * sampleCount * sizeof(double));
raw = (double*)malloc(3 * sampleCount * sizeof(double));
double* D = new double[10 * sampleCount];
double* raw = new double[3 * sampleCount];
j = 0; //array index for good measurements
// printf("\r\nAccepted measurements (file index, internal index, ...)\r\n");
for (i = 0; i < sampleCount; i++)
{
x = buf[i * 3 + 0];
y = buf[i * 3 + 1];
z = buf[i * 3 + 2];
x2 = sqrt(x * x + y * y + z * z); //vector length
x2 = fabs(x2 - xave) / xs; //standard deviation from mean
if ((nxsrej == 0) || (x2 <= nxsrej)) {
// accepted measurement
// printf("%d, %d: %6.1f %6.1f %6.1f\r\n",i,j,x,y,z);
// summarize statistics, give user opportunity to reject outlying measurements
double nxsrej = 0;
raw[3 * j] = x;
raw[3 * j + 1] = y;
raw[3 * j + 2] = z;
D[j] = x * x;
D[sampleCount + j] = y * y;
D[sampleCount * 2 + j] = z * z;
D[sampleCount * 3 + j] = 2.0 * y * z;
D[sampleCount * 4 + j] = 2.0 * x * z;
D[sampleCount * 5 + j] = 2.0 * x * y;
D[sampleCount * 6 + j] = 2.0 * x;
D[sampleCount * 7 + j] = 2.0 * y;
D[sampleCount * 8 + j] = 2.0 * z;
D[sampleCount * 9 + j] = 1.0;
j++; //count good measurements
int j = 0; //array index for good measurements
// printf("\r\nAccepted measurements (file index, internal index, ...)\r\n");
for (int i = 0; i < sampleCount; i++)
{
double x = buf[i * 3 + 0];
double y = buf[i * 3 + 1];
double z = buf[i * 3 + 2];
double x2 = sqrt(x * x + y * y + z * z); //vector length
x2 = fabs(x2 - xave) / xs; //standard deviation from mean
if ((nxsrej == 0) || (x2 <= nxsrej)) {
// accepted measurement
// printf("%d, %d: %6.1f %6.1f %6.1f\r\n",i,j,x,y,z);
raw[3 * j] = x;
raw[3 * j + 1] = y;
raw[3 * j + 2] = z;
D[j] = x * x;
D[sampleCount + j] = y * y;
D[sampleCount * 2 + j] = z * z;
D[sampleCount * 3 + j] = 2.0 * y * z;
D[sampleCount * 4 + j] = 2.0 * x * z;
D[sampleCount * 5 + j] = 2.0 * x * y;
D[sampleCount * 6 + j] = 2.0 * x;
D[sampleCount * 7 + j] = 2.0 * y;
D[sampleCount * 8 + j] = 2.0 * z;
D[sampleCount * 9 + j] = 1.0;
j++; //count good measurements
}
}
}
free(raw);
delete[] raw;
//printf("\r\nExpected norm of local field vector Hm? (Enter 0 for default %8.1f) ", xave);
//scanf("%lf", &hm);
//if (hm == 0.0) hm = xave;
//printf("\r\nSet Hm = %8.1f\r\n", hm);
hm = xave;
double hm = xave;
// allocate memory for matrix S
double S[10*10];
double* S = new double[10 * 10];
Multiply_Self_Transpose(S, D, 10, sampleCount);
free(D);
delete[] D;
// Create pre-inverted constraint matrix C
double C[6*6];
C[0] = 0.0; C[1] = 0.5; C[2] = 0.5; C[3] = 0.0; C[4] = 0.0; C[5] = 0.0;
C[6] = 0.5; C[7] = 0.0; C[8] = 0.5; C[9] = 0.0; C[10] = 0.0; C[11] = 0.0;
C[12] = 0.5; C[13] = 0.5; C[14] = 0.0; C[15] = 0.0; C[16] = 0.0; C[17] = 0.0;
C[18] = 0.0; C[19] = 0.0; C[20] = 0.0; C[21] = -0.25; C[22] = 0.0; C[23] = 0.0;
C[24] = 0.0; C[25] = 0.0; C[26] = 0.0; C[27] = 0.0; C[28] = -0.25; C[29] = 0.0;
C[30] = 0.0; C[31] = 0.0; C[32] = 0.0; C[33] = 0.0; C[34] = 0.0; C[35] = -0.25;
double S11[6 * 6];
double* S11 = new double[6 * 6];
Get_Submatrix(S11, 6, 6, S, 10, 0, 0);
double S12[6 * 4];
double* S12 = new double[6 * 4];
Get_Submatrix(S12, 6, 4, S, 10, 0, 6);
double S12t[4 * 6];
double* S12t = new double[4 * 6];
Get_Submatrix(S12t, 4, 6, S, 10, 6, 0);
double S22[4 * 4];
double* S22 = new double[4 * 4];
Get_Submatrix(S22, 4, 4, S, 10, 6, 6);
delete[] S;
double S22_1[4 * 4];
for (i = 0; i < 16; i++)
S22_1[i] = S22[i];
Choleski_LU_Decomposition(S22_1, 4);
Choleski_LU_Inverse(S22_1, 4);
Choleski_LU_Decomposition(S22, 4);
Choleski_LU_Inverse(S22, 4);
// Calculate S22a = S22_1 * S12t 4*6 = 4x4 * 4x6 C = AB
double S22a[4 * 6];
Multiply_Matrices(S22a, S22_1, 4, 4, S12t, 6);
// Calculate S22a = S22 * S12t 4*6 = 4x4 * 4x6 C = AB
double* S22a = new double[4 * 6];
Multiply_Matrices(S22a, S22, 4, 4, S12t, 6);
delete[] S22;
delete[] S12t;
// Then calculate S22b = S12 * S22a ( 6x6 = 6x4 * 4x6)
double S22b[6 * 6];
double* S22b = new double[6 * 6];
Multiply_Matrices(S22b, S12, 6, 4, S22a, 6);
delete[] S12;
// Calculate SS = S11 - S22b
double SS[6 * 6];
for (i = 0; i < 36; i++)
double* SS = new double[6 * 6];
for (int i = 0; i < 36; i++)
SS[i] = S11[i] - S22b[i];
double E[6 * 6];
Multiply_Matrices(E, C, 6, 6, SS, 6);
delete[] S11;
delete[] S22b;
double SSS[6 * 6];
// Create pre-inverted constraint matrix C
double* C = new double[6 * 6]{
0.0, 0.5, 0.5, 0.0, 0.0, 0.0,
0.5, 0.0, 0.5, 0.0, 0.0, 0.0,
0.5, 0.5, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, -0.25, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, -0.25, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, -0.25
};
double* E = new double[6 * 6];
Multiply_Matrices(E, C, 6, 6, SS, 6);
delete[] C;
delete[] SS;
double* SSS = new double[6 * 6];
Hessenberg_Form_Elementary(E, SSS, 6);
double eigen_real[6];
double eigen_imag[6];
QR_Hessenberg_Matrix(E, SSS, eigen_real, eigen_imag, 6, 100);
index = 0;
maxval = eigen_real[0];
for (i = 1; i < 6; i++)
int index = 0;
{
if (eigen_real[i] > maxval)
double eigen_real[6];
double eigen_imag[6];
QR_Hessenberg_Matrix(E, SSS, eigen_real, eigen_imag, 6, 100);
delete[] E;
double maxval = eigen_real[0];
for (int i = 1; i < 6; i++)
{
maxval = eigen_real[i];
index = i;
if (eigen_real[i] > maxval)
{
maxval = eigen_real[i];
index = i;
}
}
}
double v1[6];
double* v1 = new double[6];
v1[0] = SSS[index];
v1[1] = SSS[index + 6];
v1[2] = SSS[index + 12];
v1[3] = SSS[index + 18];
v1[4] = SSS[index + 24];
v1[5] = SSS[index + 30];
delete[] SSS;
// normalize v1
norm = sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3] + v1[4] * v1[4] + v1[5] * v1[5]);
v1[0] /= norm;
v1[1] /= norm;
v1[2] /= norm;
v1[3] /= norm;
v1[4] /= norm;
v1[5] /= norm;
{
double norm = sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2] + v1[3] * v1[3] + v1[4] * v1[4] + v1[5] * v1[5]);
v1[0] /= norm;
v1[1] /= norm;
v1[2] /= norm;
v1[3] /= norm;
v1[4] /= norm;
v1[5] /= norm;
}
if (v1[0] < 0.0)
{
@@ -166,114 +171,134 @@ void CalculateCalibration(float *buf, int sampleCount, float BAinv[4][3])
}
// Calculate v2 = S22a * v1 ( 4x1 = 4x6 * 6x1)
double v2[4];
double* v2 = new double[4];
Multiply_Matrices(v2, S22a, 4, 6, v1, 1);
delete[] S22a;
double v[10];
double* U = new double[3];
double* Q = new double[3 * 3];
double J;
{
double v[10];
v[0] = v1[0];
v[1] = v1[1];
v[2] = v1[2];
v[3] = v1[3];
v[4] = v1[4];
v[5] = v1[5];
delete[] v1;
v[6] = -v2[0];
v[7] = -v2[1];
v[8] = -v2[2];
v[9] = -v2[3];
delete[] v2;
v[0] = v1[0];
v[1] = v1[1];
v[2] = v1[2];
v[3] = v1[3];
v[4] = v1[4];
v[5] = v1[5];
v[6] = -v2[0];
v[7] = -v2[1];
v[8] = -v2[2];
v[9] = -v2[3];
Q[0] = v[0];
Q[1] = v[5];
Q[2] = v[4];
Q[3] = v[5];
Q[4] = v[1];
Q[5] = v[3];
Q[6] = v[4];
Q[7] = v[3];
Q[8] = v[2];
double Q [3 * 3];
U[0] = v[6];
U[1] = v[7];
U[2] = v[8];
Q[0] = v[0];
Q[1] = v[5];
Q[2] = v[4];
Q[3] = v[5];
Q[4] = v[1];
Q[5] = v[3];
Q[6] = v[4];
Q[7] = v[3];
Q[8] = v[2];
J = v[9];
}
double U[3];
double* B = new double[3];
{
double Q_1[3 * 3];
for (int i = 0; i < 9; i++)
Q_1[i] = Q[i];
Choleski_LU_Decomposition(Q_1, 3);
Choleski_LU_Inverse(Q_1, 3);
U[0] = v[6];
U[1] = v[7];
U[2] = v[8];
double Q_1[3 * 3];
for (i = 0; i < 9; i++)
Q_1[i] = Q[i];
Choleski_LU_Decomposition(Q_1, 3);
Choleski_LU_Inverse(Q_1, 3);
// Calculate B = Q-1 * U ( 3x1 = 3x3 * 3x1)
double B[3];
Multiply_Matrices(B, Q_1, 3, 3, U, 1);
B[0] = -B[0]; // x-axis combined bias
B[1] = -B[1]; // y-axis combined bias
B[2] = -B[2]; // z-axis combined bias
// Calculate B = Q-1 * U ( 3x1 = 3x3 * 3x1)
Multiply_Matrices(B, Q_1, 3, 3, U, 1);
delete[] U;
B[0] = -B[0]; // x-axis combined bias
B[1] = -B[1]; // y-axis combined bias
B[2] = -B[2]; // z-axis combined bias
}
// First calculate QB = Q * B ( 3x1 = 3x3 * 3x1)
double QB[3];
Multiply_Matrices(QB, Q, 3, 3, B, 1);
double btqb;
{
double QB[3];
Multiply_Matrices(QB, Q, 3, 3, B, 1);
// Then calculate btqb = BT * QB ( 1x1 = 1x3 * 3x1)
Multiply_Matrices(&btqb, B, 1, 3, QB, 1);
// Calculate hmb = sqrt(btqb - J).
J = v[9];
hmb = sqrt(btqb - J);
// Then calculate btqb = BT * QB ( 1x1 = 1x3 * 3x1)
Multiply_Matrices(&btqb, B, 1, 3, QB, 1);
}
// Calculate SQ, the square root of matrix Q
double SSSS [3 * 3];
double* SSSS = new double[3 * 3];
Hessenberg_Form_Elementary(Q, SSSS, 3);
double eigen_real3[3];
double eigen_imag3[3];
QR_Hessenberg_Matrix(Q, SSSS, eigen_real3, eigen_imag3, 3, 100);
double* Dz = new double[3 * 3]{0};
{
double eigen_real3[3];
double eigen_imag3[3];
QR_Hessenberg_Matrix(Q, SSSS, eigen_real3, eigen_imag3, 3, 100);
delete[] Q;
// normalize eigenvectors
norm1 = sqrt(SSSS[0] * SSSS[0] + SSSS[3] * SSSS[3] + SSSS[6] * SSSS[6]);
SSSS[0] /= norm1;
SSSS[3] /= norm1;
SSSS[6] /= norm1;
norm2 = sqrt(SSSS[1] * SSSS[1] + SSSS[4] * SSSS[4] + SSSS[7] * SSSS[7]);
SSSS[1] /= norm2;
SSSS[4] /= norm2;
SSSS[7] /= norm2;
norm3 = sqrt(SSSS[2] * SSSS[2] + SSSS[5] * SSSS[5] + SSSS[8] * SSSS[8]);
SSSS[2] /= norm3;
SSSS[5] /= norm3;
SSSS[8] /= norm3;
Dz[0] = sqrt(eigen_real3[0]);
Dz[4] = sqrt(eigen_real3[1]);
Dz[8] = sqrt(eigen_real3[2]);
}
double Dz[3 * 3];
for (i = 0; i < 9; i++)
Dz[i] = 0.0;
Dz[0] = sqrt(eigen_real3[0]);
Dz[4] = sqrt(eigen_real3[1]);
Dz[8] = sqrt(eigen_real3[2]);
{
// normalize eigenvectors
double norm = sqrt(SSSS[0] * SSSS[0] + SSSS[3] * SSSS[3] + SSSS[6] * SSSS[6]);
SSSS[0] /= norm;
SSSS[3] /= norm;
SSSS[6] /= norm;
norm = sqrt(SSSS[1] * SSSS[1] + SSSS[4] * SSSS[4] + SSSS[7] * SSSS[7]);
SSSS[1] /= norm;
SSSS[4] /= norm;
SSSS[7] /= norm;
norm = sqrt(SSSS[2] * SSSS[2] + SSSS[5] * SSSS[5] + SSSS[8] * SSSS[8]);
SSSS[2] /= norm;
SSSS[5] /= norm;
SSSS[8] /= norm;
}
double vdz[3 * 3];
Multiply_Matrices(vdz, SSSS, 3, 3, Dz, 3);
double* SQ = new double[3 * 3];
{
double vdz[3 * 3];
Multiply_Matrices(vdz, SSSS, 3, 3, Dz, 3);
delete[] Dz;
Transpose_Square_Matrix(SSSS, 3);
Multiply_Matrices(SQ, vdz, 3, 3, SSSS, 3);
delete[] SSSS;
}
Transpose_Square_Matrix(SSSS, 3);
double SQ[3 * 3];
Multiply_Matrices(SQ, vdz, 3, 3, SSSS, 3);
// hm = 0.569;
double A_1[3 * 3];
double* A_1 = new double[3 * 3];
// Calculate hmb = sqrt(btqb - J).
double hmb = sqrt(btqb - J);
for (i = 0; i < 9; i++)
for (int i = 0; i < 9; i++)
A_1[i] = SQ[i] * hm / hmb;
delete[] SQ;
for (i = 0; i < 3; i++)
for (int i = 0; i < 3; i++)
BAinv[0][i] = B[i];
for (i = 0; i < 3; i++)
delete[] B;
for (int i = 0; i < 3; i++)
{
BAinv[i + 1][0] = A_1[i * 3];
BAinv[i + 1][1] = A_1[i * 3 + 1];
BAinv[i + 1][2] = A_1[i * 3 + 2];
}
delete[] A_1;
}
@@ -1580,8 +1605,8 @@ void BackSubstitute_Real_Vector(double* H, double eigen_real[],
double* pH;
double* pV;
double x;
double u[4];
double v[2];
double u[4] = {0};
double v[2] = {0};
int i, j, k;
k = row;
@@ -1645,9 +1670,9 @@ void BackSubstitute_Complex_Vector(double* H, double eigen_real[],
double* pH;
double* pV;
double x, y;
double u[4];
double v[2];
double w[2];
double u[4] = {0};
double v[2] = {0};
double w[2] = {0};
int i, j, k;
k = row - 1;
@@ -2060,7 +2085,7 @@ int Upper_Triangular_Solve(double* U, double B[], double x[], int n)
int Upper_Triangular_Inverse(double* U, int n)
{
int i, j, k;
double* p_i, * p_j, * p_k;
double* p_i, * p_k;
double sum;
// Invert the diagonal elements of the upper triangular matrix U.

6
platformio.ini
View File

@@ -18,6 +18,7 @@ monitor_speed = 115200
framework = arduino
build_flags =
;If you want to set hardcoded WiFi SSID and password, uncomment and edit the lines below
;To uncomment, only remove ";" and leave the two spaces in front of the tags
; '" - quotes are necessary!
; -DWIFI_CREDS_SSID='"SSID"'
; -DWIFI_CREDS_PASSWD='"PASSWORD"'
@@ -40,6 +41,9 @@ build_unflags = -Os
[env:esp12e]
platform = espressif8266
board = esp12e
; Comment out this line below if you have any trouble uploading the firmware
; and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed
upload_speed = 921600
; Uncomment below if you want to build for ESP-01
;[env:esp01_1m]
@@ -51,3 +55,5 @@ board = esp12e
; [env:esp32]
; platform = espressif32
; board = esp32dev
; Comment out this line below if you have any trouble uploading the firmware - and if it has a CP2102 on it (a square chip next to the usb port): change to 3000000 (3 million) for even faster upload speed
; upload_speed = 921600

2
src/batterymonitor.cpp
View File

@@ -39,7 +39,7 @@ void BatteryMonitor::Setup()
}
if (address == 0)
{
Serial.println(F("[ERR] MCP3021 not found on I2C bus"));
m_Logger.error("MCP3021 not found on I2C bus");
}
#endif
}

3
src/batterymonitor.h
View File

@@ -29,6 +29,7 @@
#include <i2cscan.h>
#include <I2Cdev.h>
#include "ledmgr.h"
#include "logging/Logger.h"
#if BATTERY_MONITOR == BAT_EXTERNAL
#ifndef PIN_BATTERY_LEVEL
@@ -59,6 +60,8 @@ private:
#endif
float voltage = -1;
float level = -1;
SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("BatteryMonitor");
};
#endif // SLIMEVR_BATTERYMONITOR_H_

19
src/debug.h
View File

@@ -23,6 +23,7 @@
#ifndef SLIMEVR_DEBUG_H_
#define SLIMEVR_DEBUG_H_
#include "consts.h"
#include "logging/Level.h"
#define IMU_MPU6050_RUNTIME_CALIBRATION // Comment to revert to startup/traditional-calibration
#define BNO_USE_ARVR_STABILIZATION // Comment to not use stabilization for BNO085+ IMUs
@@ -33,11 +34,15 @@
#define ENABLE_TAP false // monitor accel for (triple) tap events and send them. Uses more cpu, disable if problems. Server does nothing with value so disabled atm
//Debug information
//#define FULL_DEBUG
#define LOG_LEVEL LOG_LEVEL_DEBUG
#if LOG_LEVEL == LOG_LEVEL_TRACE
#define FULL_DEBUG
#endif
#define serialDebug false // Set to true to get Serial output for debugging
#define serialBaudRate 115200
#define UPDATE_IMU_UNCONNECTED 1
//#define SEND_UPDATES_UNCONNECTED 1
#define LED_INTERVAL_STANDBUY 10000
#define STATUS_PRINT_INTERVAL 15000
#define ENABLE_LEDS true
@@ -71,7 +76,11 @@
#define I2C_SPEED 400000
#define FIRMWARE_BUILD_NUMBER 9
#define FIRMWARE_VERSION "0.2.0"
// Send inspection packets over the network to a profiler
// Not recommended for production
#define ENABLE_INSPECTION false
#define FIRMWARE_BUILD_NUMBER 10
#define FIRMWARE_VERSION "0.2.1"
#endif // SLIMEVR_DEBUG_H_

12
src/ledmgr.cpp
View File

@@ -24,6 +24,8 @@
namespace LEDManager
{
SlimeVR::Logging::Logger logger("LEDManager");
/*!
* @brief Turn a LED on
* @param pin
@@ -138,7 +140,7 @@ namespace LEDManager
break;
}
if(printStatus)
Serial.println("[STATUS] LOW BATTERY");
logger.debug("LOW BATTERY");
} else if((currentStatus & LED_STATUS_IMU_ERROR) > 0) {
count = IMU_ERROR_COUNT;
switch(currentStage) {
@@ -154,7 +156,7 @@ namespace LEDManager
break;
}
if(printStatus)
Serial.println("[STATUS] IMU ERROR");
logger.debug("IMU ERROR");
} else if((currentStatus & LED_STATUS_WIFI_CONNECTING) > 0) {
count = WIFI_CONNECTING_COUNT;
switch(currentStage) {
@@ -170,7 +172,7 @@ namespace LEDManager
break;
}
if(printStatus)
Serial.println("[STATUS] WIFI CONNECTING");
logger.debug("WIFI CONNECTING");
} else if((currentStatus & LED_STATUS_SERVER_CONNECTING) > 0) {
count = SERVER_CONNECTING_COUNT;
switch(currentStage) {
@@ -186,10 +188,10 @@ namespace LEDManager
break;
}
if(printStatus)
Serial.println("[STATUS] SERVER CONNECTING");
logger.debug("SERVER CONNECTING");
} else {
if(printStatus)
Serial.println("[STATUS] OK");
logger.debug("OK");
#if defined(LED_INTERVAL_STANDBUY) && LED_INTERVAL_STANDBUY > 0
count = 1;
switch(currentStage) {

1
src/ledmgr.h
View File

@@ -25,6 +25,7 @@
#include <Arduino.h>
#include "globals.h"
#include "logging/Logger.h"
#define LED_STATUS_SERVER_CONNECTING 2
#define LED_STATUS_WIFI_CONNECTING 4

28
src/logging/Level.cpp Normal file
View File

@@ -0,0 +1,28 @@
#include "Level.h"
namespace SlimeVR
{
namespace Logging
{
const char *levelToString(Level level)
{
switch (level)
{
case TRACE:
return "TRACE";
case DEBUG:
return "DEBUG";
case INFO:
return "INFO";
case WARN:
return "WARN";
case ERROR:
return "ERROR";
case FATAL:
return "FATAL";
default:
return "UNKNOWN";
}
}
}
}

29
src/logging/Level.h Normal file
View File

@@ -0,0 +1,29 @@
#ifndef LOGGING_LEVEL_H
#define LOG_LEVEL_TRACE 0
#define LOG_LEVEL_DEBUG 1
#define LOG_LEVEL_INFO 2
#define LOG_LEVEL_WARN 3
#define LOG_LEVEL_ERROR 4
#define LOG_LEVEL_FATAL 5
namespace SlimeVR
{
namespace Logging
{
enum Level
{
TRACE = LOG_LEVEL_TRACE,
DEBUG = LOG_LEVEL_DEBUG,
INFO = LOG_LEVEL_INFO,
WARN = LOG_LEVEL_WARN,
ERROR = LOG_LEVEL_ERROR,
FATAL = LOG_LEVEL_FATAL
};
const char *levelToString(Level level);
}
}
#define LOGGING_LEVEL_H
#endif

82
src/logging/Logger.cpp Normal file
View File

@@ -0,0 +1,82 @@
#include "Logger.h"
namespace SlimeVR
{
namespace Logging
{
void Logger::setTag(const char *tag)
{
m_Tag = (char *)malloc(strlen(tag) + 1);
strcpy(m_Tag, tag);
}
void Logger::trace(const char *format, ...)
{
va_list args;
va_start(args, format);
log(TRACE, format, args);
va_end(args);
}
void Logger::debug(const char *format, ...)
{
va_list args;
va_start(args, format);
log(DEBUG, format, args);
va_end(args);
}
void Logger::info(const char *format, ...)
{
va_list args;
va_start(args, format);
log(INFO, format, args);
va_end(args);
}
void Logger::warn(const char *format, ...)
{
va_list args;
va_start(args, format);
log(WARN, format, args);
va_end(args);
}
void Logger::error(const char *format, ...)
{
va_list args;
va_start(args, format);
log(ERROR, format, args);
va_end(args);
}
void Logger::fatal(const char *format, ...)
{
va_list args;
va_start(args, format);
log(FATAL, format, args);
va_end(args);
}
void Logger::log(Level level, const char *format, va_list args)
{
if (level < LOG_LEVEL)
{
return;
}
char buffer[256];
vsnprintf(buffer, 256, format, args);
char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2];
strcpy(buf, m_Prefix);
if (m_Tag != nullptr)
{
strcat(buf, ":");
strcat(buf, m_Tag);
}
Serial.printf("[%-5s] [%s] %s\n", levelToString(level), buf, buffer);
}
}
}

109
src/logging/Logger.h Normal file
View File

@@ -0,0 +1,109 @@
#ifndef LOGGING_LOGGER_H
#define LOGGING_LOGGER_H
#include "Level.h"
#include "debug.h"
#include <Arduino.h>
namespace SlimeVR
{
namespace Logging
{
class Logger
{
public:
Logger(const char *prefix) : m_Prefix(prefix), m_Tag(nullptr){};
Logger(const char *prefix, const char *tag) : m_Prefix(prefix), m_Tag(nullptr)
{
setTag(tag);
};
~Logger()
{
if (m_Tag != nullptr)
{
free(m_Tag);
}
}
void setTag(const char *tag);
void trace(const char *str, ...);
void debug(const char *str, ...);
void info(const char *str, ...);
void warn(const char *str, ...);
void error(const char *str, ...);
void fatal(const char *str, ...);
template <typename T>
inline void traceArray(const char *str, const T *array, int size)
{
logArray(TRACE, str, array, size);
}
template <typename T>
inline void debugArray(const char *str, const T *array, int size)
{
logArray(DEBUG, str, array, size);
}
template <typename T>
inline void infoArray(const char *str, const T *array, int size)
{
logArray(INFO, str, array, size);
}
template <typename T>
inline void warnArray(const char *str, const T *array, int size)
{
logArray(WARN, str, array, size);
}
template <typename T>
inline void errorArray(const char *str, const T *array, int size)
{
logArray(ERROR, str, array, size);
}
template <typename T>
inline void fatalArray(const char *str, const T *array, int size)
{
logArray(FATAL, str, array, size);
}
private:
void log(Level level, const char *str, va_list args);
template <typename T>
void logArray(Level level, const char *str, const T *array, int size)
{
if (level < LOG_LEVEL)
{
return;
}
char buf[strlen(m_Prefix) + (m_Tag == nullptr ? 0 : strlen(m_Tag)) + 2];
strcpy(buf, m_Prefix);
if (m_Tag != nullptr)
{
strcat(buf, ":");
strcat(buf, m_Tag);
}
Serial.printf("[%-5s] [%s] %s", levelToString(level), buf, str);
for (size_t i = 0; i < size; i++)
{
Serial.print(array[i]);
}
Serial.println();
}
const char *const m_Prefix;
char *m_Tag;
};
}
}
#endif

44
src/main.cpp
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -23,7 +23,7 @@
#include "Wire.h"
#include "ota.h"
#include "sensors/sensorfactory.h"
#include "sensors/SensorManager.h"
#include "configuration.h"
#include "network/network.h"
#include "globals.h"
@@ -32,8 +32,11 @@
#include "serial/serialcommands.h"
#include "ledmgr.h"
#include "batterymonitor.h"
#include "logging/Logger.h"
SlimeVR::Logging::Logger logger("SlimeVR");
SlimeVR::Sensors::SensorManager sensorManager;
SensorFactory sensors {};
int sensorToCalibrate = -1;
bool blinking = false;
unsigned long blinkStart = 0;
@@ -43,6 +46,13 @@ BatteryMonitor battery;
void setup()
{
Serial.begin(serialBaudRate);
Serial.println();
Serial.println();
Serial.println();
logger.info("SlimeVR v" FIRMWARE_VERSION " starting up...");
//wifi_set_sleep_type(NONE_SLEEP_T);
// Glow diode while loading
#if ENABLE_LEDS
@@ -52,11 +62,8 @@ void setup()
LEDManager::on(LOADING_LED);
#endif
Serial.begin(serialBaudRate);
SerialCommands::setUp();
Serial.println();
Serial.println();
Serial.println();
#if IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_MPU9250
I2CSCAN::clearBus(PIN_IMU_SDA, PIN_IMU_SCL); // Make sure the bus isn't stuck when resetting ESP without powering it down
// Do it only for MPU, cause reaction of BNO to this is not investigated yet
@@ -72,8 +79,7 @@ void setup()
// Wait for IMU to boot
delay(500);
sensors.create();
sensors.motionSetup();
sensorManager.setup();
Network::setUp();
OTA::otaSetup(otaPassword);
@@ -87,24 +93,8 @@ void loop()
LEDManager::ledStatusUpdate();
SerialCommands::update();
OTA::otaUpdate();
Network::update(sensors.getFirst(), sensors.getSecond());
#ifndef UPDATE_IMU_UNCONNECTED
if (ServerConnection::isConnected())
{
#endif
sensors.motionLoop();
#ifndef UPDATE_IMU_UNCONNECTED
}
#endif
// Send updates
#ifndef SEND_UPDATES_UNCONNECTED
if (ServerConnection::isConnected())
{
#endif
sensors.sendData();
#ifndef SEND_UPDATES_UNCONNECTED
}
#endif
Network::update(sensorManager.getFirst(), sensorManager.getSecond());
sensorManager.update();
battery.Loop();
#ifdef TARGET_LOOPTIME_MICROS

19
src/network/packets.h
View File

@@ -48,11 +48,19 @@
#define PACKET_SIGNAL_STRENGTH 19
#define PACKET_TEMPERATURE 20
#define PACKET_INSPECTION 105 // 0x69
#define PACKET_RECEIVE_HEARTBEAT 1
#define PACKET_RECEIVE_VIBRATE 2
#define PACKET_RECEIVE_HANDSHAKE 3
#define PACKET_RECEIVE_COMMAND 4
#define PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA 1
#define PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA 2
#define PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA 3
#define PACKET_INSPECTION_DATATYPE_INT 1
#define PACKET_INSPECTION_DATATYPE_FLOAT 2
namespace Network {
// PACKET_HEARTBEAT 0
void sendHeartbeat();
@@ -93,6 +101,15 @@ namespace Network {
// PACKET_TEMPERATURE 20
void sendTemperature(float temperature, uint8_t sensorId);
#if ENABLE_INSPECTION
void sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA);
void sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA);
void sendInspectionFusedIMUData(uint8_t sensorId, Quat quaternion);
void sendInspectionCorrectionData(uint8_t sensorId, Quat quaternion);
#endif
}
namespace DataTransfer {
@@ -112,4 +129,4 @@ namespace DataTransfer {
int getWriteError();
}
#endif // SLIMEVR_PACKETS_H_
#endif // SLIMEVR_PACKETS_H_

287
src/network/udpclient.cpp
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -24,6 +24,7 @@
#include "udpclient.h"
#include "ledmgr.h"
#include "packets.h"
#include "logging/Logger.h"
#define TIMEOUT 3000UL
@@ -48,6 +49,9 @@ size_t serialLength = 0;
unsigned char buf[8];
// TODO: Cleanup with proper classes
SlimeVR::Logging::Logger udpClientLogger("UDPClient");
template <typename T>
unsigned char * convert_to_chars(T src, unsigned char * target)
{
@@ -148,6 +152,11 @@ namespace DataTransfer {
// PACKET_HEARTBEAT 0
void Network::sendHeartbeat() {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_HEARTBEAT);
DataTransfer::sendPacketNumber();
@@ -157,9 +166,11 @@ void Network::sendHeartbeat() {
// PACKET_ACCEL 4
void Network::sendAccel(float* vector, uint8_t sensorId) {
#ifndef SEND_UPDATES_UNCONNECTED
if(!connected) return; // bno080sensor.cpp function call not in sendData() but in motionLoop()
#endif
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_ACCEL);
DataTransfer::sendPacketNumber();
@@ -172,9 +183,11 @@ void Network::sendAccel(float* vector, uint8_t sensorId) {
// PACKET_RAW_CALIBRATION_DATA 6
void Network::sendRawCalibrationData(float* vector, uint8_t calibrationType, uint8_t sensorId) {
#ifndef SEND_UPDATES_UNCONNECTED
if(!connected) return; // mpu9250sensor.cpp startCalibration()
#endif
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA);
DataTransfer::sendPacketNumber();
@@ -188,9 +201,11 @@ void Network::sendRawCalibrationData(float* vector, uint8_t calibrationType, uin
}
void Network::sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8_t sensorId) {
#ifndef SEND_UPDATES_UNCONNECTED
if(!connected) return; // function not used?
#endif
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA);
DataTransfer::sendPacketNumber();
@@ -205,9 +220,11 @@ void Network::sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8
// PACKET_CALIBRATION_FINISHED 7
void Network::sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId) {
#ifndef SEND_UPDATES_UNCONNECTED
if(!connected) return; // mpu6050sensor.cpp mpu9250sensor.cpp startCalibration()
#endif
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_CALIBRATION_FINISHED);
DataTransfer::sendPacketNumber();
@@ -219,9 +236,11 @@ void Network::sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId)
// PACKET_BATTERY_LEVEL 12
void Network::sendBatteryLevel(float batteryVoltage, float batteryPercentage) {
#ifndef SEND_UPDATES_UNCONNECTED
if(!connected) return;
#endif
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_BATTERY_LEVEL);
DataTransfer::sendPacketNumber();
@@ -233,6 +252,11 @@ void Network::sendBatteryLevel(float batteryVoltage, float batteryPercentage) {
// PACKET_TAP 13
void Network::sendTap(uint8_t value, uint8_t sensorId) {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_TAP);
DataTransfer::sendPacketNumber();
@@ -244,9 +268,11 @@ void Network::sendTap(uint8_t value, uint8_t sensorId) {
// PACKET_ERROR 14
void Network::sendError(uint8_t reason, uint8_t sensorId) {
#ifndef SEND_UPDATES_UNCONNECTED
if(!connected) return;
#endif
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_ERROR);
DataTransfer::sendPacketNumber();
@@ -258,6 +284,11 @@ void Network::sendError(uint8_t reason, uint8_t sensorId) {
// PACKET_SENSOR_INFO 15
void Network::sendSensorInfo(Sensor * sensor) {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_SENSOR_INFO);
DataTransfer::sendPacketNumber();
@@ -270,6 +301,11 @@ void Network::sendSensorInfo(Sensor * sensor) {
// PACKET_ROTATION_DATA 17
void Network::sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_t accuracyInfo, uint8_t sensorId) {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_ROTATION_DATA);
DataTransfer::sendPacketNumber();
@@ -286,6 +322,11 @@ void Network::sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_
// PACKET_MAGNETOMETER_ACCURACY 18
void Network::sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId) {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_MAGNETOMETER_ACCURACY);
DataTransfer::sendPacketNumber();
@@ -297,6 +338,11 @@ void Network::sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId) {
// PACKET_SIGNAL_STRENGTH 19
void Network::sendSignalStrength(uint8_t signalStrength) {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_SIGNAL_STRENGTH);
DataTransfer::sendPacketNumber();
@@ -308,6 +354,11 @@ void Network::sendSignalStrength(uint8_t signalStrength) {
// PACKET_TEMPERATURE 20
void Network::sendTemperature(float temperature, uint8_t sensorId) {
if(!connected)
{
return;
}
if(DataTransfer::beginPacket()) {
DataTransfer::sendPacketType(PACKET_TEMPERATURE);
DataTransfer::sendPacketNumber();
@@ -336,15 +387,163 @@ void Network::sendHandshake() {
WiFi.macAddress(mac);
DataTransfer::sendBytes(mac, 6); // MAC address string
if(!DataTransfer::endPacket()) {
Serial.print("Handshake write error: ");
Serial.println(Udp.getWriteError());
udpClientLogger.error("Handshake write error: %d", Udp.getWriteError());
}
} else {
Serial.print("Handshake write error: ");
Serial.println(Udp.getWriteError());
udpClientLogger.error("Handshake write error: %d", Udp.getWriteError());
}
}
#if ENABLE_INSPECTION
void Network::sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA)
{
if (!connected)
{
return;
}
if(!DataTransfer::beginPacket())
{
udpClientLogger.error("RawIMUData write begin error: %d", Udp.getWriteError());
return;
}
DataTransfer::sendPacketType(PACKET_INSPECTION);
DataTransfer::sendPacketNumber();
DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA);
DataTransfer::sendByte(sensorId);
DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_INT);
DataTransfer::sendInt(rX);
DataTransfer::sendInt(rY);
DataTransfer::sendInt(rZ);
DataTransfer::sendByte(rA);
DataTransfer::sendInt(aX);
DataTransfer::sendInt(aY);
DataTransfer::sendInt(aZ);
DataTransfer::sendByte(aA);
DataTransfer::sendInt(mX);
DataTransfer::sendInt(mY);
DataTransfer::sendInt(mZ);
DataTransfer::sendByte(mA);
if(!DataTransfer::endPacket())
{
udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError());
}
}
void Network::sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA)
{
if (!connected)
{
return;
}
if (!DataTransfer::beginPacket())
{
udpClientLogger.error("RawIMUData write begin error: %d", Udp.getWriteError());
return;
}
DataTransfer::sendPacketType(PACKET_INSPECTION);
DataTransfer::sendPacketNumber();
DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA);
DataTransfer::sendByte(sensorId);
DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
DataTransfer::sendFloat(rX);
DataTransfer::sendFloat(rY);
DataTransfer::sendFloat(rZ);
DataTransfer::sendByte(rA);
DataTransfer::sendFloat(aX);
DataTransfer::sendFloat(aY);
DataTransfer::sendFloat(aZ);
DataTransfer::sendByte(aA);
DataTransfer::sendFloat(mX);
DataTransfer::sendFloat(mY);
DataTransfer::sendFloat(mZ);
DataTransfer::sendByte(mA);
if(!DataTransfer::endPacket())
{
udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError());
}
}
void Network::sendInspectionFusedIMUData(uint8_t sensorId, Quat quaternion)
{
if (!connected)
{
return;
}
if (!DataTransfer::beginPacket())
{
udpClientLogger.error("FusedIMUData write begin error: %d", Udp.getWriteError());
return;
}
DataTransfer::sendPacketType(PACKET_INSPECTION);
DataTransfer::sendPacketNumber();
DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA);
DataTransfer::sendByte(sensorId);
DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
DataTransfer::sendFloat(quaternion.x);
DataTransfer::sendFloat(quaternion.y);
DataTransfer::sendFloat(quaternion.z);
DataTransfer::sendFloat(quaternion.w);
if(!DataTransfer::endPacket())
{
udpClientLogger.error("FusedIMUData write end error: %d", Udp.getWriteError());
}
}
void Network::sendInspectionCorrectionData(uint8_t sensorId, Quat quaternion)
{
if (!connected)
{
return;
}
if (!DataTransfer::beginPacket())
{
udpClientLogger.error("CorrectionData write begin error: %d", Udp.getWriteError());
return;
}
DataTransfer::sendPacketType(PACKET_INSPECTION);
DataTransfer::sendPacketNumber();
DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA);
DataTransfer::sendByte(sensorId);
DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
DataTransfer::sendFloat(quaternion.x);
DataTransfer::sendFloat(quaternion.y);
DataTransfer::sendFloat(quaternion.z);
DataTransfer::sendFloat(quaternion.w);
if(!DataTransfer::endPacket())
{
udpClientLogger.error("CorrectionData write end error: %d", Udp.getWriteError());
}
}
#endif
void returnLastPacket(int len) {
if(DataTransfer::beginPacket()) {
DataTransfer::sendBytes(incomingPacket, len);
@@ -374,12 +573,13 @@ void ServerConnection::connect()
if (packetSize)
{
// receive incoming UDP packets
Serial.printf("[Handshake] Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
int len = Udp.read(incomingPacket, sizeof(incomingPacket));
Serial.print("[Handshake] UDP packet contents: ");
for (int i = 0; i < len; ++i)
Serial.print((byte)incomingPacket[i]);
Serial.println();
#ifdef FULL_DEBUG
udpClientLogger.trace("Received %d bytes from %s, port %d", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
udpClientLogger.traceArray("UDP packet contents: ", incomingPacket, len);
#endif
// Handshake is different, it has 3 in the first byte, not the 4th, and data starts right after
switch (incomingPacket[0])
{
@@ -392,10 +592,8 @@ void ServerConnection::connect()
lastPacketMs = now;
connected = true;
LEDManager::unsetLedStatus(LED_STATUS_SERVER_CONNECTING);
#ifndef SEND_UPDATES_UNCONNECTED
LEDManager::off(LOADING_LED);
#endif
Serial.printf("[Handshake] Handshake successful, server is %s:%d\n", Udp.remoteIP().toString().c_str(), + Udp.remotePort());
udpClientLogger.debug("Handshake successful, server is %s:%d", Udp.remoteIP().toString().c_str(), + Udp.remotePort());
return;
default:
continue;
@@ -404,23 +602,19 @@ void ServerConnection::connect()
else
{
break;
}
}
}
if(lastConnectionAttemptMs + 1000 < now)
{
lastConnectionAttemptMs = now;
Serial.println("[Handshake] Looking for the server...");
udpClientLogger.info("Looking for the server...");
Network::sendHandshake();
#ifndef SEND_UPDATES_UNCONNECTED
LEDManager::on(LOADING_LED);
#endif
}
#ifndef SEND_UPDATES_UNCONNECTED
else if(lastConnectionAttemptMs + 20 < now)
{
LEDManager::off(LOADING_LED);
}
#endif
}
void ServerConnection::resetConnection() {
@@ -437,13 +631,11 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
lastPacketMs = millis();
int len = Udp.read(incomingPacket, sizeof(incomingPacket));
// receive incoming UDP packets
#if serialDebug == true
Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
Serial.print("UDP packet contents: ");
for (int i = 0; i < len; ++i)
Serial.print((byte)incomingPacket[i]);
Serial.println();
#endif
#ifdef FULL_DEBUG
udpClientLogger.trace("Received %d bytes from %s, port %d", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
udpClientLogger.traceArray("UDP packet contents: ", incomingPacket, len);
#endif
switch (convert_chars<int>(incomingPacket))
{
@@ -455,7 +647,7 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
break;
case PACKET_RECEIVE_HANDSHAKE:
// Assume handshake successful
Serial.println("Handshake received again, ignoring");
udpClientLogger.warn("Handshake received again, ignoring");
break;
case PACKET_RECEIVE_COMMAND:
@@ -468,7 +660,7 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
break;
case PACKET_SENSOR_INFO:
if(len < 6) {
Serial.println("Wrong sensor info packet");
udpClientLogger.warn("Wrong sensor info packet");
break;
}
if(incomingPacket[4] == 0) {
@@ -489,7 +681,7 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
connected = false;
sensorStateNotified1 = false;
sensorStateNotified2 = false;
Serial.println("Connection to server timed out");
udpClientLogger.warn("Connection to server timed out");
}
}
@@ -499,4 +691,3 @@ void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) {
updateSensorState(sensor, sensor2);
}
}

22
src/network/wifihandler.cpp
View File

@@ -23,6 +23,7 @@
#include "globals.h"
#include "network.h"
#include "ledmgr.h"
#include "logging/Logger.h"
#if !ESP8266
#include "esp_wifi.h"
#endif
@@ -34,6 +35,9 @@ uint8_t wifiState = 0;
bool hadWifi = false;
unsigned long last_rssi_sample = 0;
// TODO: Cleanup with proper classes
SlimeVR::Logging::Logger wifiHandlerLogger("WiFiHandler");
void reportWifiError() {
if(lastWifiReportTime + 1000 < millis()) {
lastWifiReportTime = millis();
@@ -59,13 +63,13 @@ IPAddress WiFiNetwork::getAddress() {
}
void WiFiNetwork::setUp() {
Serial.println("[NOTICE] WiFi: Setting up WiFi");
wifiHandlerLogger.info("Setting up WiFi");
WiFi.persistent(true);
WiFi.mode(WIFI_STA);
WiFi.hostname("SlimeVR FBT Tracker");
Serial.printf("[NOTICE] WiFi: Loaded credentials for SSID %s and pass length %d\n", WiFi.SSID().c_str(), WiFi.psk().length());
wifiHandlerLogger.info("Loaded credentials for SSID %s and pass length %d", WiFi.SSID().c_str(), WiFi.psk().length());
wl_status_t status = WiFi.begin(); // Should connect to last used access point, see https://arduino-esp8266.readthedocs.io/en/latest/esp8266wifi/station-class.html#begin
Serial.printf("[NOTICE] Status: %d\n", status);
wifiHandlerLogger.debug("Status: %d", status);
wifiState = 1;
wifiConnectionTimeout = millis();
@@ -95,7 +99,7 @@ void WiFiNetwork::setUp() {
}
else
{
Serial.println("[ERR] Unable to get wifi config, power saving not enabled!");
wifiHandlerLogger.error("Unable to get WiFi config, power saving not enabled!");
}
#endif
#endif
@@ -106,14 +110,14 @@ void onConnected() {
LEDManager::unsetLedStatus(LED_STATUS_WIFI_CONNECTING);
isWifiConnected = true;
hadWifi = true;
Serial.printf("[NOTICE] WiFi: Connected successfully to SSID '%s', ip address %s\n", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
wifiHandlerLogger.info("Connected successfully to SSID '%s', ip address %s", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
}
void WiFiNetwork::upkeep() {
upkeepProvisioning();
if(WiFi.status() != WL_CONNECTED) {
if(isWifiConnected) {
Serial.printf("[NOTICE] WiFi: Connection to WiFi lost, reconnecting...");
wifiHandlerLogger.warn("Connection to WiFi lost, reconnecting...");
isWifiConnected = false;
}
LEDManager::setLedStatus(LED_STATUS_WIFI_CONNECTING);
@@ -127,8 +131,8 @@ void WiFiNetwork::upkeep() {
// Try hardcoded credentials now
WiFi.begin(WIFI_CREDS_SSID, WIFI_CREDS_PASSWD);
wifiConnectionTimeout = millis();
Serial.printf("[NOTICE] WiFi: Can't connect from saved credentials, status: %d.\n", WiFi.status());
Serial.println("[NOTICE] WiFi: Trying hardcoded credentials...");
wifiHandlerLogger.error("Can't connect from saved credentials, status: %d.", WiFi.status());
wifiHandlerLogger.debug("Trying hardcoded credentials...");
#endif
wifiState = 2;
return;
@@ -136,7 +140,7 @@ void WiFiNetwork::upkeep() {
// Start smart config
if(!hadWifi && !WiFi.smartConfigDone() && wifiConnectionTimeout + 11000 < millis()) {
if(WiFi.status() != WL_IDLE_STATUS) {
Serial.printf("[NOTICE] WiFi: Can't connect from any credentials, status: %d.\n", WiFi.status());
wifiHandlerLogger.error("Can't connect from any credentials, status: %d.", WiFi.status());
}
startProvisioning();
}

5
src/network/wifiprovisioning.cpp
View File

@@ -21,11 +21,14 @@
THE SOFTWARE.
*/
#include "network.h"
#include "logging/Logger.h"
// TODO Currently provisioning implemented via SmartConfig
// it sucks.
// TODO: New implementation: https://github.com/SlimeVR/SlimeVR-Tracker-ESP/issues/71
// TODO: Cleanup with proper classes
SlimeVR::Logging::Logger wifiProvisioningLogger("WiFiProvisioning");
bool provisioning = false;
void WiFiNetwork::upkeepProvisioning() {
@@ -35,7 +38,7 @@ void WiFiNetwork::upkeepProvisioning() {
void WiFiNetwork::startProvisioning() {
if(WiFi.beginSmartConfig()) {
provisioning = true;
Serial.println("[NOTICE] WiFi: SmartConfig started");
wifiProvisioningLogger.info("SmartConfig started");
}
}

40
src/sensors/sensorfactory.h → src/sensors/EmptySensor.h
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Copyright (c) 2022 TheDevMinerTV
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -20,28 +20,28 @@
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SLIMEVR_SENSORFACTORY_H_
#define SLIMEVR_SENSORFACTORY_H_
#include "globals.h"
#ifndef SENSORS_EMPTYSENSOR_H
#define SENSORS_EMPTYSENSOR_H
#include "sensor.h"
class SensorFactory
namespace SlimeVR
{
public:
SensorFactory();
~SensorFactory();
void create();
void motionSetup();
void motionLoop();
void sendData();
void startCalibration(int sensorId, int calibrationType);
Sensor *getFirst() { return sensor1; };
Sensor *getSecond() { return sensor2; };
namespace Sensors
{
class EmptySensor : public Sensor
{
public:
EmptySensor(uint8_t id) : Sensor("EmptySensor", 255, id, 0, 0.0){};
~EmptySensor(){};
protected:
Sensor *sensor1;
Sensor *sensor2;
};
void motionSetup() override final{};
void motionLoop() override final{};
void sendData() override final{};
void startCalibration(int calibrationType) override final{};
};
}
}
#endif // SLIMEVR_SENSORFACTORY_H_
#endif

41
src/sensors/ErroneousSensor.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2022 TheDevMinerTV
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "ErroneousSensor.h"
#include "network/network.h"
namespace SlimeVR
{
namespace Sensors
{
void ErroneousSensor::motionSetup()
{
m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType));
}
uint8_t ErroneousSensor::getSensorState()
{
return SENSOR_ERROR;
};
}
}

51
src/sensors/ErroneousSensor.h Normal file
View File

@@ -0,0 +1,51 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2022 TheDevMinerTV
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SENSORS_ERRONEOUSSENSOR_H
#define SENSORS_ERRONEOUSSENSOR_H
#include "sensor.h"
namespace SlimeVR
{
namespace Sensors
{
class ErroneousSensor : public Sensor
{
public:
ErroneousSensor(uint8_t id, uint8_t type) : Sensor("ErroneousSensor", type, id, 0, 0.0), m_ExpectedType(type){};
~ErroneousSensor(){};
void motionSetup() override;
void motionLoop() override final{};
void sendData() override{};
void startCalibration(int calibrationType) override final{};
uint8_t getSensorState() override;
private:
uint8_t m_ExpectedType;
};
}
}
#endif

139
src/sensors/SensorManager.cpp Normal file
View File

@@ -0,0 +1,139 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2022 TheDevMinerTV
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "SensorManager.h"
#include <i2cscan.h>
#include "network/network.h"
#include "bno055sensor.h"
#include "bno080sensor.h"
#include "mpu9250sensor.h"
#include "mpu6050sensor.h"
#include "bmi160sensor.h"
#include "icm20948sensor.h"
#include "ErroneousSensor.h"
namespace SlimeVR
{
namespace Sensors
{
void SensorManager::setup()
{
uint8_t firstIMUAddress = 0;
uint8_t secondIMUAddress = 0;
{
#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086
firstIMUAddress = I2CSCAN::pickDevice(0x4A, 0x4B, true);
#elif IMU == IMU_BNO055
firstIMUAddress = I2CSCAN::pickDevice(0x29, 0x28, true);
#elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948
firstIMUAddress = I2CSCAN::pickDevice(0x68, 0x69, true);
#else
#error Unsupported primary IMU
#endif
if (firstIMUAddress == 0)
{
m_Sensor1 = new ErroneousSensor(0, IMU);
}
else
{
m_Logger.trace("Primary IMU found at address 0x%02X", firstIMUAddress);
#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086
m_Sensor1 = new BNO080Sensor(0, IMU, firstIMUAddress, IMU_ROTATION, PIN_IMU_INT);
#elif IMU == IMU_BNO055
m_Sensor1 = new BNO055Sensor(0, firstIMUAddress, IMU_ROTATION);
#elif IMU == IMU_MPU9250
m_Sensor1 = new MPU9250Sensor(0, firstIMUAddress, IMU_ROTATION);
#elif IMU == IMU_BMI160
m_Sensor1 = new BMI160Sensor(0, firstIMUAddress, IMU_ROTATION);
#elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050
m_Sensor1 = new MPU6050Sensor(0, IMU, firstIMUAddress, IMU_ROTATION);
#elif IMU == IMU_ICM20948
m_Sensor1 = new ICM20948Sensor(0, firstIMUAddress, IMU_ROTATION);
#endif
}
m_Sensor1->motionSetup();
}
{
#if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086
secondIMUAddress = I2CSCAN::pickDevice(0x4B, 0x4A, false);
#elif SECOND_IMU == IMU_BNO055
secondIMUAddress = I2CSCAN::pickDevice(0x28, 0x29, false);
#elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948
secondIMUAddress = I2CSCAN::pickDevice(0x69, 0x68, false);
#else
#error Unsupported secondary IMU
#endif
if (secondIMUAddress == firstIMUAddress)
{
m_Logger.debug("No secondary IMU connected");
}
else if (secondIMUAddress == 0)
{
m_Sensor2 = new ErroneousSensor(1, SECOND_IMU);
}
else
{
m_Logger.trace("Secondary IMU found at address 0x%02X", secondIMUAddress);
#if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086
m_Sensor2 = new BNO080Sensor(1, IMU, secondIMUAddress, SECOND_IMU_ROTATION, PIN_IMU_INT_2);
#elif SECOND_IMU == IMU_BNO055
m_Sensor2 = new BNO055Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION);
#elif SECOND_IMU == IMU_MPU9250
m_Sensor2 = new MPU9250Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION);
#elif SECOND_IMU == IMU_BMI160
m_Sensor2 = new BMI160Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION);
#elif SECOND_IMU == IMU_MPU6500 || IMU == IMU_MPU6050
m_Sensor2 = new MPU6050Sensor(1, IMU, secondIMUAddress, SECOND_IMU_ROTATION);
#elif SECOND_IMU == IMU_ICM20948
m_Sensor2 = new ICM20948Sensor(1, secondIMUAddress, SECOND_IMU_ROTATION);
#endif
}
m_Sensor2->motionSetup();
}
}
void SensorManager::update()
{
// Gather IMU data
m_Sensor1->motionLoop();
m_Sensor2->motionLoop();
if (!ServerConnection::isConnected())
{
return;
}
// Send updates
m_Sensor1->sendData();
m_Sensor2->sendData();
}
}
}

62
src/sensors/SensorManager.h Normal file
View File

@@ -0,0 +1,62 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2022 TheDevMinerTV
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef SLIMEVR_SENSORMANAGER
#define SLIMEVR_SENSORMANAGER
#include "globals.h"
#include "sensor.h"
#include "EmptySensor.h"
#include "logging/Logger.h"
namespace SlimeVR
{
namespace Sensors
{
class SensorManager
{
public:
SensorManager()
: m_Logger(SlimeVR::Logging::Logger("SensorManager")), m_Sensor1(new EmptySensor(0)), m_Sensor2(new EmptySensor(0)) {}
~SensorManager()
{
delete m_Sensor1;
delete m_Sensor2;
}
void setup();
void update();
Sensor *getFirst() { return m_Sensor1; };
Sensor *getSecond() { return m_Sensor2; };
private:
SlimeVR::Logging::Logger m_Logger;
Sensor *m_Sensor1;
Sensor *m_Sensor2;
};
}
}
#endif // SLIMEVR_SENSORFACTORY_H_

80
src/sensors/bmi160sensor.cpp
View File

@@ -22,6 +22,7 @@
*/
#include "bmi160sensor.h"
#include "network/network.h"
// Typical sensitivity at 25C
// See p. 9 of https://www.mouser.com/datasheet/2/783/BST-BMI160-DS000-1509569.pdf
@@ -57,24 +58,28 @@ void BMI160Sensor::motionSetup() {
// initialize device
imu.initialize(addr);
if(!imu.testConnection()) {
Serial.print("[ERR] Can't communicate with BMI160, response 0x");
Serial.println(imu.getDeviceID(), HEX);
m_Logger.fatal("Can't connect to BMI160 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
LEDManager::signalAssert();
return;
}
Serial.print("[OK] Connected to BMI160, ID 0x");
Serial.println(imu.getDeviceID(), HEX);
m_Logger.info("Connected to BMI160 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
int16_t ax, ay, az;
imu.getAcceleration(&ax, &ay, &az);
if(az < 0 && 10 * (ax * ax + ay * ay) < az * az) {
float g_az = (float)az / 8192; // For 4G sensitivity
if(g_az < -0.75f) {
LEDManager::off(CALIBRATING_LED);
Serial.println("Calling Calibration... Flip front to confirm start calibration.");
m_Logger.info("Flip front to confirm start calibration");
delay(5000);
imu.getAcceleration(&ax, &ay, &az);
if(az > 0 && 10 * (ax * ax + ay * ay) < az * az)
g_az = (float)az / 8192;
if(g_az > 0.75f)
{
m_Logger.debug("Starting calibration...");
startCalibration(0);
}
LEDManager::on(CALIBRATING_LED);
}
@@ -84,6 +89,16 @@ void BMI160Sensor::motionSetup() {
}
void BMI160Sensor::motionLoop() {
#if ENABLE_INSPECTION
{
int16_t rX, rY, rZ, aX, aY, aZ;
imu.getRotation(&rX, &rY, &rZ);
imu.getAcceleration(&aX, &aY, &aZ);
Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255);
}
#endif
now = micros();
deltat = now - last; //seconds since last update
last = now;
@@ -95,6 +110,13 @@ void BMI160Sensor::motionLoop() {
mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat * 1.0e-6f);
quaternion.set(-q[2], q[1], q[3], q[0]);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
newData = true;
@@ -113,6 +135,16 @@ float BMI160Sensor::getTemperature()
void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3])
{
#if ENABLE_INSPECTION
{
int16_t rX, rY, rZ, aX, aY, aZ, mX, mY, mZ;
imu.getRotation(&rX, &rY, &rZ);
imu.getAcceleration(&aX, &aY, &aZ);
Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255);
}
#endif
float temperature = getTemperature();
float tempDiff = temperature - calibration->temperature;
uint8_t quant = map(temperature, 15, 75, 0, 12);
@@ -147,17 +179,21 @@ void BMI160Sensor::getScaledValues(float Gxyz[3], float Axyz[3])
void BMI160Sensor::startCalibration(int calibrationType) {
LEDManager::on(CALIBRATING_LED);
Serial.println("[NOTICE] Gathering raw data for device calibration...");
m_Logger.debug("Gathering raw data for device calibration...");
DeviceConfig * const config = getConfigPtr();
// Wait for sensor to calm down before calibration
Serial.println("[NOTICE] Put down the device and wait for baseline gyro reading calibration");
m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
delay(2000);
float temperature = getTemperature();
config->calibration[sensorId].temperature = temperature;
uint16_t gyroCalibrationSamples = 2500;
float rawGxyz[3] = {0};
Serial.printf("[NOTICE] Calibration temperature: %f\n", temperature);
#ifdef FULL_DEBUG
m_Logger.trace("Calibration temperature: %f", temperature);
#endif
for (int i = 0; i < gyroCalibrationSamples; i++)
{
LEDManager::on(CALIBRATING_LED);
@@ -171,15 +207,18 @@ void BMI160Sensor::startCalibration(int calibrationType) {
config->calibration[sensorId].G_off[0] = rawGxyz[0] / gyroCalibrationSamples;
config->calibration[sensorId].G_off[1] = rawGxyz[1] / gyroCalibrationSamples;
config->calibration[sensorId].G_off[2] = rawGxyz[2] / gyroCalibrationSamples;
Serial.printf("[NOTICE] Gyro calibration results: %f %f %f\n", config->calibration[sensorId].G_off[0], config->calibration[sensorId].G_off[1], config->calibration[sensorId].G_off[2]);
#ifdef FULL_DEBUG
m_Logger.trace("Gyro calibration results: %f %f %f", config->calibration[sensorId].G_off[0], config->calibration[sensorId].G_off[1], config->calibration[sensorId].G_off[2]);
#endif
// Blink calibrating led before user should rotate the sensor
Serial.println("[NOTICE] After 3seconds, Gently rotate the device while it's gathering accelerometer data");
m_Logger.info("After 3 seconds, Gently rotate the device while it's gathering accelerometer data");
LEDManager::on(CALIBRATING_LED);
delay(1500);
LEDManager::off(CALIBRATING_LED);
delay(1500);
Serial.println("[NOTICE] Gathering accelerometer data start!");
m_Logger.debug("Gathering accelerometer data...");
uint16_t accelCalibrationSamples = 300;
float *calibrationDataAcc = (float*)malloc(accelCalibrationSamples * 3 * sizeof(float));
@@ -194,24 +233,27 @@ void BMI160Sensor::startCalibration(int calibrationType) {
LEDManager::off(CALIBRATING_LED);
delay(100);
}
Serial.println("[NOTICE] Calibration data gathered");
LEDManager::off(CALIBRATING_LED);
Serial.println("[NOTICE] Now Calculate Calibration data");
m_Logger.debug("Calculating calibration data...");
float A_BAinv[4][3];
CalculateCalibration(calibrationDataAcc, accelCalibrationSamples, A_BAinv);
free(calibrationDataAcc);
Serial.println("[NOTICE] Finished Calculate Calibration data");
Serial.println("[NOTICE] Now Saving EEPROM");
m_Logger.debug("Finished Calculate Calibration data");
m_Logger.debug("Accelerometer calibration matrix:");
m_Logger.debug("{");
for (int i = 0; i < 3; i++)
{
config->calibration[sensorId].A_B[i] = A_BAinv[0][i];
config->calibration[sensorId].A_Ainv[0][i] = A_BAinv[1][i];
config->calibration[sensorId].A_Ainv[1][i] = A_BAinv[2][i];
config->calibration[sensorId].A_Ainv[2][i] = A_BAinv[3][i];
m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]);
}
m_Logger.debug("}");
m_Logger.debug("Now Saving EEPROM");
setConfig(*config);
Serial.println("[NOTICE] Finished Saving EEPROM");
m_Logger.debug("Finished Saving EEPROM");
m_Logger.info("Calibration data gathered");
delay(5000);
}

2
src/sensors/bmi160sensor.h
View File

@@ -29,7 +29,7 @@
class BMI160Sensor : public Sensor {
public:
BMI160Sensor(){};
BMI160Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("BMI160Sensor", IMU_BMI160, id, address, rotation){};
~BMI160Sensor(){};
void motionSetup() override final;
void motionLoop() override final;

23
src/sensors/bno055sensor.cpp
View File

@@ -30,8 +30,7 @@ void BNO055Sensor::motionSetup() {
delay(3000);
if (!imu.begin(Adafruit_BNO055::OPERATION_MODE_IMUPLUS))
{
Serial.print("[ERR] IMU BNO055: Can't connect to ");
Serial.println(getIMUNameByType(sensorType));
m_Logger.fatal("Can't connect to BNO055 at address 0x%02x", addr);
LEDManager::signalAssert();
return;
}
@@ -39,17 +38,33 @@ void BNO055Sensor::motionSetup() {
delay(1000);
imu.setAxisRemap(Adafruit_BNO055::REMAP_CONFIG_P0);
imu.setAxisSign(Adafruit_BNO055::REMAP_SIGN_P0);
Serial.print("[NOTICE] Connected to");
Serial.println(getIMUNameByType(sensorType));
m_Logger.info("Connected to BNO055 at address 0x%02x", addr);
working = true;
configured = true;
}
void BNO055Sensor::motionLoop() {
#if ENABLE_INSPECTION
{
Vector3 gyro = imu.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
Vector3 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
Vector3 mag = imu.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
Network::sendInspectionRawIMUData(sensorId, UNPACK_VECTOR(gyro), 255, UNPACK_VECTOR(accel), 255, UNPACK_VECTOR(mag), 255);
}
#endif
// TODO Optimize a bit with setting rawQuat directly
Quat quat = imu.getQuat();
quaternion.set(quat.x, quat.y, quat.z, quat.w);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
if(!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) {
newData = true;
lastQuatSent = quaternion;

2
src/sensors/bno055sensor.h
View File

@@ -27,7 +27,7 @@
class BNO055Sensor : public Sensor
{
public:
BNO055Sensor(){};
BNO055Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("BNO055Sensor", IMU_BNO055, id, address, rotation){};
~BNO055Sensor(){};
void motionSetup() override final;
void motionLoop() override final;

114
src/sensors/bno080sensor.cpp
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -24,32 +24,34 @@
#include "sensors/bno080sensor.h"
#include "network/network.h"
#include "ledmgr.h"
#include "utils.h"
void BNO080Sensor::motionSetup()
{
#ifdef FULL_DEBUG
imu.enableDebugging(Serial);
#endif
if(!imu.begin(addr, Wire, intPin)) {
Serial.print("[ERR] IMU BNO08X: Can't connect to ");
Serial.println(getIMUNameByType(sensorType));
if(!imu.begin(addr, Wire, m_IntPin)) {
m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr);
LEDManager::signalAssert();
return;
}
Serial.print("[NOTICE] IMU BNO08X: Connected to ");
Serial.print(getIMUNameByType(sensorType));
Serial.print(" on 0x");
Serial.print(addr, HEX);
Serial.print(". Info: SW Version Major: 0x");
Serial.print(imu.swMajor, HEX);
Serial.print(" SW Version Minor: 0x");
Serial.print(imu.swMinor, HEX);
Serial.print(" SW Part Number: 0x");
Serial.print(imu.swPartNumber, HEX);
Serial.print(" SW Build Number: 0x");
Serial.print(imu.swBuildNumber, HEX);
Serial.print(" SW Version Patch: 0x");
Serial.println(imu.swVersionPatch, HEX);
m_Logger.info("Connected to %s on 0x%02x. "
"Info: SW Version Major: 0x%02x "
"SW Version Minor: 0x%02x "
"SW Part Number: 0x%02x "
"SW Build Number: 0x%02x "
"SW Version Patch: 0x%02x",
getIMUNameByType(sensorType),
addr,
imu.swMajor,
imu.swMinor,
imu.swPartNumber,
imu.swBuildNumber,
imu.swVersionPatch
);
bool useStabilization = false;
#ifdef BNO_USE_ARVR_STABILIZATION
useStabilization = true;
@@ -72,6 +74,13 @@ void BNO080Sensor::motionSetup()
}
}
imu.enableTapDetector(100);
#ifdef ENABLE_INSPECTION
imu.enableRawGyro(10);
imu.enableRawAccelerometer(10);
imu.enableRawMagnetometer(10);
#endif
lastReset = 0;
lastData = millis();
working = true;
@@ -83,6 +92,27 @@ void BNO080Sensor::motionLoop()
//Look for reports from the IMU
while (imu.dataAvailable())
{
#if ENABLE_INSPECTION
{
int16_t rX = imu.getRawGyroX();
int16_t rY = imu.getRawGyroY();
int16_t rZ = imu.getRawGyroZ();
uint8_t rA = imu.getGyroAccuracy();
int16_t aX = imu.getRawAccelX();
int16_t aY = imu.getRawAccelY();
int16_t aZ = imu.getRawAccelZ();
uint8_t aA = imu.getAccelAccuracy();
int16_t mX = imu.getRawMagX();
int16_t mY = imu.getRawMagY();
int16_t mZ = imu.getRawMagZ();
uint8_t mA = imu.getMagAccuracy();
Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA);
}
#endif
lastReset = 0;
lastData = millis();
if (useMagnetometerAllTheTime || !useMagnetometerCorrection)
@@ -91,6 +121,13 @@ void BNO080Sensor::motionLoop()
{
imu.getQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, magneticAccuracyEstimate, calibrationAccuracy);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
newData = true;
@@ -104,12 +141,26 @@ void BNO080Sensor::motionLoop()
{
imu.getGameQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, calibrationAccuracy);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
newData = true;
}
if (imu.hasNewMagQuat())
{
imu.getMagQuat(magQuaternion.x, magQuaternion.y, magQuaternion.z, magQuaternion.w, magneticAccuracyEstimate, magCalibrationAccuracy);
magQuaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionCorrectionData(sensorId, quaternion);
}
#endif
newMagData = true;
}
}
@@ -124,11 +175,19 @@ void BNO080Sensor::motionLoop()
imu.getAccel(v[0], v[1], v[2], acc);
Network::sendAccel(v, PACKET_ACCEL);
}
if (intPin == 255 || imu.I2CTimedOut())
if (m_IntPin == 255 || imu.I2CTimedOut())
break;
}
if (lastData + 1000 < millis() && configured)
{
while(true) {
BNO080Error error = imu.readError();
if(error.error_source == 255)
break;
lastError = error;
m_Logger.error("BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d",
error.severity, error.error_sequence_number, error.error_source, error.error, error.error_module, error.error_code);
}
LEDManager::setLedStatus(LED_STATUS_IMU_ERROR);
working = false;
lastData = millis();
@@ -138,10 +197,9 @@ void BNO080Sensor::motionLoop()
lastReset = rr;
Network::sendError(rr, this->sensorId);
}
Serial.print("[ERR] Sensor ");
Serial.print(sensorId);
Serial.print(" was reset: ");
Serial.println(rr);
m_Logger.error("Sensor %d doesn't respond. Last reset reason:", sensorId, lastReset);
m_Logger.error("Last error: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d",
lastError.severity, lastError.error_sequence_number, lastError.error_source, lastError.error, lastError.error_module, lastError.error_code);
}
}
@@ -157,15 +215,9 @@ void BNO080Sensor::sendData()
Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId);
if (useMagnetometerAllTheTime)
Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId);
#ifdef FULL_DEBUG
Serial.print("[DBG] Quaternion: ");
Serial.print(quaternion.x);
Serial.print(",");
Serial.print(quaternion.y);
Serial.print(",");
Serial.print(quaternion.z);
Serial.print(",");
Serial.println(quaternion.w);
m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(quaternion));
#endif
}
if (newMagData)

6
src/sensors/bno080sensor.h
View File

@@ -26,7 +26,8 @@
class BNO080Sensor : public Sensor
{
public:
BNO080Sensor(){};
BNO080Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t intPin)
: Sensor("BNO080Sensor", type, id, address, rotation), m_IntPin(intPin) {};
~BNO080Sensor(){};
void motionSetup() override final;
void motionLoop() override final;
@@ -37,9 +38,12 @@ public:
private:
BNO080 imu{};
uint8_t m_IntPin;
uint8_t tap;
unsigned long lastData = 0;
uint8_t lastReset = 0;
BNO080Error lastError{};
// Magnetometer specific members
Quat magQuaternion{};

567
src/sensors/icm20948sensor.cpp
View File

@@ -36,6 +36,9 @@ int bias_save_periods[] = { 120, 180, 300, 600, 600 }; // 2min + 3min + 5min + 1
void ICM20948Sensor::save_bias(bool repeat) {
#if defined(SAVE_BIAS) && SAVE_BIAS
#ifdef FULL_DEBUG
m_Logger.trace("Saving Bias");
#endif
#if ESP8266
int8_t count;
int32_t bias_a[3], bias_g[3], bias_m[3];
@@ -58,16 +61,24 @@ void ICM20948Sensor::save_bias(bool repeat) {
EEPROM.begin(4096); // max memory usage = 4096
EEPROM.get(addr + 100, count); // 1st imu counter in EEPROM addr: 0x69+100=205, 2nd addr: 0x68+100=204
Serial.printf("[0x%02X] EEPROM position: %d, count: %d \n", addr, addr + 100, count);
#ifdef FULL_DEBUG
m_Logger.trace("[0x%02X] EEPROM position: %d, count: %d", addr, addr + 100, count);
#endif
if(count < 0 || count > 42) {
count = sensorId; // 1st imu counter is even number, 2nd is odd
} else if(repeat) {
count++;
}
EEPROM.put(addr + 100, count);
Serial.printf("[0x%02X] bias gyro save(%d): [%d, %d, %d] \n", addr, count * 12, bias_g[0], bias_g[1], bias_g[2]);
Serial.printf("[0x%02X] bias accel save(%d): [%d, %d, %d] \n", addr, count * 12, bias_a[0], bias_a[1], bias_a[2]);
Serial.printf("[0x%02X] bias CPass save(%d): [%d, %d, %d] \n\n", addr, count * 12, bias_m[0], bias_m[1], bias_m[2]);
#ifdef FULL_DEBUG
m_Logger.trace("[0x%02X] bias gyro save(%d): [%d, %d, %d]", addr, count * 12, bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("[0x%02X] bias accel save(%d): [%d, %d, %d]", addr, count * 12, bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("[0x%02X] bias CPass save(%d): [%d, %d, %d]", addr, count * 12, bias_m[0], bias_m[1], bias_m[2]);
#endif
if (gyro_set) {
EEPROM.put(1024 + (count * 12), bias_g); // 1024 ~ 2008
}
@@ -86,7 +97,6 @@ void ICM20948Sensor::save_bias(bool repeat) {
}
}
#elif ESP32
int bias_a[3], bias_g[3], bias_m[3];
imu.GetBiasGyroX(&bias_g[0]);
@@ -105,57 +115,44 @@ void ICM20948Sensor::save_bias(bool repeat) {
bool gyro_set = bias_g[0] && bias_g[1] && bias_g[2];
bool mag_set = bias_m[0] && bias_m[1] && bias_m[2];
#ifdef FULL_DEBUG
Serial.println("bias gyro result:");
Serial.println(bias_g[0]);
Serial.println(bias_g[1]);
Serial.println(bias_g[2]);
Serial.println("end gyro");
Serial.println("bias accel result:");
Serial.println(bias_a[0]);
Serial.println(bias_a[1]);
Serial.println(bias_a[2]);
Serial.println("end accel");
Serial.println("bias mag result:");
Serial.println(bias_m[0]);
Serial.println(bias_m[1]);
Serial.println(bias_m[2]);
Serial.println("end mag");
#endif
#ifdef FULL_DEBUG
m_Logger.trace("bias gyro on IMU: %d, %d, %d", bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("bias accel on IMU: %d, %d, %d", bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("bias mag on IMU: %d, %d, %d", bias_m[0], bias_m[1], bias_m[2]);
#endif
bool auxiliary = sensorId == 1;
if (accel_set) {
// Save accel
prefs.putInt(auxiliary ? "ba01" : "ba00", bias_a[0]);
prefs.putInt(auxiliary ? "ba11" : "ba10", bias_a[1]);
prefs.putInt(auxiliary ? "ba21" : "ba20", bias_a[2]);
// Save accel
prefs.putInt(auxiliary ? "ba01" : "ba00", bias_a[0]);
prefs.putInt(auxiliary ? "ba11" : "ba10", bias_a[1]);
prefs.putInt(auxiliary ? "ba21" : "ba20", bias_a[2]);
#ifdef FULL_DEBUG
Serial.println("Wrote Accel Bias");
#endif
#ifdef FULL_DEBUG
m_Logger.trace("Wrote Accel Bias");
#endif
}
if (gyro_set) {
// Save gyro
prefs.putInt(auxiliary ? "bg01" : "bg00", bias_g[0]);
prefs.putInt(auxiliary ? "bg11" : "bg10", bias_g[1]);
prefs.putInt(auxiliary ? "bg21" : "bg20", bias_g[2]);
// Save gyro
prefs.putInt(auxiliary ? "bg01" : "bg00", bias_g[0]);
prefs.putInt(auxiliary ? "bg11" : "bg10", bias_g[1]);
prefs.putInt(auxiliary ? "bg21" : "bg20", bias_g[2]);
#ifdef FULL_DEBUG
Serial.println("Wrote Gyro Bias");
#endif
#ifdef FULL_DEBUG
m_Logger.trace("Wrote Gyro Bias");
#endif
}
if (mag_set) {
// Save mag
prefs.putInt(auxiliary ? "bm01" : "bm00", bias_m[0]);
prefs.putInt(auxiliary ? "bm11" : "bm10", bias_m[1]);
prefs.putInt(auxiliary ? "bm21" : "bm20", bias_m[2]);
// Save mag
prefs.putInt(auxiliary ? "bm01" : "bm00", bias_m[0]);
prefs.putInt(auxiliary ? "bm11" : "bm10", bias_m[1]);
prefs.putInt(auxiliary ? "bm21" : "bm20", bias_m[2]);
#ifdef FULL_DEBUG
Serial.println("Wrote Mag Bias");
#endif
#ifdef FULL_DEBUG
m_Logger.trace("Wrote Mag Bias");
#endif
}
#endif
@@ -166,182 +163,108 @@ void ICM20948Sensor::save_bias(bool repeat) {
{
timer.in(bias_save_periods[bias_save_counter] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->save_bias(true); return false; }, this);
}
}
}
#endif
}
void ICM20948Sensor::motionSetup() {
#ifdef FULL_DEBUG
imu.enableDebugging(Serial);
#endif
// SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error
boolean tracker = false;
if (addr == 0x68) {
tracker = false;
} else if (addr == 0x69)
{
tracker = true;
} else {
Serial.print("[ERR] IMU ICM20948: I2C Address not supportet by ICM20948 library: ");
Serial.println(addr, HEX);
return;
}
//Serial.printf("[INFO] SensorId: %i Addr: 0x%02x IMU ICM20948: Start Init with addr = %s\n", sensorId, this->addr, tracker ? "true" : "false"); //only for debug
ICM_20948_Status_e imu_err = imu.begin(Wire, tracker);
if (imu_err != ICM_20948_Stat_Ok) {
Serial.printf("[ERR] SensorId: %i Addr: 0x%02x IMU ICM20948: Can't connect to 0x%02x Error Code: 0x%02x\n", sensorId, this->addr, this->addr, imu_err);
LEDManager::signalAssert();
return;
}
void ICM20948Sensor::load_bias() {
#if defined(LOAD_BIAS) && LOAD_BIAS
#if ESP8266
int8_t count;
int32_t bias_g[3], bias_a[3], bias_m[3];
count = 0;
EEPROM.begin(4096); // max memory usage = 4096
EEPROM.get(addr + 100, count); // 1st imu counter in EEPROM addr: 0x69+100=205, 2nd addr: 0x68+100=204
// Configure imu setup and load any stored bias values
if(imu.initializeDMP() == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02x DMP initialized\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02x DMP Failed to initialize\n", sensorId, addr);
return;
}
#ifdef FULL_DEBUG
m_Logger.trace("[0x%02X] EEPROM position: %d, count: %d", addr, addr + 100, count);
#endif
if (USE_6_AXIS)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X use 6-axis...\n", sensorId, addr);
if(imu.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X Enabled DMP Senor for Game Rotation Vector\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X Enabling DMP Senor for Game Rotation Vector Failed\n", sensorId, addr);
return;
}
}
else
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X use 9-axis...\n", sensorId, addr);
if(imu.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X Enabled DMP Senor for Sensor Orientation\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X Enabling DMP Senor Orientation Failed\n", sensorId, addr);
return;
}
}
if(count < 0 || count > 42) {
count = sensorId; // 1st imu counter is even number, 2nd is odd
EEPROM.put(addr + 100, count);
}
EEPROM.get(1024 + (count * 12), bias_g); // 1024 ~ 2008
EEPROM.get(2046 + (count * 12), bias_a); // 2046 ~ 3030
EEPROM.get(3072 + (count * 12), bias_m); // 3072 ~ 4056
EEPROM.end();
// Might need to set up other DMP functions later, just Quad6/Quad9 for now
#ifdef FULL_DEBUG
m_Logger.trace("[0x%02X] EEPROM gyro get(%d): [%d, %d, %d]", addr, count * 12, bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("[0x%02X] EEPROM accel get(%d): [%d, %d, %d]", addr, count * 12, bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("[0x%02X] EEPROM CPass get(%d): [%d, %d, %d]", addr, count * 12, bias_m[0], bias_m[1], bias_m[2]);
#endif
if (USE_6_AXIS)
{
if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, 1.25) == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X Set Quat6 to 100Hz frequency\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X Failed to Set Quat6 to 100Hz frequency\n", sensorId, addr);
return;
}
}
else
{
if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, 1.25) == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X Set Quat9 to 100Hz frequency\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X Failed to Set Quat9 to 100Hz frequency\n", sensorId, addr);
return;
}
}
imu.SetBiasGyroX(bias_g[0]);
imu.SetBiasGyroY(bias_g[1]);
imu.SetBiasGyroZ(bias_g[2]);
// Enable the FIFO
if(imu.enableFIFO() == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X FIFO Enabled\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X FIFO Enabling Failed\n", sensorId, addr);
return;
}
imu.SetBiasAccelX(bias_a[0]);
imu.SetBiasAccelY(bias_a[1]);
imu.SetBiasAccelZ(bias_a[2]);
// Enable the DMP
if(imu.enableDMP() == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X DMP Enabled\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X DMP Enabling Failed\n", sensorId, addr);
return;
}
imu.SetBiasCPassX(bias_m[0]);
imu.SetBiasCPassY(bias_m[1]);
imu.SetBiasCPassZ(bias_m[2]);
#elif ESP32
bool auxiliary = sensorId == 1;
// Reset DMP
if(imu.resetDMP() == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X Reset DMP\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X Failed to reset DMP\n", sensorId, addr);
return;
}
int32_t bias_a[3], bias_g[3], bias_m[3];
// Reset FIFO
if(imu.resetFIFO() == ICM_20948_Stat_Ok)
{
Serial.printf("[INFO] SensorId: %i Addr: 0x%02X Reset FIFO\n", sensorId, addr);
}
else
{
Serial.printf("[ERR] SensorId: %i Addr: 0x%02X Failed to reset FIFO\n", sensorId, addr);
return;
}
bias_a[0] = prefs.getInt(auxiliary ? "ba01" : "ba00", 0);
bias_a[1] = prefs.getInt(auxiliary ? "ba11" : "ba10", 0);
bias_a[2] = prefs.getInt(auxiliary ? "ba21" : "ba20", 0);
bias_g[0] = prefs.getInt(auxiliary ? "bg01" : "bg00", 0);
bias_g[1] = prefs.getInt(auxiliary ? "bg11" : "bg10", 0);
bias_g[2] = prefs.getInt(auxiliary ? "bg21" : "bg20", 0);
bias_m[0] = prefs.getInt(auxiliary ? "bm01" : "bm00", 0);
bias_m[1] = prefs.getInt(auxiliary ? "bm11" : "bm10", 0);
bias_m[2] = prefs.getInt(auxiliary ? "bm21" : "bm20", 0);
imu.SetBiasGyroX(bias_g[0]);
imu.SetBiasGyroY(bias_g[1]);
imu.SetBiasGyroZ(bias_g[2]);
#if defined(LOAD_BIAS) && LOAD_BIAS && ESP8266
int8_t count;
int32_t bias_g[3], bias_a[3], bias_m[3];
count = 0;
EEPROM.begin(4096); // max memory usage = 4096
EEPROM.get(addr + 100, count); // 1st imu counter in EEPROM addr: 0x69+100=205, 2nd addr: 0x68+100=204
Serial.printf("[0x%02X] EEPROM position: %d, count: %d \n", addr, addr + 100, count);
if(count < 0 || count > 42) {
count = sensorId; // 1st imu counter is even number, 2nd is odd
EEPROM.put(addr + 100, count);
}
EEPROM.get(1024 + (count * 12), bias_g); // 1024 ~ 2008
EEPROM.get(2046 + (count * 12), bias_a); // 2046 ~ 3030
EEPROM.get(3072 + (count * 12), bias_m); // 3072 ~ 4056
EEPROM.end();
Serial.printf("[0x%02X] EEPROM gyro get(%d): [%d, %d, %d] \n", addr, count * 12, bias_g[0], bias_g[1], bias_g[2]);
Serial.printf("[0x%02X] EEPROM accel get(%d): [%d, %d, %d] \n", addr, count * 12, bias_a[0], bias_a[1], bias_a[2]);
Serial.printf("[0x%02X] EEPROM CPass get(%d): [%d, %d, %d] \n\n", addr, count * 12, bias_m[0], bias_m[1], bias_m[2]);
imu.SetBiasAccelX(bias_a[0]);
imu.SetBiasAccelY(bias_a[1]);
imu.SetBiasAccelZ(bias_a[2]);
imu.SetBiasGyroX(bias_g[0]);
imu.SetBiasGyroY(bias_g[1]);
imu.SetBiasGyroZ(bias_g[2]);
imu.SetBiasCPassX(bias_m[0]);
imu.SetBiasCPassY(bias_m[1]);
imu.SetBiasCPassZ(bias_m[2]);
imu.SetBiasAccelX(bias_a[0]);
imu.SetBiasAccelY(bias_a[1]);
imu.SetBiasAccelZ(bias_a[2]);
// Display both values from ESP32 and read back from IMU
#ifdef FULL_DEBUG
m_Logger.trace("On IMU and on ESP32 should match");
m_Logger.trace("bias gyro on ESP32 : [%d, %d, %d]", bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("bias accel on ESP32: [%d, %d, %d]", bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("bias mag on ESP32 : [%d, %d, %d]", bias_m[0], bias_m[1], bias_m[2]);
imu.GetBiasGyroX(&bias_g[0]);
imu.GetBiasGyroY(&bias_g[1]);
imu.GetBiasGyroZ(&bias_g[2]);
imu.SetBiasCPassX(bias_m[0]);
imu.SetBiasCPassY(bias_m[1]);
imu.SetBiasCPassZ(bias_m[2]);
#else
if(BIAS_DEBUG)
{
imu.GetBiasAccelX(&bias_a[0]);
imu.GetBiasAccelY(&bias_a[1]);
imu.GetBiasAccelZ(&bias_a[2]);
imu.GetBiasCPassX(&bias_m[0]);
imu.GetBiasCPassY(&bias_m[1]);
imu.GetBiasCPassZ(&bias_m[2]);
m_Logger.trace("bias gyro on IMU : [%d, %d, %d]", bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("bias accel on IMU: [%d, %d, %d]", bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("bias mag on IMU : [%d, %d, %d]", bias_m[0], bias_m[1], bias_m[2]);
#endif
#endif // #if esp8266 / elif ESP32
#if BIAS_DEBUG
int bias_a[3], bias_g[3], bias_m[3];
imu.GetBiasGyroX(&bias_g[0]);
@@ -356,24 +279,9 @@ void ICM20948Sensor::motionSetup() {
imu.GetBiasCPassY(&bias_m[1]);
imu.GetBiasCPassZ(&bias_m[2]);
Serial.print("Starting Gyro Bias is ");
Serial.print(bias_g[0]);
Serial.print(",");
Serial.print(bias_g[1]);
Serial.print(",");
Serial.println(bias_g[2]);
Serial.print("Starting Accel Bias is ");
Serial.print(bias_a[0]);
Serial.print(",");
Serial.print(bias_a[1]);
Serial.print(",");
Serial.println(bias_a[2]);
Serial.print("Starting CPass Bias is ");
Serial.print(bias_m[0]);
Serial.print(",");
Serial.print(bias_m[1]);
Serial.print(",");
Serial.println(bias_m[2]);
m_Logger.trace("Starting Gyro Bias is %d, %d, %d", bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("Starting Accel Bias is %d, %d, %d", bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("Starting CPass Bias is %d, %d, %d", bias_m[0], bias_m[1], bias_m[2]);
//Sets all bias to 90
bias_g[0] = 90;
@@ -423,39 +331,183 @@ void ICM20948Sensor::motionSetup() {
imu.GetBiasCPassY(&bias_m[1]);
imu.GetBiasCPassZ(&bias_m[2]);
Serial.println("All set bias should be 90");
m_Logger.trace("All set bias should be 90");
Serial.print("Set Gyro Bias is ");
Serial.print(bias_g[0]);
Serial.print(",");
Serial.print(bias_g[1]);
Serial.print(",");
Serial.println(bias_g[2]);
Serial.print("Set Accel Bias is ");
Serial.print(bias_a[0]);
Serial.print(",");
Serial.print(bias_a[1]);
Serial.print(",");
Serial.println(bias_a[2]);
Serial.print("Set CPass Bias is ");
Serial.print(bias_m[0]);
Serial.print(",");
Serial.print(bias_m[1]);
Serial.print(",");
Serial.println(bias_m[2]);
}
m_Logger.trace("Set Gyro Bias is %d, %d, %d", bias_g[0], bias_g[1], bias_g[2]);
m_Logger.trace("Set Accel Bias is %d, %d, %d", bias_a[0], bias_a[1], bias_a[2]);
m_Logger.trace("Set CPass Bias is %d, %d, %d", bias_m[0], bias_m[1], bias_m[2]);
#endif // BIAS_DEBUG
#endif // LOAD_BIAS
}
void ICM20948Sensor::motionSetup() {
#ifdef ESP32
prefs.begin("ICM20948", false);
#endif
#ifdef FULL_DEBUG
imu.enableDebugging(Serial);
#endif
// SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error
boolean tracker = false;
if (addr == 0x68) {
tracker = false;
} else if (addr == 0x69)
{
tracker = true;
} else {
m_Logger.fatal("I2C Address not supported by ICM20948 library: 0x%02x", addr);
return;
}
//m_Logger.debug("Start Init with addr = %s", tracker ? "true" : "false");
ICM_20948_Status_e imu_err = imu.begin(Wire, tracker);
if (imu_err != ICM_20948_Stat_Ok) {
m_Logger.fatal("Can't connect to ICM20948 at address 0x%02x, error code: 0x%02x", addr, imu_err);
LEDManager::signalAssert();
return;
}
// Configure imu setup and load any stored bias values
if(imu.initializeDMP() == ICM_20948_Stat_Ok)
{
m_Logger.debug("DMP initialized");
}
else
{
m_Logger.fatal("Failed to initialize DMP");
return;
}
if (USE_6_AXIS)
{
m_Logger.debug("Using 6 axis configuration");
if(imu.enableDMPSensor(INV_ICM20948_SENSOR_GAME_ROTATION_VECTOR) == ICM_20948_Stat_Ok)
{
m_Logger.debug("Enabled DMP sensor for game rotation vector");
}
else
{
m_Logger.fatal("Failed to enable DMP sensor for game rotation vector");
return;
}
}
else
{
m_Logger.debug("Using 9 axis configuration");
if(imu.enableDMPSensor(INV_ICM20948_SENSOR_ORIENTATION) == ICM_20948_Stat_Ok)
{
m_Logger.debug("Enabled DMP sensor for sensor orientation");
}
else
{
m_Logger.fatal("Failed to enable DMP sensor orientation");
return;
}
}
// Might need to set up other DMP functions later, just Quad6/Quad9 for now
if (USE_6_AXIS)
{
if(imu.setDMPODRrate(DMP_ODR_Reg_Quat6, 1.25) == ICM_20948_Stat_Ok)
{
m_Logger.debug("Set Quat6 to 100Hz frequency");
}
else
{
m_Logger.fatal("Failed to set Quat6 to 100Hz frequency");
return;
}
}
else
{
if(imu.setDMPODRrate(DMP_ODR_Reg_Quat9, 1.25) == ICM_20948_Stat_Ok)
{
m_Logger.debug("Set Quat9 to 100Hz frequency");
}
else
{
m_Logger.fatal("Failed to set Quat9 to 100Hz frequency");
return;
}
}
// Enable the FIFO
if(imu.enableFIFO() == ICM_20948_Stat_Ok)
{
m_Logger.debug("FIFO Enabled");
}
else
{
m_Logger.fatal("Failed to enable FIFO");
return;
}
// Enable the DMP
if(imu.enableDMP() == ICM_20948_Stat_Ok)
{
m_Logger.debug("DMP Enabled");
}
else
{
m_Logger.fatal("Failed to enable DMP");
return;
}
// Reset DMP
if(imu.resetDMP() == ICM_20948_Stat_Ok)
{
m_Logger.debug("Reset DMP");
}
else
{
m_Logger.fatal("Failed to reset DMP");
return;
}
// Reset FIFO
if(imu.resetFIFO() == ICM_20948_Stat_Ok)
{
m_Logger.debug("Reset FIFO");
}
else
{
m_Logger.fatal("Failed to reset FIFO");
return;
}
load_bias();
lastData = millis();
working = true;
#if ESP8266 && defined(SAVE_BIAS)
#if defined(SAVE_BIAS) && SAVE_BIAS
timer.in(bias_save_periods[0] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->save_bias(true); return false; }, this);
#endif
}
void ICM20948Sensor::motionLoop() {
#if ENABLE_INSPECTION
{
(void)imu.getAGMT();
float rX = imu.gyrX();
float rY = imu.gyrY();
float rZ = imu.gyrZ();
float aX = imu.accX();
float aY = imu.accY();
float aZ = imu.accZ();
float mX = imu.magX();
float mY = imu.magY();
float mZ = imu.magZ();
Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255);
}
#endif
timer.tick();
bool dataavaliable = true;
@@ -480,6 +532,13 @@ void ICM20948Sensor::motionLoop() {
quaternion.y = q2;
quaternion.z = q3;
quaternion *= sensorOffset; //imu rotation
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
newData = true;
lastData = millis();
}
@@ -501,6 +560,13 @@ void ICM20948Sensor::motionLoop() {
quaternion.y = q2;
quaternion.z = q3;
quaternion *= sensorOffset; //imu rotation
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
newData = true;
lastData = millis();
}
@@ -508,25 +574,22 @@ void ICM20948Sensor::motionLoop() {
}
else
{
if (readStatus == ICM_20948_Stat_FIFONoDataAvail)
if (readStatus == ICM_20948_Stat_FIFONoDataAvail || lastData + 1000 < millis())
{
dataavaliable = false;
}
#ifdef FULL_DEBUG
else
{
Serial.print("e0x");
Serial.print(readStatus, HEX);
Serial.print(" ");
m_Logger.trace("e0x%02x", readStatus);
}
#endif
#endif
}
}
if(lastData + 1000 < millis()) {
working = false;
lastData = millis();
Serial.print("[ERR] Sensor timeout ");
Serial.println(addr);
lastData = millis();
m_Logger.error("Sensor timeout I2C Address 0x%02x", addr);
Network::sendError(1, this->sensorId);
}
}

20
src/sensors/icm20948sensor.h
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain, SlimeVR contributors
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -27,29 +27,31 @@
#include "sensor.h"
#include <arduino-timer.h> // Used for periodically saving bias
#ifdef ESP32
#include <Preferences.h> // ICM bias saving. ESP8266 use eprom
#include <Preferences.h> // ICM bias saving. ESP8266 use eprom
#endif
class ICM20948Sensor : public Sensor {
class ICM20948Sensor : public Sensor
{
public:
ICM20948Sensor() = default;
ICM20948Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM20948Sensor", IMU_ICM20948, id, address, rotation) {}
~ICM20948Sensor() override = default;
void motionSetup() override final;
void motionLoop() override final;
void sendData() override final;
void startCalibration(int calibrationType) override final;
void save_bias(bool repeat);
void load_bias();
private:
unsigned long lastData = 0;
int bias_save_counter = 0;
bool newTap;
ICM_20948_I2C imu;
ICM_20948_Device_t pdev;
icm_20948_DMP_data_t dmpData {};
icm_20948_DMP_data_t dmpData{};
#define OVERRIDEDMPSETUP true
#define OVERRIDEDMPSETUP true
#ifdef ESP32
Preferences prefs;
Timer<> timer = timer_create_default();
@@ -57,7 +59,7 @@ private:
#ifdef ESP8266
Timer<> timer = timer_create_default();
#endif
//TapDetector tapDetector;
// TapDetector tapDetector;
};
#endif // SLIMEVR_ICM20948SENSOR_H_
#endif // SLIMEVR_ICM20948SENSOR_H_

43
src/sensors/mpu6050sensor.cpp
View File

@@ -43,13 +43,11 @@ void MPU6050Sensor::motionSetup()
imu.initialize(addr);
if (!imu.testConnection())
{
Serial.print("[ERR] MPU6050: Can't communicate with MPU, response ");
Serial.println(imu.getDeviceID(), HEX);
m_Logger.fatal("Can't connect to MPU6050 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
return;
}
Serial.print("[OK] MPU6050: Connected to MPU, ID 0x");
Serial.println(imu.getDeviceID(), HEX);
m_Logger.info("Connected to MPU6050 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
devStatus = imu.dmpInitialize();
@@ -59,7 +57,7 @@ void MPU6050Sensor::motionSetup()
// We don't have to manually calibrate if we are using the dmp's automatic calibration
#else // IMU_MPU6050_RUNTIME_CALIBRATION
Serial.println(F("[NOTICE] Performing startup calibration of accel and gyro..."));
m_Logger.debug("Performing startup calibration of accel and gyro...");
// Do a quick and dirty calibration. As the imu warms up the offsets will change a bit, but this will be good-enough
delay(1000); // A small sleep to give the users a chance to stop it from moving
@@ -71,14 +69,14 @@ void MPU6050Sensor::motionSetup()
LEDManager::pattern(LOADING_LED, 50, 50, 5);
// turn on the DMP, now that it's ready
Serial.println(F("[NOTICE] Enabling DMP..."));
m_Logger.debug("Enabling DMP...");
imu.setDMPEnabled(true);
// TODO: Add interrupt support
// mpuIntStatus = imu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("[NOTICE] DMP ready! Waiting for first interrupt..."));
m_Logger.debug("DMP ready! Waiting for first interrupt...");
dmpReady = true;
// get expected DMP packet size for later comparison
@@ -93,14 +91,22 @@ void MPU6050Sensor::motionSetup()
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("[ERR] DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
m_Logger.error("DMP Initialization failed (code %d)", devStatus);
}
}
void MPU6050Sensor::motionLoop()
{
#if ENABLE_INSPECTION
{
int16_t rX, rY, rZ, aX, aY, aZ;
imu.getRotation(&rX, &rY, &rZ);
imu.getAcceleration(&aX, &aY, &aZ);
Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255);
}
#endif
if (!dmpReady)
return;
@@ -109,6 +115,13 @@ void MPU6050Sensor::motionLoop()
imu.dmpGetQuaternion(&rawQuat, fifoBuffer);
quaternion.set(-rawQuat.y, rawQuat.x, rawQuat.z, rawQuat.w);
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
newData = true;
@@ -120,7 +133,7 @@ void MPU6050Sensor::motionLoop()
void MPU6050Sensor::startCalibration(int calibrationType) {
LEDManager::on(CALIBRATING_LED);
#ifdef IMU_MPU6050_RUNTIME_CALIBRATION
Serial.println("MPU is using automatic runtime calibration. Place down the device and it should automatically calibrate after a few seconds");
m_Logger.info("MPU is using automatic runtime calibration. Place down the device and it should automatically calibrate after a few seconds");
// Lie to the server and say we've calibrated
switch (calibrationType)
@@ -135,7 +148,7 @@ void MPU6050Sensor::startCalibration(int calibrationType) {
LEDManager::off(CALIBRATING_LED);
#else //!IMU_MPU6050_RUNTIME_CALIBRATION
Serial.println("Put down the device and wait for baseline gyro reading calibration");
m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
delay(2000);
imu.setDMPEnabled(false);
@@ -143,8 +156,8 @@ void MPU6050Sensor::startCalibration(int calibrationType) {
imu.CalibrateAccel(6);
imu.setDMPEnabled(true);
Serial.println("Calibrated!");
Serial.println("[NOTICE] Starting offset finder");
m_Logger.debug("Gathered baseline gyro reading");
m_Logger.debug("Starting offset finder");
DeviceConfig *const config = getConfigPtr();
switch (calibrationType)
@@ -167,7 +180,7 @@ void MPU6050Sensor::startCalibration(int calibrationType) {
break;
}
Serial.println("[NOTICE] Process is over");
m_Logger.info("Calibration finished");
LEDMGR::off(CALIBRATING_LED);
#endif // !IMU_MPU6050_RUNTIME_CALIBRATION

4
src/sensors/mpu6050sensor.h
View File

@@ -26,7 +26,7 @@
class MPU6050Sensor : public Sensor
{
public:
MPU6050Sensor(){};
MPU6050Sensor(uint8_t id, uint8_t type, uint8_t address, float rotation) : Sensor("MPU6050Sensor", type, id, address, rotation){};
~MPU6050Sensor(){};
void motionSetup() override final;
void motionLoop() override final;
@@ -42,4 +42,4 @@ private:
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]{}; // FIFO storage buffer
};
};

84
src/sensors/mpu9250sensor.cpp
View File

@@ -43,27 +43,30 @@ void MPU9250Sensor::motionSetup() {
// initialize device
imu.initialize(addr);
if(!imu.testConnection()) {
Serial.print("[ERR] MPU9250: Can't communicate with MPU, response 0x");
Serial.println(imu.getDeviceID(), HEX);
m_Logger.fatal("Can't connect to MPU9250 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
return;
}
Serial.print("[OK] MPU9250: Connected to MPU, ID 0x");
Serial.println(imu.getDeviceID(), HEX);
m_Logger.info("Connected to MPU9250 (0x%02x) at address 0x%02x", imu.getDeviceID(), addr);
int16_t ax,ay,az;
// turn on while flip back to calibrate. then, flip again after 5 seconds.
// TODO: Move calibration invoke after calibrate button on slimeVR server available
// turn on while flip back to calibrate. then, flip again after 5 seconds.
// TODO: Move calibration invoke after calibrate button on slimeVR server available
imu.getAcceleration(&ax, &ay, &az);
if(az<0 && 10.0*(ax*ax+ay*ay)<az*az) {
float g_az = (float)az / 16384; // For 2G sensitivity
if(g_az < -0.75f) {
digitalWrite(CALIBRATING_LED, HIGH);
Serial.println("Calling Calibration... Flip front to confirm start calibration.");
m_Logger.info("Flip front to confirm start calibration");
delay(5000);
digitalWrite(CALIBRATING_LED, LOW);
imu.getAcceleration(&ax, &ay, &az);
if(az>0 && 10.0*(ax*ax+ay*ay)<az*az)
g_az = (float)az / 16384;
if(g_az > 0.75f)
{
m_Logger.debug("Starting calibration...");
startCalibration(0);
}
}
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
devStatus = imu.dmpInitialize();
@@ -76,14 +79,14 @@ void MPU9250Sensor::motionSetup() {
}
// turn on the DMP, now that it's ready
Serial.println(F("[NOTICE] Enabling DMP..."));
m_Logger.debug("Enabling DMP...");
imu.setDMPEnabled(true);
// TODO: Add interrupt support
// mpuIntStatus = imu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("[NOTICE] DMP ready! Waiting for first interrupt..."));
m_Logger.debug("DMP ready! Waiting for first interrupt...");
dmpReady = true;
// get expected DMP packet size for later comparison
@@ -94,9 +97,7 @@ void MPU9250Sensor::motionSetup() {
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("[ERR] DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
m_Logger.error("DMP Initialization failed (code %d)", devStatus);
}
#else
working = true;
@@ -106,6 +107,17 @@ void MPU9250Sensor::motionSetup() {
void MPU9250Sensor::motionLoop() {
#if ENABLE_INSPECTION
{
int16_t rX, rY, rZ, aX, aY, aZ, mX, mY, mZ;
imu.getRotation(&rX, &rY, &rZ);
imu.getAcceleration(&aX, &aY, &aZ);
imu.getMagnetometer(&mX, &mY, &mZ);
Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255);
}
#endif
#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
// Update quaternion
if(!dmpReady)
@@ -142,6 +154,13 @@ void MPU9250Sensor::motionLoop() {
#endif
quaternion *= sensorOffset;
#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif
if(!lastQuatSent.equalsWithEpsilon(quaternion)) {
newData = true;
lastQuatSent = quaternion;
@@ -194,12 +213,12 @@ void MPU9250Sensor::getMPUScaled()
#else
for (i = 0; i < 3; i++)
Mxyz[i] = (Mxyz[i] - calibration->M_B[i]);
#endif
#endif
}
void MPU9250Sensor::startCalibration(int calibrationType) {
LEDManager::on(CALIBRATING_LED);
Serial.println("[NOTICE] Gathering raw data for device calibration...");
m_Logger.debug("Gathering raw data for device calibration...");
constexpr int calibrationSamples = 300;
DeviceConfig *config = getConfigPtr();
// Reset values
@@ -208,7 +227,7 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
Gxyz[2] = 0;
// Wait for sensor to calm down before calibration
Serial.println("[NOTICE] Put down the device and wait for baseline gyro reading calibration");
m_Logger.info("Put down the device and wait for baseline gyro reading calibration");
delay(2000);
for (int i = 0; i < calibrationSamples; i++)
{
@@ -221,14 +240,18 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
Gxyz[0] /= calibrationSamples;
Gxyz[1] /= calibrationSamples;
Gxyz[2] /= calibrationSamples;
Serial.printf("[NOTICE] Gyro calibration results: %f %f %f\n", Gxyz[0], Gxyz[1], Gxyz[2]);
#ifdef FULL_DEBUG
m_Logger.trace("Gyro calibration results: %f %f %f", Gxyz[0], Gxyz[1], Gxyz[2]);
#endif
Network::sendRawCalibrationData(Gxyz, CALIBRATION_TYPE_EXTERNAL_GYRO, 0);
config->calibration[sensorId].G_off[0] = Gxyz[0];
config->calibration[sensorId].G_off[1] = Gxyz[1];
config->calibration[sensorId].G_off[2] = Gxyz[2];
// Blink calibrating led before user should rotate the sensor
Serial.println("[NOTICE] Gently rotate the device while it's gathering accelerometer and magnetometer data");
m_Logger.info("Gently rotate the device while it's gathering accelerometer and magnetometer data");
LEDManager::pattern(CALIBRATING_LED, 15, 300, 3000/310);
float *calibrationDataAcc = (float*)malloc(calibrationSamples * 3 * sizeof(float));
float *calibrationDataMag = (float*)malloc(calibrationSamples * 3 * sizeof(float));
@@ -248,31 +271,40 @@ void MPU9250Sensor::startCalibration(int calibrationType) {
LEDManager::off(CALIBRATING_LED);
delay(250);
}
Serial.println("[NOTICE] Now Calculate Calibration data");
m_Logger.debug("Calculating calibration data...");
float A_BAinv[4][3];
float M_BAinv[4][3];
CalculateCalibration(calibrationDataAcc, calibrationSamples, A_BAinv);
CalculateCalibration(calibrationDataMag, calibrationSamples, M_BAinv);
free(calibrationDataAcc);
CalculateCalibration(calibrationDataMag, calibrationSamples, M_BAinv);
free(calibrationDataMag);
Serial.println("[NOTICE] Finished Calculate Calibration data");
Serial.println("[NOTICE] Now Saving EEPROM");
m_Logger.debug("Finished Calculate Calibration data");
m_Logger.debug("Accelerometer calibration matrix:");
m_Logger.debug("{");
for (int i = 0; i < 3; i++)
{
config->calibration[sensorId].A_B[i] = A_BAinv[0][i];
config->calibration[sensorId].A_Ainv[0][i] = A_BAinv[1][i];
config->calibration[sensorId].A_Ainv[1][i] = A_BAinv[2][i];
config->calibration[sensorId].A_Ainv[2][i] = A_BAinv[3][i];
m_Logger.debug(" %f, %f, %f, %f", A_BAinv[0][i], A_BAinv[1][i], A_BAinv[2][i], A_BAinv[3][i]);
}
m_Logger.debug("}");
m_Logger.debug("[INFO] Magnetometer calibration matrix:");
m_Logger.debug("{");
for (int i = 0; i < 3; i++) {
config->calibration[sensorId].M_B[i] = M_BAinv[0][i];
config->calibration[sensorId].M_Ainv[0][i] = M_BAinv[1][i];
config->calibration[sensorId].M_Ainv[1][i] = M_BAinv[2][i];
config->calibration[sensorId].M_Ainv[2][i] = M_BAinv[3][i];
m_Logger.debug(" %f, %f, %f, %f", M_BAinv[0][i], M_BAinv[1][i], M_BAinv[2][i], M_BAinv[3][i]);
}
m_Logger.debug("}");
m_Logger.debug("Now Saving EEPROM");
setConfig(*config);
LEDManager::off(CALIBRATING_LED);
Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0);
Serial.println("[NOTICE] Finished Saving EEPROM");
Serial.println("[NOTICE] Calibration data gathered and sent");
m_Logger.debug("Finished Saving EEPROM");
m_Logger.info("Calibration data gathered");
}

13
src/sensors/mpu9250sensor.h
View File

@@ -21,13 +21,14 @@
THE SOFTWARE.
*/
#include "sensor.h"
#include "logging/Logger.h"
#include <MPU9250_6Axis_MotionApps_V6_12.h>
class MPU9250Sensor : public Sensor
{
public:
MPU9250Sensor(){};
MPU9250Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("MPU9250Sensor", IMU_MPU9250, id, address, rotation){};
~MPU9250Sensor(){};
void motionSetup() override final;
void motionLoop() override final;
@@ -43,15 +44,15 @@ private:
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]{}; // FIFO storage buffer
//raw data and scaled as vector
// raw data and scaled as vector
int skipCalcMag = 0;
float q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter
float Axyz[3]{};
float Gxyz[3]{};
float Mxyz[3]{};
float rawMag[3]{};
Quat correction{0,0,0,0};
Quat correction{0, 0, 0, 0};
// Loop timing globals
unsigned long now = 0, last = 0; //micros() timers
float deltat = 0; //loop time in seconds
};
unsigned long now = 0, last = 0; // micros() timers
float deltat = 0; // loop time in seconds
};

23
src/sensors/sensor.cpp
View File

@@ -25,14 +25,6 @@
#include <i2cscan.h>
#include "calibration.h"
void Sensor::setupSensor(uint8_t expectedSensorType, uint8_t sensorId, uint8_t addr, uint8_t intPin) {
this->sensorType = expectedSensorType;
this->addr = addr;
this->intPin = intPin;
this->sensorId = sensorId;
this->sensorOffset = {Quat(Vector3(0, 0, 1), sensorId == 0 ? IMU_ROTATION : SECOND_IMU_ROTATION)};
}
uint8_t Sensor::getSensorState() {
return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE;
}
@@ -41,17 +33,10 @@ void Sensor::sendData() {
if(newData) {
newData = false;
Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId);
#ifdef FULL_DEBUG
Serial.print("[DBG] Quaternion: ");
Serial.print(quaternion.x);
Serial.print(",");
Serial.print(quaternion.y);
Serial.print(",");
Serial.print(quaternion.z);
Serial.print(",");
Serial.print(quaternion.w);
Serial.print("\n");
#endif
#ifdef FULL_DEBUG
m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(quaternion));
#endif
}
}

28
src/sensors/sensor.h
View File

@@ -1,6 +1,6 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
@@ -29,6 +29,8 @@
#include <vector3.h>
#include "configuration.h"
#include "globals.h"
#include "logging/Logger.h"
#include "utils.h"
#define DATA_TYPE_NORMAL 1
#define DATA_TYPE_CORRECTION 2
@@ -36,9 +38,15 @@
class Sensor
{
public:
Sensor(){};
Sensor(const char *sensorName, uint8_t type, uint8_t id, uint8_t address, float rotation)
: addr(address), sensorId(id), sensorType(type), sensorOffset({Quat(Vector3(0, 0, 1), rotation)}), m_Logger(SlimeVR::Logging::Logger(sensorName))
{
char buf[4];
sprintf(buf, "%u", id);
m_Logger.setTag(buf);
}
virtual ~Sensor(){};
void setupSensor(uint8_t expectedSensorType, uint8_t sensorId, uint8_t addr, uint8_t intPin);
virtual void motionSetup(){};
virtual void motionLoop(){};
virtual void sendData();
@@ -57,28 +65,18 @@ public:
protected:
uint8_t addr = 0;
uint8_t intPin = 255;
uint8_t sensorId = 0;
uint8_t sensorType = 0;
bool configured = false;
bool newData = false;
bool working = false;
uint8_t calibrationAccuracy = 0;
Quat sensorOffset = Quat(Vector3(0, 0, 1), IMU_ROTATION);
Quat sensorOffset;
Quat quaternion{};
Quat lastQuatSent{};
};
class EmptySensor : public Sensor
{
public:
EmptySensor(){};
~EmptySensor(){};
void motionSetup() override final{};
void motionLoop() override final{};
void sendData() override final{};
void startCalibration(int calibrationType) override final{};
SlimeVR::Logging::Logger m_Logger;
};
const char * getIMUNameByType(int imuType);

136
src/sensors/sensorfactory.cpp
View File

@@ -1,136 +0,0 @@
/*
SlimeVR Code is placed under the MIT license
Copyright (c) 2021 Eiren Rain & SlimeVR contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include <i2cscan.h>
#include "sensorfactory.h"
#include "bno055sensor.h"
#include "bno080sensor.h"
#include "mpu9250sensor.h"
#include "mpu6050sensor.h"
#include "bmi160sensor.h"
#include "icm20948sensor.h"
SensorFactory::SensorFactory()
{
}
SensorFactory::~SensorFactory()
{
delete sensor1;
delete sensor2;
}
void SensorFactory::create()
{
uint8_t first_addr = 0;
#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO086
this->sensor1 = new BNO080Sensor();
first_addr = I2CSCAN::pickDevice(0x4A, 0x4B, true);
#elif IMU == IMU_BNO055
this->sensor1 = new BNO055Sensor();
first_addr = I2CSCAN::pickDevice(0x29, 0x28, true);
#elif IMU == IMU_MPU9250
this->sensor1 = new MPU9250Sensor();
first_addr = I2CSCAN::pickDevice(0x68, 0x69, true);
#elif IMU == IMU_BMI160
this->sensor1 = new BMI160Sensor();
first_addr = I2CSCAN::pickDevice(0x68, 0x69, true);
#elif IMU == IMU_MPU6500 || IMU == IMU_MPU6050
this->sensor1 = new MPU6050Sensor();
first_addr = I2CSCAN::pickDevice(0x68, 0x69, true);
#elif IMU == IMU_ICM20948
this->sensor1 = new ICM20948Sensor();
first_addr = I2CSCAN::pickDevice(0x68, 0x69, true);
#else
#error Unsupported IMU
#endif
if(first_addr == 0)
this->sensor1 = new EmptySensor();
this->sensor1->setupSensor(IMU, 0, first_addr, PIN_IMU_INT);
uint8_t second_addr = 0;
#ifndef SECOND_IMU
this->sensor2 = new EmptySensor();
#elif SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086
this->sensor2 = new BNO080Sensor();
second_addr = I2CSCAN::pickDevice(0x4B, 0x4A, false);
#elif SECOND_IMU == IMU_BNO055
this->sensor2 = new BNO055Sensor();
second_addr = I2CSCAN::pickDevice(0x28, 0x29, false);
#elif SECOND_IMU == IMU_MPU9250
this->sensor2 = new MPU9250Sensor();
second_addr = I2CSCAN::pickDevice(0x69, 0x68, false);
#elif SECOND_IMU == IMU_BMI160
this->sensor2 = new BMI160Sensor();
second_addr = I2CSCAN::pickDevice(0x69, 0x68, false);
#elif SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050
this->sensor2 = new MPU6050Sensor();
second_addr = I2CSCAN::pickDevice(0x69, 0x68, false);
#elif SECOND_IMU == IMU_ICM20948
this->sensor2 = new ICM20948Sensor();
second_addr = I2CSCAN::pickDevice(0x69, 0x68, false);
#else
#error Unsupported secondary IMU
#endif
if(first_addr == second_addr)
this->sensor2 = new EmptySensor();
this->sensor2->setupSensor(SECOND_IMU, 1, second_addr, PIN_IMU_INT_2);
}
void SensorFactory::motionSetup()
{
sensor1->motionSetup();
#ifdef SECOND_IMU
sensor2->motionSetup();
#endif
}
void SensorFactory::motionLoop()
{
sensor1->motionLoop();
#ifdef SECOND_IMU
sensor2->motionLoop();
#endif
}
void SensorFactory::sendData()
{
sensor1->sendData();
#ifdef SECOND_IMU
sensor2->sendData();
#endif
}
void SensorFactory::startCalibration(int sensorId, int calibrationType)
{
switch(sensorId) {
case 0:
sensor1->startCalibration(calibrationType);
break;
case 1:
sensor2->startCalibration(calibrationType);
break;
}
}

34
src/serial/serialcommands.cpp
View File

@@ -23,10 +23,13 @@
#include "serialcommands.h"
#include "network/network.h"
#include "logging/Logger.h"
#include <CmdCallback.hpp>
#include <EEPROM.h>
namespace SerialCommands {
SlimeVR::Logging::Logger logger("SerialCommands");
CmdCallback<5> cmdCallbacks;
CmdParser cmdParser;
CmdBuffer<64> cmdBuffer;
@@ -34,30 +37,27 @@ namespace SerialCommands {
void cmdSet(CmdParser * parser) {
if(parser->getParamCount() != 1 && parser->equalCmdParam(1, "WIFI") ) {
if(parser->getParamCount() < 3) {
Serial.println("[ERR] CMD SET WIFI ERROR: Too few arguments");
Serial.println("[NOTICE] Syntax: SET WIFI \"<SSID>\" \"<PASSWORD>\"");
logger.error("CMD SET WIFI ERROR: Too few arguments");
logger.info("Syntax: SET WIFI \"<SSID>\" \"<PASSWORD>\"");
} else {
WiFiNetwork::setWiFiCredentials(parser->getCmdParam(2), parser->getCmdParam(3));
Serial.println("[OK] CMD SET WIFI OK: New wifi credentials set, reconnecting");
logger.info("CMD SET WIFI OK: New wifi credentials set, reconnecting");
}
} else {
Serial.println("[ERR] CMD SET ERROR: Unrecognized variable to set");
logger.error("CMD SET ERROR: Unrecognized variable to set");
}
}
void cmdGet(CmdParser * parser) {
if(parser->getParamCount() != 1 && parser->equalCmdParam(1, "INFO") ) {
Serial.print("[OK] SlimeVR Tracker, ");
Serial.print("board: ");
Serial.print(BOARD);
Serial.print(", hardware: ");
Serial.print(HARDWARE_MCU);
Serial.print(", build: ");
Serial.print(FIRMWARE_BUILD_NUMBER);
Serial.print(", firmware: ");
Serial.print(FIRMWARE_VERSION);
Serial.print(", address: ");
Serial.println(WiFiNetwork::getAddress().toString());
logger.info(
"SlimeVR Tracker, board: %d, hardware: %d, build: %d, firmware: %s, address: %s",
BOARD,
HARDWARE_MCU,
FIRMWARE_BUILD_NUMBER,
FIRMWARE_VERSION,
WiFiNetwork::getAddress().toString().c_str()
);
// TODO Print sensors number and types
}
}
@@ -67,12 +67,12 @@ namespace SerialCommands {
}
void cmdReboot(CmdParser * parser) {
Serial.println("[OK] REBOOT");
logger.info("REBOOT");
ESP.restart();
}
void cmdFactoryReset(CmdParser * parser) {
Serial.print("[OK] FACTORY RESET");
logger.info("FACTORY RESET");
for (int i = 0; i <= 4096; i++) // Clear EEPROM
EEPROM.write(i, 0xFF);
EEPROM.commit();

7
src/utils.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef UTILS_H
#define UNPACK_VECTOR(V) V.x, V.y, V.z
#define UNPACK_QUATERNION(Q) Q.x, Q.y, Q.z, Q.w
#define UTILS_H
#endif