diff --git a/.gitignore b/.gitignore index 53b1951..d9fec7d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,3 @@ .pio -.vscode/.browse.c_cpp.db* -.vscode/c_cpp_properties.json -.vscode/launch.json -.vscode/ipch +.vscode/* build/ diff --git a/.vscode/extensions.json b/.vscode/extensions.json deleted file mode 100644 index e80666b..0000000 --- a/.vscode/extensions.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ] -} diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index d9dcd8e..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -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" - } -} \ No newline at end of file diff --git a/ci/build.py b/ci/build.py index fd30cb9..6a880d2 100644 --- a/ci/build.py +++ b/ci/build.py @@ -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__": diff --git a/ci/devices.json b/ci/devices.json new file mode 100644 index 0000000..63cf01f --- /dev/null +++ b/ci/devices.json @@ -0,0 +1,12 @@ +[ + { + "platform": "espressif8266", + "platformio_board": "esp12e", + "board": "SLIMEVR" + }, + { + "platform": "espressif32", + "platformio_board": "esp32dev", + "board": "WROOM32" + } +] diff --git a/ci/platformio.ini b/ci/platformio.ini new file mode 100644 index 0000000..c78a480 --- /dev/null +++ b/ci/platformio.ini @@ -0,0 +1,8 @@ +[env] +lib_deps= + https://github.com/SlimeVR/CmdParser.git +monitor_speed = 115200 +framework = arduino +build_flags = + -O2 +build_unflags = -Os diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp index 96bb5a6..9abb40f 100644 --- a/lib/bno080/BNO080.cpp +++ b/lib/bno080/BNO080.cpp @@ -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) diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index 1d92618..b227f55 100644 --- a/lib/bno080/BNO080.h +++ b/lib/bno080/BNO080.h @@ -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]; diff --git a/lib/magneto/magneto1.4.cpp b/lib/magneto/magneto1.4.cpp index c508bec..b47e523 100644 --- a/lib/magneto/magneto1.4.cpp +++ b/lib/magneto/magneto1.4.cpp @@ -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. diff --git a/platformio.ini b/platformio.ini index 0679873..9cd51c8 100644 --- a/platformio.ini +++ b/platformio.ini @@ -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 diff --git a/src/batterymonitor.cpp b/src/batterymonitor.cpp index 1933a8c..3286d20 100644 --- a/src/batterymonitor.cpp +++ b/src/batterymonitor.cpp @@ -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 } diff --git a/src/batterymonitor.h b/src/batterymonitor.h index 9fa29f8..413f062 100644 --- a/src/batterymonitor.h +++ b/src/batterymonitor.h @@ -29,6 +29,7 @@ #include #include #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_ diff --git a/src/debug.h b/src/debug.h index 2484c9c..6a8c40b 100644 --- a/src/debug.h +++ b/src/debug.h @@ -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_ diff --git a/src/ledmgr.cpp b/src/ledmgr.cpp index 7730a14..5d60169 100644 --- a/src/ledmgr.cpp +++ b/src/ledmgr.cpp @@ -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) { diff --git a/src/ledmgr.h b/src/ledmgr.h index df987d8..53c2d56 100644 --- a/src/ledmgr.h +++ b/src/ledmgr.h @@ -25,6 +25,7 @@ #include #include "globals.h" +#include "logging/Logger.h" #define LED_STATUS_SERVER_CONNECTING 2 #define LED_STATUS_WIFI_CONNECTING 4 diff --git a/src/logging/Level.cpp b/src/logging/Level.cpp new file mode 100644 index 0000000..71bce78 --- /dev/null +++ b/src/logging/Level.cpp @@ -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"; + } + } + } +} diff --git a/src/logging/Level.h b/src/logging/Level.h new file mode 100644 index 0000000..3bd8a72 --- /dev/null +++ b/src/logging/Level.h @@ -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 diff --git a/src/logging/Logger.cpp b/src/logging/Logger.cpp new file mode 100644 index 0000000..de7a5a6 --- /dev/null +++ b/src/logging/Logger.cpp @@ -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); + } + } +} diff --git a/src/logging/Logger.h b/src/logging/Logger.h new file mode 100644 index 0000000..dac6ed8 --- /dev/null +++ b/src/logging/Logger.h @@ -0,0 +1,109 @@ +#ifndef LOGGING_LOGGER_H +#define LOGGING_LOGGER_H + +#include "Level.h" +#include "debug.h" +#include + +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 + inline void traceArray(const char *str, const T *array, int size) + { + logArray(TRACE, str, array, size); + } + + template + inline void debugArray(const char *str, const T *array, int size) + { + logArray(DEBUG, str, array, size); + } + + template + inline void infoArray(const char *str, const T *array, int size) + { + logArray(INFO, str, array, size); + } + + template + inline void warnArray(const char *str, const T *array, int size) + { + logArray(WARN, str, array, size); + } + + template + inline void errorArray(const char *str, const T *array, int size) + { + logArray(ERROR, str, array, size); + } + + template + 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 + 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 diff --git a/src/main.cpp b/src/main.cpp index 2ae8e58..f9aefd5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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 diff --git a/src/network/packets.h b/src/network/packets.h index 9ce14fd..d3602d6 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -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_ \ No newline at end of file +#endif // SLIMEVR_PACKETS_H_ diff --git a/src/network/udpclient.cpp b/src/network/udpclient.cpp index 4ddd447..78a3984 100644 --- a/src/network/udpclient.cpp +++ b/src/network/udpclient.cpp @@ -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 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(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); } } - diff --git a/src/network/wifihandler.cpp b/src/network/wifihandler.cpp index 4340050..998481f 100644 --- a/src/network/wifihandler.cpp +++ b/src/network/wifihandler.cpp @@ -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(); } diff --git a/src/network/wifiprovisioning.cpp b/src/network/wifiprovisioning.cpp index ef978cd..38d3075 100644 --- a/src/network/wifiprovisioning.cpp +++ b/src/network/wifiprovisioning.cpp @@ -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"); } } diff --git a/src/sensors/sensorfactory.h b/src/sensors/EmptySensor.h similarity index 67% rename from src/sensors/sensorfactory.h rename to src/sensors/EmptySensor.h index bbc38d9..dff2693 100644 --- a/src/sensors/sensorfactory.h +++ b/src/sensors/EmptySensor.h @@ -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_ \ No newline at end of file +#endif diff --git a/src/sensors/ErroneousSensor.cpp b/src/sensors/ErroneousSensor.cpp new file mode 100644 index 0000000..340b7d8 --- /dev/null +++ b/src/sensors/ErroneousSensor.cpp @@ -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; + }; + } +} diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h new file mode 100644 index 0000000..8e2e2e7 --- /dev/null +++ b/src/sensors/ErroneousSensor.h @@ -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 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp new file mode 100644 index 0000000..c8545b2 --- /dev/null +++ b/src/sensors/SensorManager.cpp @@ -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 +#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(); + } + } +} diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h new file mode 100644 index 0000000..74dd601 --- /dev/null +++ b/src/sensors/SensorManager.h @@ -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_ diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 6ff1d3d..8ce68ab 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -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); } diff --git a/src/sensors/bmi160sensor.h b/src/sensors/bmi160sensor.h index 160bcc1..c0c597c 100644 --- a/src/sensors/bmi160sensor.h +++ b/src/sensors/bmi160sensor.h @@ -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; diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index ada1d1a..6552ca9 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -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; diff --git a/src/sensors/bno055sensor.h b/src/sensors/bno055sensor.h index 923c7d9..b6eca65 100644 --- a/src/sensors/bno055sensor.h +++ b/src/sensors/bno055sensor.h @@ -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; diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 4e5cc16..af7a422 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -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) diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index f7ba7df..44a4179 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -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{}; diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 104ced8..f50102c 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -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); } } diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index 616a026..0c88abf 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -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 // Used for periodically saving bias #ifdef ESP32 - #include // ICM bias saving. ESP8266 use eprom +#include // 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_ \ No newline at end of file +#endif // SLIMEVR_ICM20948SENSOR_H_ diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index ea40e9f..530d525 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -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 diff --git a/src/sensors/mpu6050sensor.h b/src/sensors/mpu6050sensor.h index 147f9e7..acf579a 100644 --- a/src/sensors/mpu6050sensor.h +++ b/src/sensors/mpu6050sensor.h @@ -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 -}; \ No newline at end of file +}; diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index be93e5b..a7c0f05 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -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)0 && 10.0*(ax*ax+ay*ay) 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"); } \ No newline at end of file diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h index e38b6f1..59ba1a7 100644 --- a/src/sensors/mpu9250sensor.h +++ b/src/sensors/mpu9250sensor.h @@ -21,13 +21,14 @@ THE SOFTWARE. */ #include "sensor.h" +#include "logging/Logger.h" #include 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 -}; \ No newline at end of file + unsigned long now = 0, last = 0; // micros() timers + float deltat = 0; // loop time in seconds +}; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index a3ae307..730f5e6 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -25,14 +25,6 @@ #include #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 } } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index b346756..d26fb7b 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -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 #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); diff --git a/src/sensors/sensorfactory.cpp b/src/sensors/sensorfactory.cpp deleted file mode 100644 index 786bb31..0000000 --- a/src/sensors/sensorfactory.cpp +++ /dev/null @@ -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 -#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; - } -} \ No newline at end of file diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index cbe8473..29b15b5 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -23,10 +23,13 @@ #include "serialcommands.h" #include "network/network.h" +#include "logging/Logger.h" #include #include 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 \"\" \"\""); + logger.error("CMD SET WIFI ERROR: Too few arguments"); + logger.info("Syntax: SET WIFI \"\" \"\""); } 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(); diff --git a/src/utils.h b/src/utils.h new file mode 100644 index 0000000..786c045 --- /dev/null +++ b/src/utils.h @@ -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 \ No newline at end of file