Skip to content

Commit

Permalink
Minor firmware refactoring (#250)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Eirenliel
Eirenliel authored Jul 2, 2023
1 parent 48d87a3 commit fe6c253
Show file tree
Hide file tree
Showing 16 changed files with 141 additions and 297 deletions.
61 changes: 14 additions & 47 deletions lib/bno080/BNO080.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1270,39 +1270,9 @@ void BNO080::enableActivityClassifier(uint16_t timeBetweenReports, uint32_t acti
setFeatureCommand(SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER, timeBetweenReports, activitiesToEnable);
}

//Sends the commands to begin calibration of the accelerometer
void BNO080::calibrateAccelerometer()
{
sendCalibrateCommand(CALIBRATE_ACCEL);
}

//Sends the commands to begin calibration of the gyro
void BNO080::calibrateGyro()
{
sendCalibrateCommand(CALIBRATE_GYRO);
}

//Sends the commands to begin calibration of the magnetometer
void BNO080::calibrateMagnetometer()
{
sendCalibrateCommand(CALIBRATE_MAG);
}

//Sends the commands to begin calibration of the planar accelerometer
void BNO080::calibratePlanarAccelerometer()
{
sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL);
}

//See 2.2 of the Calibration Procedure document 1000-4044
void BNO080::calibrateAll()
{
sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
}

void BNO080::endCalibration()
{
sendCalibrateCommand(CALIBRATE_STOP); //Disables all calibrations
sendCalibrateCommand(0); //Disables all calibrations
}

//See page 51 of reference manual - ME Calibration Response
Expand Down Expand Up @@ -1374,37 +1344,34 @@ void BNO080::sendCommand(uint8_t command)

//This tells the BNO080 to begin calibrating
//See page 50 of reference manual and the 1000-4044 calibration doc
//The argument is a set of binary flags see SH2_CAL_ACCEL
void BNO080::sendCalibrateCommand(uint8_t thingToCalibrate)
{
/*shtpData[3] = 0; //P0 - Accel Cal Enable
shtpData[4] = 0; //P1 - Gyro Cal Enable
/*
shtpData[3] = 0; //P0 - Accel Cal Enable
shtpData[4] = 0; //P1 - Gyro In-Hand Cal Enable
shtpData[5] = 0; //P2 - Mag Cal Enable
shtpData[6] = 0; //P3 - Subcommand 0x00
shtpData[7] = 0; //P4 - Planar Accel Cal Enable
shtpData[8] = 0; //P5 - Reserved
shtpData[8] = 0; //P5 - Gyro On-Table Cal Enable
shtpData[9] = 0; //P6 - Reserved
shtpData[10] = 0; //P7 - Reserved
shtpData[11] = 0; //P8 - Reserved*/
shtpData[11] = 0; //P8 - Reserved
*/

for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array
shtpData[x] = 0;

if (thingToCalibrate == CALIBRATE_ACCEL)
if ((thingToCalibrate & SH2_CAL_ACCEL) > 1)
shtpData[3] = 1;
else if (thingToCalibrate == CALIBRATE_GYRO)
if ((thingToCalibrate & SH2_CAL_GYRO_IN_HAND) > 1)
shtpData[4] = 1;
else if (thingToCalibrate == CALIBRATE_MAG)
if ((thingToCalibrate & SH2_CAL_MAG) > 1)
shtpData[5] = 1;
else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
if ((thingToCalibrate & SH2_CAL_PLANAR) > 1)
shtpData[7] = 1;
else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG)
{
shtpData[3] = 1;
shtpData[4] = 1;
shtpData[5] = 1;
}
else if (thingToCalibrate == CALIBRATE_STOP)
; //Do nothing, bytes are set to zero
if ((thingToCalibrate & SH2_CAL_ON_TABLE) > 1)
shtpData[8] = 1;

//Make the internal calStatus variable non-zero (operation failed) so that user can test while we wait
calibrationStatus = 1;
Expand Down
18 changes: 6 additions & 12 deletions lib/bno080/BNO080.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,12 +129,11 @@ const byte CHANNEL_GYRO = 5;
#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 SH2_CAL_ACCEL (0x01)
#define SH2_CAL_GYRO_IN_HAND (0x02)
#define SH2_CAL_MAG (0x04)
#define SH2_CAL_PLANAR (0x08)
#define SH2_CAL_ON_TABLE (0x10)

#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)
Expand Down Expand Up @@ -238,12 +237,7 @@ class BNO080
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
Expand Down
2 changes: 2 additions & 0 deletions src/GlobalVars.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,12 @@
#include "status/StatusManager.h"
#include "configuration/Configuration.h"
#include "sensors/SensorManager.h"
#include <arduino-timer.h>

extern SlimeVR::LEDManager ledManager;
extern SlimeVR::Status::StatusManager statusManager;
extern SlimeVR::Configuration::Configuration configuration;
extern SlimeVR::Sensors::SensorManager sensorManager;
extern Timer<> globalTimer;

#endif
8 changes: 4 additions & 4 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,15 +23,12 @@

#include "Wire.h"
#include "ota.h"
#include "sensors/SensorManager.h"
#include "configuration/Configuration.h"
#include "GlobalVars.h"
#include "network/network.h"
#include "globals.h"
#include "credentials.h"
#include <i2cscan.h>
#include "serial/serialcommands.h"
#include "LEDManager.h"
#include "status/StatusManager.h"
#include "batterymonitor.h"
#include "logging/Logger.h"

Expand All @@ -40,6 +37,7 @@ SlimeVR::Sensors::SensorManager sensorManager;
SlimeVR::LEDManager ledManager(LED_PIN);
SlimeVR::Status::StatusManager statusManager;
SlimeVR::Configuration::Configuration configuration;
Timer<> globalTimer;

int sensorToCalibrate = -1;
bool blinking = false;
Expand All @@ -52,6 +50,7 @@ BatteryMonitor battery;
void setup()
{
Serial.begin(serialBaudRate);
globalTimer = timer_create_default();

#ifdef ESP32C3
// Wait for the Computer to be able to connect.
Expand Down Expand Up @@ -111,6 +110,7 @@ void setup()

void loop()
{
globalTimer.tick();
SerialCommands::update();
OTA::otaUpdate();
Network::update(sensorManager.getFirst(), sensorManager.getSecond());
Expand Down
4 changes: 0 additions & 4 deletions src/network/packets.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,10 +105,6 @@ namespace Network {
#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
}

Expand Down
64 changes: 0 additions & 64 deletions src/network/udpclient.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -479,70 +479,6 @@ void Network::sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, flo
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) {
Expand Down
25 changes: 10 additions & 15 deletions src/sensors/bmi160sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -400,9 +400,9 @@ void BMI160Sensor::motionLoop() {
return;
}

quaternion.set(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
fusedRotation.set(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);

const Quat q = quaternion;
const Quat q = fusedRotation;
sensor_real_t vecGravity[3];
vecGravity[0] = 2 * (q.x * q.z - q.w * q.y);
vecGravity[1] = 2 * (q.w * q.x + q.y * q.z);
Expand All @@ -413,22 +413,17 @@ void BMI160Sensor::motionLoop() {
linAccel.y = lastAxyz[1] - vecGravity[1] * CONST_EARTH_GRAVITY;
linAccel.z = lastAxyz[2] - vecGravity[2] * CONST_EARTH_GRAVITY;

linearAcceleration[0] = linAccel.x;
linearAcceleration[1] = linAccel.y;
linearAcceleration[2] = linAccel.z;
acceleration[0] = linAccel.x;
acceleration[1] = linAccel.y;
acceleration[2] = linAccel.z;
newAcceleration = true;

quaternion *= sensorOffset;
fusedRotation *= sensorOffset;

#if ENABLE_INSPECTION
if (!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation))
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif

if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
{
newData = true;
lastQuatSent = quaternion;
newFusedRotation = true;
lastFusedRotationSent = fusedRotation;
}

optimistic_yield(100);
Expand Down
23 changes: 9 additions & 14 deletions src/sensors/bno055sensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,27 +62,22 @@ void BNO055Sensor::motionLoop() {

// TODO Optimize a bit with setting rawQuat directly
Quat quat = imu.getQuat();
quaternion.set(quat.x, quat.y, quat.z, quat.w);
quaternion *= sensorOffset;
fusedRotation.set(quat.x, quat.y, quat.z, quat.w);
fusedRotation *= sensorOffset;

#if SEND_ACCELERATION
{
Vector3 accel = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
this->linearAcceleration[0] = accel.x;
this->linearAcceleration[1] = accel.y;
this->linearAcceleration[2] = accel.z;
this->acceleration[0] = accel.x;
this->acceleration[1] = accel.y;
this->acceleration[2] = accel.z;
this->newAcceleration = true;
}
#endif

#if ENABLE_INSPECTION
{
Network::sendInspectionFusedIMUData(sensorId, quaternion);
}
#endif

if(!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) {
newData = true;
lastQuatSent = quaternion;
if(!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) {
newFusedRotation = true;
lastFusedRotationSent = fusedRotation;
}
}

Expand Down
Loading

0 comments on commit fe6c253

Please sign in to comment.