Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
Skyward Boardcore
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Analyze
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Avionics
Software Development
Skyward Boardcore
Commits
71691ac9
Commit
71691ac9
authored
4 years ago
by
Riccardo Musso
Browse files
Options
Downloads
Patches
Plain Diff
Clang-formatted the code
parent
444181d4
Branches
Branches containing commit
Tags
Tags containing commit
No related merge requests found
Changes
2
Show whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/shared/calibration/BiasCalibration.h
+31
-32
31 additions, 32 deletions
src/shared/calibration/BiasCalibration.h
src/shared/calibration/Calibration.h
+10
-7
10 additions, 7 deletions
src/shared/calibration/Calibration.h
with
41 additions
and
39 deletions
src/shared/calibration/BiasCalibration.h
+
31
−
32
View file @
71691ac9
...
@@ -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
;
...
...
This diff is collapsed.
Click to expand it.
src/shared/calibration/Calibration.h
+
10
−
7
View file @
71691ac9
...
@@ -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
);
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment