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

Added new offset finder code for MPU6050

Browse Source 
Added code for BNO08X
Added code to send serial to server
This commit is contained in:
Eiren Rain
2021-02-23 17:57:44 +03:00
parent 1de6d62ef3
commit 5ec06bf075
10 changed files with 2438 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
include/6axismotion.cpp
View File

@@ -1,4 +1,5 @@
#include "motionbase.cpp"
#include "motionbase.h"
#include "MPU6050OffsetFinder.cpp"
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
@@ -69,7 +70,7 @@ void motionLoop() {
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
accelgyro.dmpGetQuaternion(&rawQuat, fifoBuffer);
q[0] = rawQuat.x;
q[1] = rawQuat.y;
@@ -86,6 +87,13 @@ void sendData() {
void performCalibration() {
digitalWrite(CALIBRATING_LED, LOW);
Serial.println("Starting offset finder");
findOffset();
Serial.println("Process is over");
digitalWrite(CALIBRATING_LED, HIGH);
}
void gatherCalibrationData() {
Serial.println("Gathering raw data for device calibration...");
int calibrationSamples = 500;
// Reset values
@@ -139,5 +147,4 @@ void performCalibration() {
delay(50);
}
Serial.println("Calibration data gathered and sent");
digitalWrite(CALIBRATING_LED, HIGH);
}

2
include/9axismotion.cpp
View File

@@ -1,4 +1,4 @@
#include "motionbase.cpp"
#include "motionbase.h"
//raw data and scaled as vector
int16_t ax, ay, az;

1637
include/BNO080.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

303
include/BNO080.h Normal file
View File

@@ -0,0 +1,303 @@
/*
This is a library written for the BNO080
SparkFun sells these at its website: www.sparkfun.com
Do you like this library? Help support SparkFun. Buy a board!
https://www.sparkfun.com/products/14686
Written by Nathan Seidle @ SparkFun Electronics, December 28th, 2017
The BNO080 IMU is a powerful triple axis gyro/accel/magnetometer coupled with an ARM processor
to maintain and complete all the complex calculations for various VR, inertial, step counting,
and movement operations.
This library handles the initialization of the BNO080 and is able to query the sensor
for different readings.
https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library
Development environment specifics:
Arduino IDE 1.8.3
SparkFun code, firmware, and software is released under the MIT License.
Please see LICENSE.md for further details.
*/
#pragma once
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <Wire.h>
#include <SPI.h>
//The default I2C address for the BNO080 on the SparkX breakout is 0x4B. 0x4A is also possible.
#define BNO080_DEFAULT_ADDRESS 0x4B
//Platform specific configurations
//Define the size of the I2C buffer based on the platform the user has
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
//I2C_BUFFER_LENGTH is defined in Wire.H
#define I2C_BUFFER_LENGTH BUFFER_LENGTH
#else
//The catch-all default is 32
#define I2C_BUFFER_LENGTH 32
#endif
//-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
//Registers
const byte CHANNEL_COMMAND = 0;
const byte CHANNEL_EXECUTABLE = 1;
const byte CHANNEL_CONTROL = 2;
const byte CHANNEL_REPORTS = 3;
const byte CHANNEL_WAKE_REPORTS = 4;
const byte CHANNEL_GYRO = 5;
//All the ways we can configure or talk to the BNO080, figure 34, page 36 reference manual
//These are used for low level communication with the sensor, on channel 2
#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD
//All the different sensors and features we can get reports from
//These are used when enabling a given sensor
#define SENSOR_REPORTID_ACCELEROMETER 0x01
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_MAGNETIC_FIELD 0x03
#define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GRAVITY 0x06
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
#define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09
#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A
#define SENSOR_REPORTID_TAP_DETECTOR 0x10
#define SENSOR_REPORTID_STEP_COUNTER 0x11
#define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13
#define SENSOR_REPORTID_RAW_ACCELEROMETER 0x14
#define SENSOR_REPORTID_RAW_GYROSCOPE 0x15
#define SENSOR_REPORTID_RAW_MAGNETOMETER 0x16
#define SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER 0x1E
#define SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR 0x28
#define SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR 0x29
//Record IDs from figure 29, page 29 reference manual
//These are used to read the metadata for each sensor type
#define FRS_RECORDID_ACCELEROMETER 0xE302
#define FRS_RECORDID_GYROSCOPE_CALIBRATED 0xE306
#define FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED 0xE309
#define FRS_RECORDID_ROTATION_VECTOR 0xE30B
//Command IDs from section 6.4, page 42
//These are used to calibrate, initialize, set orientation, tare etc the sensor
#define COMMAND_ERRORS 1
#define COMMAND_COUNTER 2
#define COMMAND_TARE 3
#define COMMAND_INITIALIZE 4
#define COMMAND_DCD 6
#define COMMAND_ME_CALIBRATE 7
#define COMMAND_DCD_PERIOD_SAVE 9
#define COMMAND_OSCILLATOR 10
#define COMMAND_CLEAR_DCD 11
#define CALIBRATE_ACCEL 0
#define CALIBRATE_GYRO 1
#define CALIBRATE_MAG 2
#define CALIBRATE_PLANAR_ACCEL 3
#define CALIBRATE_ACCEL_GYRO_MAG 4
#define CALIBRATE_STOP 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)
class BNO080
{
public:
boolean begin(uint8_t deviceAddress = BNO080_DEFAULT_ADDRESS, TwoWire &wirePort = Wire, uint8_t intPin = 255); //By default use the default I2C addres, and use Wire port, and don't declare an INT pin
boolean beginSPI(uint8_t user_CSPin, uint8_t user_WAKPin, uint8_t user_INTPin, uint8_t user_RSTPin, uint32_t spiPortSpeed = 3000000, SPIClass &spiPort = SPI);
void enableDebugging(Stream &debugPort = Serial); //Turn on debug printing. If user doesn't specify then Serial will be used.
void softReset(); //Try to reset the IMU via software
uint8_t resetReason(); //Query the IMU for the reason it last reset
void modeOn(); //Use the executable channel to turn the BNO on
void modeSleep(); //Use the executable channel to put the BNO to sleep
float qToFloat(int16_t fixedPointValue, uint8_t qPoint); //Given a Q value, converts fixed point floating to regular floating point number
boolean waitForI2C(); //Delay based polling for I2C traffic
boolean waitForSPI(); //Delay based polling for INT pin to go low
boolean receivePacket(void);
boolean getData(uint16_t bytesRemaining); //Given a number of bytes, send the requests in I2C_BUFFER_LENGTH chunks
boolean sendPacket(uint8_t channelNumber, uint8_t dataLength);
void printPacket(void); //Prints the current shtp header and data packets
void printHeader(void); //Prints the current shtp header (only)
void enableRotationVector(uint16_t timeBetweenReports);
void enableGameRotationVector(uint16_t timeBetweenReports);
void enableARVRStabilizedRotationVector(uint16_t timeBetweenReports);
void enableARVRStabilizedGameRotationVector(uint16_t timeBetweenReports);
void enableAccelerometer(uint16_t timeBetweenReports);
void enableLinearAccelerometer(uint16_t timeBetweenReports);
void enableGyro(uint16_t timeBetweenReports);
void enableMagnetometer(uint16_t timeBetweenReports);
void enableTapDetector(uint16_t timeBetweenReports);
void enableStepCounter(uint16_t timeBetweenReports);
void enableStabilityClassifier(uint16_t timeBetweenReports);
void enableActivityClassifier(uint16_t timeBetweenReports, uint32_t activitiesToEnable, uint8_t (&activityConfidences)[9]);
void enableRawAccelerometer(uint16_t timeBetweenReports);
void enableRawGyro(uint16_t timeBetweenReports);
void enableRawMagnetometer(uint16_t timeBetweenReports);
void enableGyroIntegratedRotationVector(uint16_t timeBetweenReports);
bool dataAvailable(void);
uint16_t getReadings(void);
uint16_t parseInputReport(void); //Parse sensor readings out of report
uint16_t parseCommandReport(void); //Parse command responses out of report
void getQuat(float &i, float &j, float &k, float &real, float &radAccuracy, uint8_t &accuracy);
float getQuatI();
float getQuatJ();
float getQuatK();
float getQuatReal();
float getQuatRadianAccuracy();
uint8_t getQuatAccuracy();
void getAccel(float &x, float &y, float &z, uint8_t &accuracy);
float getAccelX();
float getAccelY();
float getAccelZ();
uint8_t getAccelAccuracy();
void getLinAccel(float &x, float &y, float &z, uint8_t &accuracy);
float getLinAccelX();
float getLinAccelY();
float getLinAccelZ();
uint8_t getLinAccelAccuracy();
void getGyro(float &x, float &y, float &z, uint8_t &accuracy);
float getGyroX();
float getGyroY();
float getGyroZ();
uint8_t getGyroAccuracy();
void getFastGyro(float &x, float &y, float &z);
float getFastGyroX();
float getFastGyroY();
float getFastGyroZ();
void getMag(float &x, float &y, float &z, uint8_t &accuracy);
float getMagX();
float getMagY();
float getMagZ();
uint8_t getMagAccuracy();
void calibrateAccelerometer();
void calibrateGyro();
void calibrateMagnetometer();
void calibratePlanarAccelerometer();
void calibrateAll();
void endCalibration();
void saveCalibration();
void requestCalibrationStatus(); //Sends command to get status
boolean calibrationComplete(); //Checks ME Cal response for byte 5, R0 - Status
uint8_t getTapDetector();
uint32_t getTimeStamp();
uint16_t getStepCount();
uint8_t getStabilityClassifier();
uint8_t getActivityClassifier();
int16_t getRawAccelX();
int16_t getRawAccelY();
int16_t getRawAccelZ();
int16_t getRawGyroX();
int16_t getRawGyroY();
int16_t getRawGyroZ();
int16_t getRawMagX();
int16_t getRawMagY();
int16_t getRawMagZ();
float getRoll();
float getPitch();
float getYaw();
void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports);
void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig);
void sendCommand(uint8_t command);
void sendCalibrateCommand(uint8_t thingToCalibrate);
//Metadata functions
int16_t getQ1(uint16_t recordID);
int16_t getQ2(uint16_t recordID);
int16_t getQ3(uint16_t recordID);
float getResolution(uint16_t recordID);
float getRange(uint16_t recordID);
uint32_t readFRSword(uint16_t recordID, uint8_t wordNumber);
void frsReadRequest(uint16_t recordID, uint16_t readOffset, uint16_t blockSize);
bool readFRSdata(uint16_t recordID, uint8_t startLocation, uint8_t wordsToRead);
//Global Variables
uint8_t shtpHeader[4]; //Each packet has a header of 4 bytes
uint8_t shtpData[MAX_PACKET_SIZE];
uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0}; //There are 6 com channels. Each channel has its own seqnum
uint8_t commandSequenceNumber = 0; //Commands have a seqNum as well. These are inside command packet, the header uses its own seqNum per channel
uint32_t metaData[MAX_METADATA_SIZE]; //There is more than 10 words in a metadata record but we'll stop at Q point 3
private:
//Variables
TwoWire *_i2cPort; //The generic connection to user's chosen I2C hardware
uint8_t _deviceAddress; //Keeps track of I2C address. setI2CAddress changes this.
Stream *_debugPort; //The stream to send debug messages to if enabled. Usually Serial.
boolean _printDebug = false; //Flag to print debugging variables
SPIClass *_spiPort; //The generic connection to user's chosen SPI hardware
unsigned long _spiPortSpeed; //Optional user defined port speed
uint8_t _cs; //Pins needed for SPI
uint8_t _wake;
uint8_t _int;
uint8_t _rst;
//These are the raw sensor values (without Q applied) pulled from the user requested Input Report
uint16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy;
uint16_t rawLinAccelX, rawLinAccelY, rawLinAccelZ, accelLinAccuracy;
uint16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy;
uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy;
uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy;
uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ;
uint8_t tapDetector;
uint16_t stepCount;
uint32_t timeStamp;
uint8_t stabilityClassifier;
uint8_t activityClassifier;
uint8_t *_activityConfidences; //Array that store the confidences of the 9 possible activities
uint8_t calibrationStatus; //Byte R0 of ME Calibration Response
uint16_t memsRawAccelX, memsRawAccelY, memsRawAccelZ; //Raw readings from MEMS sensor
uint16_t memsRawGyroX, memsRawGyroY, memsRawGyroZ; //Raw readings from MEMS sensor
uint16_t memsRawMagX, memsRawMagY, memsRawMagZ; //Raw readings from MEMS sensor
//These Q values are defined in the datasheet but can also be obtained by querying the meta data records
//See the read metadata example for more info
int16_t rotationVector_Q1 = 14;
int16_t rotationVectorAccuracy_Q1 = 12; //Heading accuracy estimate in radians. The Q point is 12.
int16_t accelerometer_Q1 = 8;
int16_t linear_accelerometer_Q1 = 8;
int16_t gyro_Q1 = 9;
int16_t magnetometer_Q1 = 4;
int16_t angular_velocity_Q1 = 10;
};

367
include/MPU6050OffsetFinder.cpp Normal file
View File

@@ -0,0 +1,367 @@
// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW
// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net)
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2019-07-11 - added PID offset generation at begninning Generates first offsets
// - in @ 6 seconds and completes with 4 more sets @ 10 seconds
// - then continues with origional 2016 calibration code.
// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz
// added temporizing printing during long computations
// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion
// dynamic speed change when closing in
// 2016-10-22 - cosmetic changes
// 2016-10-19 - initial release of IMU_Zero
// 2013-05-08 - added multiple output formats
// - added seamless Fastwire support
// 2011-10-07 - initial release of MPU6050_RAW
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2011 Jeff Rowberg
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.
If an MPU6050
* is an ideal member of its tribe,
* is properly warmed up,
* is at rest in a neutral position,
* is in a location where the pull of gravity is exactly 1g, and
* has been loaded with the best possible offsets,
then it will report 0 for all accelerations and displacements, except for
Z acceleration, for which it will report 16384 (that is, 2^14). Your device
probably won't do quite this well, but good offsets will all get the baseline
outputs close to these target values.
Put the MPU6050 on a flat and horizontal surface, and leave it operating for
5-10 minutes so its temperature gets stabilized.
Run this program. A "----- done -----" line will indicate that it has done its best.
With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take
a few minutes to get there.
Along the way, it will generate a dozen or so lines of output, showing that for each
of the 6 desired offsets, it is
* first, trying to find two estimates, one too low and one too high, and
* then, closing in until the bracket can't be made smaller.
The line just above the "done" line will look something like
[567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4]
As will have been shown in interspersed header lines, the six groups making up this
line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration,
X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed
that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration,
and so on.
The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer.
===============================================
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU9250.h"
#include "motionbase.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
const char LBRACKET = '[';
const char RBRACKET = ']';
const char COMMA = ',';
const char BLANK = ' ';
const char PERIOD = '.';
const int iAx = 0;
const int iAy = 1;
const int iAz = 2;
const int iGx = 3;
const int iGy = 4;
const int iGz = 5;
const int usDelay = 3150; // empirical, to hold sampling to 200 Hz
const int NFast = 1000; // the bigger, the better (but slower)
const int NSlow = 10000; // ..
const int LinesBetweenHeaders = 5;
int LowValue[6];
int HighValue[6];
int Smoothed[6];
int LowOffset[6];
int HighOffset[6];
int Target[6];
int LinesOut;
int N;
void ForceHeader()
{
LinesOut = 99;
}
void GetSmoothed()
{
int16_t RawValue[6];
int i;
long Sums[6];
for (i = iAx; i <= iGz; i++)
{
Sums[i] = 0;
}
// unsigned long Start = micros();
for (i = 1; i <= N; i++)
{ // get sums
accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz],
&RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
if ((i % 500) == 0)
Serial.print(PERIOD);
delayMicroseconds(usDelay);
for (int j = iAx; j <= iGz; j++)
Sums[j] = Sums[j] + RawValue[j];
} // get sums
// unsigned long usForN = micros() - Start;
// Serial.print(" reading at ");
// Serial.print(1000000/((usForN+N/2)/N));
// Serial.println(" Hz");
for (i = iAx; i <= iGz; i++)
{
Smoothed[i] = (Sums[i] + N / 2) / N;
}
} // GetSmoothed
void Initialize()
{
Serial.println("PID tuning Each Dot = 100 readings");
/*A tidbit on how PID (PI actually) tuning works.
When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and
integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral
uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it
to the integral value. Each reading narrows the error down to the desired offset. The greater the error from
set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the
integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the
noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100
readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to
the fact it reacts to any noise.
*/
accelgyro.CalibrateAccel(6);
accelgyro.CalibrateGyro(6);
Serial.println("\nat 600 Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("700 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("800 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("900 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println();
accelgyro.CalibrateAccel(1);
accelgyro.CalibrateGyro(1);
Serial.println("1000 Total Readings");
accelgyro.PrintActiveOffsets();
Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:");
} // Initialize
void SetOffsets(int TheOffsets[6])
{
accelgyro.setXAccelOffset(TheOffsets[iAx]);
accelgyro.setYAccelOffset(TheOffsets[iAy]);
accelgyro.setZAccelOffset(TheOffsets[iAz]);
accelgyro.setXGyroOffset(TheOffsets[iGx]);
accelgyro.setYGyroOffset(TheOffsets[iGy]);
accelgyro.setZGyroOffset(TheOffsets[iGz]);
} // SetOffsets
void ShowProgress()
{
if (LinesOut >= LinesBetweenHeaders)
{ // show header
Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro");
LinesOut = 0;
} // show header
Serial.print(BLANK);
for (int i = iAx; i <= iGz; i++)
{
Serial.print(LBRACKET);
Serial.print(LowOffset[i]),
Serial.print(COMMA);
Serial.print(HighOffset[i]);
Serial.print("] --> [");
Serial.print(LowValue[i]);
Serial.print(COMMA);
Serial.print(HighValue[i]);
if (i == iGz)
{
Serial.println(RBRACKET);
}
else
{
Serial.print("]\t");
}
}
LinesOut++;
} // ShowProgress
void SetAveraging(int NewN)
{
N = NewN;
Serial.print("averaging ");
Serial.print(N);
Serial.println(" readings each time");
} // SetAveraging
void PullBracketsIn()
{
boolean AllBracketsNarrow;
boolean StillWorking;
int NewOffset[6];
Serial.println("\nclosing in:");
AllBracketsNarrow = false;
ForceHeader();
StillWorking = true;
while (StillWorking)
{
StillWorking = false;
if (AllBracketsNarrow && (N == NFast))
{
SetAveraging(NSlow);
}
else
{
AllBracketsNarrow = true;
} // tentative
for (int i = iAx; i <= iGz; i++)
{
if (HighOffset[i] <= (LowOffset[i] + 1))
{
NewOffset[i] = LowOffset[i];
}
else
{ // binary search
StillWorking = true;
NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
if (HighOffset[i] > (LowOffset[i] + 10))
{
AllBracketsNarrow = false;
}
} // binary search
}
SetOffsets(NewOffset);
GetSmoothed();
for (int i = iAx; i <= iGz; i++)
{ // closing in
if (Smoothed[i] > Target[i])
{ // use lower half
HighOffset[i] = NewOffset[i];
HighValue[i] = Smoothed[i];
} // use lower half
else
{ // use upper half
LowOffset[i] = NewOffset[i];
LowValue[i] = Smoothed[i];
} // use upper half
} // closing in
ShowProgress();
} // still working
} // PullBracketsIn
void PullBracketsOut()
{
boolean Done = false;
int NextLowOffset[6];
int NextHighOffset[6];
Serial.println("expanding:");
ForceHeader();
while (!Done)
{
Done = true;
SetOffsets(LowOffset);
GetSmoothed();
for (int i = iAx; i <= iGz; i++)
{ // got low values
LowValue[i] = Smoothed[i];
if (LowValue[i] >= Target[i])
{
Done = false;
NextLowOffset[i] = LowOffset[i] - 1000;
}
else
{
NextLowOffset[i] = LowOffset[i];
}
} // got low values
SetOffsets(HighOffset);
GetSmoothed();
for (int i = iAx; i <= iGz; i++)
{ // got high values
HighValue[i] = Smoothed[i];
if (HighValue[i] <= Target[i])
{
Done = false;
NextHighOffset[i] = HighOffset[i] + 1000;
}
else
{
NextHighOffset[i] = HighOffset[i];
}
} // got high values
ShowProgress();
for (int i = iAx; i <= iGz; i++)
{
LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done
HighOffset[i] = NextHighOffset[i]; // ..
}
} // keep going
} // PullBracketsOut
void findOffset()
{
Initialize();
for (int i = iAx; i <= iGz; i++)
{ // set targets and initial guesses
Target[i] = 0; // must fix for ZAccel
HighOffset[i] = 0;
LowOffset[i] = 0;
} // set targets and initial guesses
Target[iAz] = 16384;
SetAveraging(NFast);
PullBracketsOut();
PullBracketsIn();
Serial.println("-------------- done --------------");
} // setup

82
include/bno085.cpp Normal file
View File

@@ -0,0 +1,82 @@
#include "motionbase.h"
#include "BNO080.cpp"
BNO080 imu;
void motionSetup()
{
delay(500);
if(!imu.begin(BNO080_DEFAULT_ADDRESS, Wire)) {
Serial.println("Can't connect to BNO08X");
for(int i = 0; i < 500; ++i) {
delay(50);
digitalWrite(LOADING_LED, LOW);
delay(50);
digitalWrite(LOADING_LED, HIGH);
}
}
Serial.println("Connected to BNO08X");
Wire.setClock(400000);
imu.enableARVRStabilizedRotationVector(30);
}
void motionLoop()
{
//Look for reports from the IMU
if (imu.dataAvailable() == true)
{
cq.x = imu.getQuatI();
cq.y = imu.getQuatJ();
cq.z = imu.getQuatK();
cq.w = imu.getQuatReal();
cq *= rotationQuat;
}
}
void sendData() {
sendQuat(&cq, PACKET_ROTATION);
}
void performCalibration() {
for(int i = 0; i < 10; ++i) {
digitalWrite(CALIBRATING_LED, LOW);
delay(20);
digitalWrite(CALIBRATING_LED, HIGH);
delay(20);
}
digitalWrite(CALIBRATING_LED, LOW);
delay(2000);
digitalWrite(CALIBRATING_LED, HIGH);
imu.calibrateGyro();
imu.requestCalibrationStatus(); // use this
while(!imu.calibrationComplete()) {
digitalWrite(CALIBRATING_LED, LOW);
delay(20);
digitalWrite(CALIBRATING_LED, HIGH);
delay(20);
imu.getReadings();
}
digitalWrite(CALIBRATING_LED, LOW);
delay(2000);
digitalWrite(CALIBRATING_LED, HIGH);
imu.calibrateAccelerometer();
while(!imu.calibrationComplete()) {
digitalWrite(CALIBRATING_LED, LOW);
delay(20);
digitalWrite(CALIBRATING_LED, HIGH);
delay(20);
imu.getReadings();
}
digitalWrite(CALIBRATING_LED, LOW);
delay(2000);
digitalWrite(CALIBRATING_LED, HIGH);
imu.calibrateMagnetometer();
while(!imu.calibrationComplete()) {
digitalWrite(CALIBRATING_LED, LOW);
delay(20);
digitalWrite(CALIBRATING_LED, HIGH);
delay(20);
imu.getReadings();
}
imu.saveCalibration();
}

7
include/motionbase.cpp → include/motionbase.h
View File

@@ -1,3 +1,6 @@
#ifndef _MOTIONBASE_H_
#define _MOTIONBASE_H_
#include "I2Cdev.cpp"
#include "MPU9250MotionApps.h"
#include "defines.h"
@@ -20,4 +23,6 @@ void motionSetup();
void motionLoop();
void sendData();
void sendData();
#endif

27
include/udpclient.cpp
View File

@@ -20,6 +20,9 @@ bool connected = false;
unsigned long lastConnectionAttemptMs;
unsigned long lastPacketMs;
uint8_t serialBuffer[128];
size_t serialLength = 0;
template <typename T>
unsigned char * convert_to_chars(T src, unsigned char * target)
{
@@ -206,6 +209,26 @@ void sendRawCalibrationData(int *const data, int type)
}
}
void sendSerial(uint8_t *const data, int length, int type) {
if (Udp.beginPacket(host, port) > 0)
{
sendType(type);
sendPacketNumber();
Udp.write(convert_to_chars(length, buf), sizeof(length));
Udp.write(data, length);
if (Udp.endPacket() == 0)
{
//Serial.print("Write error: ");
//Serial.println(Udp.getWriteError());
}
}
else
{
//Serial.print("Write error: ");
//Serial.println(Udp.getWriteError());
}
}
void returnLastPacket(int len) {
if (Udp.beginPacket(host, port) > 0)
{
@@ -290,6 +313,10 @@ void clientUpdate()
break;
}
}
//while(Serial.available()) {
// size_t bytesRead = Serial.readBytes(serialBuffer, min(Serial.available(), sizeof(serialBuffer)));
// sendSerial(serialBuffer, bytesRead, PACKET_SERIAL);
//}
if(lastPacketMs + TIMEOUT < millis())
{
connected = false;

1
include/udpclient.h
View File

@@ -17,6 +17,7 @@
#define PACKET_CONFIG 8
#define PACKET_RAW_MAGENTOMETER 9
#define PACKET_PING_PONG 10
#define PACKET_SERIAL 11
#define PACKET_RECIEVE_HEARTBEAT 1
#define PACKET_RECIEVE_VIBRATE 2

6
src/main.cpp
View File

@@ -20,7 +20,7 @@
#include "Wire.h"
#include "ota.cpp"
#include "6axismotion.cpp"
#include "bno085.cpp"
bool isCalibrating = false;
bool blinking = false;
@@ -61,7 +61,9 @@ void setup()
digitalWrite(LOADING_LED, LOW);
// join I2C bus (I2Cdev library doesn't do this automatically)
Wire.begin(D6, D5);
Wire.flush();
Wire.begin(D2, D1);
Wire.setClockStretchLimit(4000);
Serial.begin(serialBaudRate);
while (!Serial)
; // wait for connection