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

Store and load calibration config in EEPROM

Browse Source 
Recive commands and config from server
Send raw calibration data on calibration command, send config data on config command
Refactor some code
This commit is contained in:
Eiren Rain
2021-01-03 17:47:48 +03:00
parent e923809def
commit ddfad5b112
5 changed files with 324 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
include/configuration.cpp Normal file
View File

@@ -0,0 +1,22 @@
#include <EEPROM.h>
#include "configuration.h"
void initializeConfig() {
EEPROM.begin(sizeof(DeviceConfig) + 1);
}
bool hasConfigStored() {
bool hasConfigStored = false;
EEPROM.get(0, hasConfigStored);
return hasConfigStored;
}
void loadConfig(DeviceConfig * cfg) {
EEPROM.get(1, cfg);
}
void saveConfig(DeviceConfig * const cfg) {
EEPROM.put(0, true);
EEPROM.put(1, cfg);
EEPROM.commit();
}

25
include/configuration.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef _OWO_CONFIG_H_
#define _OWO_CONFIG_H_
struct CalibrationConfig {
//acel offsets and correction matrix
float A_B[3];
float A_Ainv[3][3];
// mag offsets and correction matrix
float M_B[3];
float M_Ainv[3][3];
//raw offsets, determined for gyro at rest
float G_off[3];
};
struct DeviceConfig {
CalibrationConfig calibration;
int deviceId;
};
void initializeConfig();
bool hasConfigStored();
void loadConfig(DeviceConfig * cfg);
void saveConfig(DeviceConfig * const cfg);
#endif // _OWO_CONFIG_H_

187
include/udpclient.cpp
View File

@@ -6,13 +6,15 @@
#include "wificredentials.h"
WiFiUDP Udp;
char incomingPacket[64]; // buffer for incoming packets
unsigned char incomingPacket[128]; // buffer for incoming packets
uint64_t packetNumber = 1;
unsigned char handshake[12] = {0,0,0,3,0,0,0,0,0,0,0,0};
unsigned char buf[64];
unsigned char buf[128];
configRecievedCallback fp_configCallback;
commandRecievedCallback fp_commandCallback;
template<typename T>
unsigned char* convert_to_chars(T src, unsigned char* target) {
unsigned char* convert_to_chars(T src, unsigned char * target) {
union {
unsigned char c[sizeof(T)];
T v;
@@ -25,7 +27,7 @@ unsigned char* convert_to_chars(T src, unsigned char* target) {
}
template<typename T>
T convert_chars(unsigned char* src) {
T convert_chars(unsigned char * const src) {
union {
unsigned char c[sizeof(T)];
T v;
@@ -36,26 +38,34 @@ T convert_chars(unsigned char* src) {
return un.v;
}
void sendVector(float * result, int type) {
void sendPacketNumber() {
uint64_t pn = packetNumber++;
// TODO Send packet number
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
}
void sendType(int type) {
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(type);
}
void sendVector(float * const result, int type) {
if(Udp.beginPacket(host, port) > 0) {
float x = result[0];
float y = result[1];
float z = result[2];
float w = 0;
uint64_t pn = packetNumber++;
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(type);
// TODO Send packet number
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
sendType(type);
sendPacketNumber();
Udp.write(convert_to_chars(x, buf), sizeof(x));
Udp.write(convert_to_chars(y, buf), sizeof(y));
Udp.write(convert_to_chars(z, buf), sizeof(z));
@@ -70,26 +80,14 @@ void sendVector(float * result, int type) {
}
}
void sendQuat(Quat * quaternion, int type) {
void sendQuat(Quat * const quaternion, int type) {
if(Udp.beginPacket(host, port) > 0) {
float x = quaternion->x;
float y = quaternion->y;
float z = quaternion->z;
float w = quaternion->w;
uint64_t pn = packetNumber++;
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(type);
// TODO Send packet number
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
sendType(type);
sendPacketNumber();
Udp.write(convert_to_chars(x, buf), sizeof(x));
Udp.write(convert_to_chars(y, buf), sizeof(y));
Udp.write(convert_to_chars(z, buf), sizeof(z));
@@ -104,26 +102,14 @@ void sendQuat(Quat * quaternion, int type) {
}
}
void sendQuat(float * quaternion, int type) {
void sendQuat(float * const quaternion, int type) {
if(Udp.beginPacket(host, port) > 0) {
float x = quaternion[0];
float y = quaternion[1];
float z = quaternion[2];
float w = quaternion[3];
uint64_t pn = packetNumber++;
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(type);
// TODO Send packet number
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
Udp.write(0);
sendType(type);
sendPacketNumber();
Udp.write(convert_to_chars(x, buf), sizeof(x));
Udp.write(convert_to_chars(y, buf), sizeof(y));
Udp.write(convert_to_chars(z, buf), sizeof(z));
@@ -138,6 +124,102 @@ void sendQuat(float * quaternion, int type) {
}
}
void sendConfig(DeviceConfig * const config, int type) {
if(Udp.beginPacket(host, port) > 0) {
DeviceConfig data = *config;
sendType(type);
sendPacketNumber();
Udp.write(convert_to_chars(data, buf), sizeof(data));
if(Udp.endPacket() == 0) {
//Serial.print("Write error: ");
//Serial.println(Udp.getWriteError());
}
} else {
//Serial.print("Write error: ");
//Serial.println(Udp.getWriteError());
}
}
void sendRawCalibrationData(int * const data, int type) {
if(Udp.beginPacket(host, port) > 0) {
int ax = data[0];
int ay = data[1];
int az = data[2];
int mx = data[3];
int my = data[4];
int mz = data[5];
sendType(type);
sendPacketNumber();
Udp.write(convert_to_chars(ax, buf), sizeof(ax));
Udp.write(convert_to_chars(ay, buf), sizeof(ay));
Udp.write(convert_to_chars(az, buf), sizeof(az));
Udp.write(convert_to_chars(mx, buf), sizeof(mx));
Udp.write(convert_to_chars(my, buf), sizeof(my));
Udp.write(convert_to_chars(mz, buf), sizeof(mz));
if(Udp.endPacket() == 0) {
//Serial.print("Write error: ");
//Serial.println(Udp.getWriteError());
}
} else {
//Serial.print("Write error: ");
//Serial.println(Udp.getWriteError());
}
}
void setConfigRecievedCallback(configRecievedCallback callback) {
fp_configCallback = callback;
}
void setCommandRecievedCallback(commandRecievedCallback callback) {
fp_commandCallback = callback;
}
void clientUpdate() {
if(WiFi.status() == WL_CONNECTED) {
int packetSize = Udp.parsePacket();
if (packetSize)
{
// receive incoming UDP packets
Serial.printf("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("UDP packet contents: ");
for(int i = 0; i < len; ++i)
Serial.print((byte) incomingPacket[i]);
Serial.println();
switch(convert_chars<int>(incomingPacket)) {
case PACKET_RECIEVE_HEARTBEAT:
break;
case PACKET_RECIEVE_VIBRATE:
break;
case PACKET_RECIEVE_HANDSHAKE:
// Assume handshake sucessful
Serial.println("Handshale recived again, ignoring");
break;
case PACKET_RECIEVE_COMMAND:
if(len < 6) {
Serial.println("Command packet too short");
break;
}
Serial.printf("Recieved command %d\n", incomingPacket[4]);
if(fp_commandCallback) {
fp_commandCallback(incomingPacket[4], &incomingPacket[5], len - 6);
}
break;
case PACKET_CONFIG:
if(len < sizeof(DeviceConfig) + 4) {
Serial.println("config packet too short");
break;
}
if(fp_configCallback) {
fp_configCallback(convert_chars<DeviceConfig>(&incomingPacket[4]));
}
break;
}
}
}
}
void connectClient() {
Serial.print("Connecting to wifi ");
Serial.println(networkName);
@@ -160,15 +242,16 @@ void connectClient() {
{
// receive incoming UDP packets
Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
int len = Udp.read(incomingPacket, 64);
int len = Udp.read(incomingPacket, sizeof(incomingPacket));
Serial.print("UDP packet contents: ");
for(int i = 0; i < len; ++i)
Serial.print((byte) incomingPacket[i]);
Serial.println();
if(incomingPacket[0] == 3) {
switch(incomingPacket[0]) {
case PACKET_HANDSHAKE:
// Assume handshake sucessful
Serial.printf("Handshale sucessful");
Serial.println("Handshale sucessful");
break;
}
}

27
include/udpclient.h
View File

@@ -5,17 +5,38 @@
#include <WiFiUdp.h>
#include <Arduino.h>
#include "quat.h"
#include "configuration.h"
#define PACKET_ROTATION 1
#define PACKET_GYRO 2
#define PACKET_HANDSHAKE 3
#define PACKET_ACCEL 4
#define PACKET_MAG 5
#define PACKET_RAW_CALIBRATION_DATA 6
#define PACKET_GYRO_CALIBRATION_DATA 7
#define PACKET_CONFIG 8
#define PACKET_RECIEVE_HEARTBEAT 1
#define PACKET_RECIEVE_VIBRATE 2
#define PACKET_RECIEVE_HANDSHAKE 3
#define PACKET_RECIEVE_COMMAND 4
#define COMMAND_CALLIBRATE 1
#define COMMAND_SEND_CONFIG 2
typedef void (* configRecievedCallback)(DeviceConfig const);
typedef void (* commandRecievedCallback)(int, void * const, int);
void connectClient();
void sendQuat(float * quaternion, int type);
void sendQuat(Quat * quaternion, int type);
void sendVector(float * result, int type);
void clientUpdate();
void sendQuat(float * const quaternion, int type);
void sendQuat(Quat * const quaternion, int type);
void sendVector(float * const result, int type);
void sendConfig(DeviceConfig * const config, int type);
void sendRawCalibrationData(int * const data, int type);
void setConfigRecievedCallback(configRecievedCallback);
void setCommandRecievedCallback(commandRecievedCallback);
template<typename T> T convert_chars(unsigned char* src);
template<typename T> unsigned char* convert_to_chars(T src, unsigned char* target);

158
src/main.cpp
View File

@@ -27,6 +27,10 @@
#include "defines.h"
#include "quat.cpp"
#include "util.cpp"
#include "configuration.cpp"
#define LOADING_LED LED_BUILTIN
#define CALIBRATING_LED LED_BUILTIN
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
@@ -36,30 +40,9 @@
MPU9250 accelgyro;
I2Cdev I2C_M;
DeviceConfig config;
const CalibrationConfig & calibration = config.calibration;
// vvvvvvvvvvvvvvvvvv VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv
//These are the previously determined offsets and scale factors for accelerometer and magnetometer, using MPU9250_cal and Magneto 1.2
//The AHRS will NOT work well or at all if these are not correct
//
// redetermined 12/16/2020
//acel offsets and correction matrix
float A_B[3]{-807.71, 136.80, 290.84};
float A_Ainv[3][3]{{0.60963, -0.00451, 0.00042},
{-0.00451, 0.62762, 0.00492},
{0.00042, 0.00492, 0.60485}};
// mag offsets and correction matrix
float M_B[3]{32.06, 34.44, -97.59};
float M_Ainv[3][3]{{1.68116, 0.08451, -0.01659},
{0.08451, 1.55939, 0.06883},
{-0.01659, 0.06883, 1.61732}};
float G_off[3] = {421.3, -236.7, 4.8}; //raw offsets, determined for gyro at rest
// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
char s[60]; //snprintf buffer
//raw data and scaled as vector
int16_t ax, ay, az;
int16_t gx, gy, gz;
@@ -67,6 +50,7 @@ int16_t mx, my, mz;
float Axyz[3];
float Gxyz[3];
float Mxyz[3];
bool isCalibrating = false;
#define gscale (250. / 32768.0) * (PI / 180.0) //gyro default 250 LSB per d/s -> rad/s
// NOW USING MAHONY FILTER
@@ -92,16 +76,54 @@ static Quat rotationQuat = Quat(Vector3(0, 0, 1), PI / 2.0);
void get_MPU_scaled(void);
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
void setConfig(DeviceConfig newConfig) {
config = newConfig;
saveConfig(&config);
}
void commandRecieved(int command, void * const data, int dataLength) {
switch(command) {
case COMMAND_CALLIBRATE:
isCalibrating = true;
break;
case COMMAND_SEND_CONFIG:
sendConfig(&config, PACKET_CONFIG);
break;
}
}
void performCalibration();
void setup()
{
//pinMode(LED_BUILTIN, OUTPUT);
//digitalWrite(LED_BUILTIN, LOW); // Glow diode while loading
// Glow diode while loading
pinMode(LOADING_LED, OUTPUT);
pinMode(CALIBRATING_LED, OUTPUT);
digitalWrite(CALIBRATING_LED, HIGH);
digitalWrite(LOADING_LED, LOW);
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin();
Serial.begin(serialBaudRate);
while (!Serial)
; //wait for connection
while (!Serial); // wait for connection
// Load default calibration values and set up callbacks
config.calibration = {
{-807.71, 136.80, 290.84},
{{0.60963, -0.00451, 0.00042},
{-0.00451, 0.62762, 0.00492},
{0.00042, 0.00492, 0.60485}},
{32.06, 34.44, -97.59},
{{1.68116, 0.08451, -0.01659},
{0.08451, 1.55939, 0.06883},
{-0.01659, 0.06883, 1.61732}},
{421.3, -236.7, 4.8}
};
if(hasConfigStored()) {
loadConfig(&config);
}
setConfigRecievedCallback(setConfig);
setCommandRecievedCallback(commandRecieved);
// initialize device
accelgyro.initialize();
@@ -112,16 +134,21 @@ void setup()
Serial.println(accelgyro.getDeviceID(), HEX);
delay(500);
}
Serial.println("Connected to MPU9250");*/
Serial.println("Connected to MPU9250");
//*/
connectClient();
//digitalWrite(LED_BUILTIN, HIGH);
digitalWrite(LOADING_LED, HIGH);
}
// AHRS loop
void loop()
{
clientUpdate();
if(isCalibrating) {
performCalibration();
}
get_MPU_scaled();
now = micros();
deltat = (now - last) * 1.0e-6; //seconds since last update
@@ -182,19 +209,19 @@ void get_MPU_scaled(void)
int i;
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
Gxyz[0] = ((float)gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
Gxyz[1] = ((float)gy - G_off[1]) * gscale;
Gxyz[2] = ((float)gz - G_off[2]) * gscale;
Gxyz[0] = ((float)gx - calibration.G_off[0]) * gscale; //250 LSB(d/s) default to radians/s
Gxyz[1] = ((float)gy - calibration.G_off[1]) * gscale;
Gxyz[2] = ((float)gz - calibration.G_off[2]) * gscale;
Axyz[0] = (float)ax;
Axyz[1] = (float)ay;
Axyz[2] = (float)az;
//apply offsets (bias) and scale factors from Magneto
for (i = 0; i < 3; i++)
temp[i] = (Axyz[i] - A_B[i]);
Axyz[0] = A_Ainv[0][0] * temp[0] + A_Ainv[0][1] * temp[1] + A_Ainv[0][2] * temp[2];
Axyz[1] = A_Ainv[1][0] * temp[0] + A_Ainv[1][1] * temp[1] + A_Ainv[1][2] * temp[2];
Axyz[2] = A_Ainv[2][0] * temp[0] + A_Ainv[2][1] * temp[1] + A_Ainv[2][2] * temp[2];
temp[i] = (Axyz[i] - calibration.A_B[i]);
Axyz[0] = calibration.A_Ainv[0][0] * temp[0] + calibration.A_Ainv[0][1] * temp[1] + calibration.A_Ainv[0][2] * temp[2];
Axyz[1] = calibration.A_Ainv[1][0] * temp[0] + calibration.A_Ainv[1][1] * temp[1] + calibration.A_Ainv[1][2] * temp[2];
Axyz[2] = calibration.A_Ainv[2][0] * temp[0] + calibration.A_Ainv[2][1] * temp[1] + calibration.A_Ainv[2][2] * temp[2];
vector_normalize(Axyz);
Mxyz[0] = (float)mx;
@@ -202,10 +229,10 @@ void get_MPU_scaled(void)
Mxyz[2] = (float)mz;
//apply offsets and scale factors from Magneto
for (i = 0; i < 3; i++)
temp[i] = (Mxyz[i] - M_B[i]);
Mxyz[0] = M_Ainv[0][0] * temp[0] + M_Ainv[0][1] * temp[1] + M_Ainv[0][2] * temp[2];
Mxyz[1] = M_Ainv[1][0] * temp[0] + M_Ainv[1][1] * temp[1] + M_Ainv[1][2] * temp[2];
Mxyz[2] = M_Ainv[2][0] * temp[0] + M_Ainv[2][1] * temp[1] + M_Ainv[2][2] * temp[2];
temp[i] = (Mxyz[i] - calibration.M_B[i]);
Mxyz[0] = calibration.M_Ainv[0][0] * temp[0] + calibration.M_Ainv[0][1] * temp[1] + calibration.M_Ainv[0][2] * temp[2];
Mxyz[1] = calibration.M_Ainv[1][0] * temp[0] + calibration.M_Ainv[1][1] * temp[1] + calibration.M_Ainv[1][2] * temp[2];
Mxyz[2] = calibration.M_Ainv[2][0] * temp[0] + calibration.M_Ainv[2][1] * temp[1] + calibration.M_Ainv[2][2] * temp[2];
vector_normalize(Mxyz);
}
@@ -303,4 +330,55 @@ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, fl
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
void performCalibration() {
digitalWrite(CALIBRATING_LED, LOW);
Serial.println("Gathering raw data for device calibration...");
int calibrationSamples = 300;
// Reset values
Gxyz[0] = 0;
Gxyz[1] = 0;
Gxyz[2] = 0;
// Wait for sensor to calm down before calibration
Serial.println("Put down the device and wait for baseline gyro reading calibration");
delay(2000);
for (int i = 0; i < calibrationSamples; i++)
{
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
Gxyz[0] += float(gx);
Gxyz[1] += float(gy);
Gxyz[2] += float(gz);
}
Gxyz[0] /= calibrationSamples;
Gxyz[1] /= calibrationSamples;
Gxyz[2] /= calibrationSamples;
Serial.printf("Gyro calibration results: %f %f %f\n", Gxyz[0], Gxyz[1], Gxyz[2]);
sendVector(Gxyz, PACKET_GYRO_CALIBRATION_DATA);
// Blink calibrating led before user should rotate the sensor
Serial.println("Gently rotate the device while it's gathering accelerometer and magnetometer data");
for(int i = 0; i < 2000 / 20; ++i) {
digitalWrite(CALIBRATING_LED, HIGH);
delay(2000 / 10);
digitalWrite(CALIBRATING_LED, LOW);
}
int calibrationData[6];
for (int i = 0; i < calibrationSamples; i++)
{
digitalWrite(CALIBRATING_LED, LOW);
accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
calibrationData[0] = ax;
calibrationData[1] = ay;
calibrationData[2] = az;
calibrationData[3] = mx;
calibrationData[4] = my;
calibrationData[5] = mz;
sendRawCalibrationData(calibrationData, PACKET_RAW_CALIBRATION_DATA);
digitalWrite(CALIBRATING_LED, HIGH);
delay(300);
}
Serial.println("Calibration data gathered and sent");
digitalWrite(CALIBRATING_LED, HIGH);
}