Skip to content
Snippets Groups Projects
Commit 71691ac9 authored by Riccardo Musso's avatar Riccardo Musso
Browse files

Clang-formatted the code

parent 444181d4
Branches
Tags
No related merge requests found
...@@ -27,52 +27,48 @@ ...@@ -27,52 +27,48 @@
/** /**
* This is the dumbest type of calibration possible: it stores a 3d vector * This is the dumbest type of calibration possible: it stores a 3d vector
* (called "bias") that will be added to every measurement. * (called "bias") that will be added to every measurement.
* During the calibration phase it will use a given reference vector (for example the gravitational * During the calibration phase it will use a given reference vector (for
* acceleration for the accelerometer), and every time you'll feed the model with a new * example the gravitational acceleration for the accelerometer), and every time
* value, you have to give it the orientation of the sensor, so it can guess the bias. * you'll feed the model with a new value, you have to give it the orientation
* of the sensor, so it can guess the bias.
*/ */
template <typename SensorData> template <typename SensorData>
class BiasCalibration : public AbstractCalibrationModel<Vec3, SensorData, Vec3, AxisOrthoOrientation> class BiasCalibration : public AbstractCalibrationModel<Vec3, SensorData, Vec3,
AxisOrthoOrientation>
{ {
public: public:
BiasCalibration() BiasCalibration() : bias(0.f, 0.f, 0.f), ref(0.f, 0.f, 1.f) {}
: bias(0.f, 0.f, 0.f), ref(0.f, 0.f, 1.f) {}
void setReferenceVector(Vec3 vec){ void setReferenceVector(Vec3 vec) { ref = vec; }
ref = vec;
}
void store(Vec3& out) const override { void store(Vec3& out) const override { out = bias; }
out = bias;
}
void load(const Vec3& in) override { void load(const Vec3& in) override { bias = in; }
bias = in;
}
void resetToIdentity() override{ void resetToIdentity() override { bias.clear(); }
bias.clear();
}
void startCalibrationStage() override{ void startCalibrationStage() override
{
specificInit(); specificInit();
sum.clear(); sum.clear();
numSamples = 0; numSamples = 0;
} }
void feed(const Vec3& measured, const AxisOrthoOrientation& ortho) override { void feed(const Vec3& measured, const AxisOrthoOrientation& ortho) override
{
Mat3 mat; Mat3 mat;
ortho.getMatrix().setTranspose(mat); ortho.getMatrix().setTranspose(mat);
sum += (mat * ref) - measured; sum += (mat * ref) - measured;
numSamples++; numSamples++;
} }
void endCalibrationStage() override { void endCalibrationStage() override { bias = sum / numSamples; }
bias = sum / numSamples;
}
SensorData correct(const SensorData& data) const override { SensorData correct(const SensorData& data) const override
static_assert(sizeof(SensorData) != sizeof(SensorData), "BiasCalibration still doesn't support the given SensorData data type."); {
static_assert(sizeof(SensorData) != sizeof(SensorData),
"BiasCalibration still doesn't support the given "
"SensorData data type.");
} }
private: private:
...@@ -83,12 +79,15 @@ private: ...@@ -83,12 +79,15 @@ private:
}; };
template <> template <>
void BiasCalibration<AccelerometerData>::specificInit(){ void BiasCalibration<AccelerometerData>::specificInit()
{
setReferenceVector({0.f, 0.f, -1.f}); setReferenceVector({0.f, 0.f, -1.f});
} }
template <> template <>
AccelerometerData BiasCalibration<AccelerometerData>::correct(const AccelerometerData& input) const { AccelerometerData BiasCalibration<AccelerometerData>::correct(
const AccelerometerData& input) const
{
AccelerometerData output; AccelerometerData output;
output.accel_x = bias.getX() + input.accel_x; output.accel_x = bias.getX() + input.accel_x;
output.accel_y = bias.getY() + input.accel_y; output.accel_y = bias.getY() + input.accel_y;
......
...@@ -22,8 +22,8 @@ ...@@ -22,8 +22,8 @@
#pragma once #pragma once
#include "math/Vec3.h"
#include "math/Matrix.h" #include "math/Matrix.h"
#include "math/Vec3.h"
/** /**
* This class generalizes all the Calibration classes used to * This class generalizes all the Calibration classes used to
...@@ -76,8 +76,10 @@ enum class Orientation ...@@ -76,8 +76,10 @@ enum class Orientation
SOUTH SOUTH
}; };
Vec3 orientationToVector(Orientation val){ Vec3 orientationToVector(Orientation val)
switch(val){ {
switch (val)
{
case Orientation::UP: case Orientation::UP:
return Vec3(0.f, 0.f, 1.f); return Vec3(0.f, 0.f, 1.f);
case Orientation::DOWN: case Orientation::DOWN:
...@@ -100,7 +102,8 @@ struct AxisOrthoOrientation ...@@ -100,7 +102,8 @@ struct AxisOrthoOrientation
{ {
Orientation x = Orientation::SOUTH, z = Orientation::UP; Orientation x = Orientation::SOUTH, z = Orientation::UP;
Mat3 getMatrix() const { Mat3 getMatrix() const
{
Vec3 vx, vy, vz; Vec3 vx, vy, vz;
vx = orientationToVector(x); vx = orientationToVector(x);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment