I currently have a robotics project which is using many (16) IMU's specifically the MPU9250 running under SPI.
As a reduced example of six sensors using the Bolder flight library
int cs[6] = {21, 25, 26, 27, 32, 14}; //chipselects
MPU9250 IMU0(SPI, 21); // Header P5
MPU9250 IMU1(SPI, 25); // Header P6
MPU9250 IMU2(SPI, 26); // Header P7
MPU9250 IMU3(SPI, 27); // Header P9
MPU9250 IMU4(SPI, 32); // Header P10
MPU9250 IMU5(SPI, 12); // Header P11
To use these sensors they all have to be calibrated and have magnetic hard and soft offsets applied to them live during use, on top of that, I also have to apply gyroscopic and accel. calibration algorithms. Which means, for each sensor, I have to call 9 different data points from each IMU and apply some maths, so I set up some arrays for storing in between values and final values and offsets:
// Offsets applied to raw x/y/z mag values
float mag_offsets[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 10.44F, 34.76F, -49.86F },
{ 8.62F, 20.41F, -12.65F },
{ -3.05F, 19.75F, -8.55F },
};
// Soft iron error compensation matrix
float mag_softiron_matrix[6][3][3] = {
// IMUs 27, 14, 32
{{ 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }},
{{ 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }},
{{ 0, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 }},
// IMUs, 21, 25, 26
{{ 1.036F, 0.017F, -0.001F }, { 0.017F, 0.954F, -0.028F }, { -0.001F, 0.028F, 1.013F }},
{{ 1.031F, 0.013F, -0.024F }, { 0.013F, 0.897F, 0.054F }, { -0.024F, 0.054F, 1.085F }},
{{ 1.057F, 0.034F, 0.017F }, { 0.034F, 0.967F, 0.038F }, { 0.017F, 0.038F, 0.981F }},
};
float mag_field_strength[3] = {38.52F, 37.24F , 38.58F };
// Offsets applied to compensate for gyro zero-drift error for x/y/z, sensor dependent
float gyro_zero_offsets[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
// Used for calculating 'in between values' prior to passing to final mag array, sensor dependent
float deltamag[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
// Following array names should always be constant and final values to be given to Magdwick filters, sensor agnostic.
float gyro[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
float accel[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
float mag[6][3] = {
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
{ 0.0F, 0.0F, 0.0F },
};
Then in the loop itself I call each object and get the sensors readings:
void loop(){
IMU0.readSensor();
IMU1.readSensor();
IMU2.readSensor();
IMU3.readSensor();
IMU4.readSensor();
IMU5.readSensor();
// update accel, gyro, mag arrays
float getAccel[6][3] = {
{ IMU0.getAccelX_mss(), IMU0.getAccelY_mss(), IMU0.getAccelZ_mss() },
{ IMU1.getAccelX_mss(), IMU1.getAccelY_mss(), IMU1.getAccelZ_mss() },
{ IMU2.getAccelX_mss(), IMU2.getAccelY_mss(), IMU2.getAccelZ_mss() },
{ IMU3.getAccelX_mss(), IMU3.getAccelY_mss(), IMU3.getAccelZ_mss() },
{ IMU4.getAccelX_mss(), IMU4.getAccelY_mss(), IMU4.getAccelZ_mss() },
{ IMU5.getAccelX_mss(), IMU5.getAccelY_mss(), IMU5.getAccelZ_mss() },
};
float getGyro[6][3] = {
{ IMU0.getGyroX_rads(), IMU0.getGyroY_rads(), IMU0.getGyroZ_rads() },
{ IMU1.getGyroX_rads(), IMU1.getGyroY_rads(), IMU1.getGyroZ_rads() },
{ IMU2.getGyroX_rads(), IMU2.getGyroY_rads(), IMU2.getGyroZ_rads() },
{ IMU3.getGyroX_rads(), IMU3.getGyroY_rads(), IMU3.getGyroZ_rads() },
{ IMU4.getGyroX_rads(), IMU4.getGyroY_rads(), IMU4.getGyroZ_rads() },
{ IMU5.getGyroX_rads(), IMU5.getGyroY_rads(), IMU5.getGyroZ_rads() },
};
float getMag[6][3] = {
{ IMU0.getMagX_uT(), IMU0.getMagY_uT(), IMU0.getMagZ_uT() },
{ IMU1.getMagX_uT(), IMU1.getMagY_uT(), IMU1.getMagZ_uT() },
{ IMU2.getMagX_uT(), IMU2.getMagY_uT(), IMU2.getMagZ_uT() },
{ IMU3.getMagX_uT(), IMU3.getMagY_uT(), IMU3.getMagZ_uT() },
{ IMU4.getMagX_uT(), IMU4.getMagY_uT(), IMU4.getMagZ_uT() },
{ IMU5.getMagX_uT(), IMU5.getMagY_uT(), IMU5.getMagZ_uT() },
};
// Apply magnetic offsets
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 4; i++) {
deltamag[j][i] = getMag[j][i] - mag_offsets[i][j];
}
}
// Apply magnetic softiron offsets
for (int k = 0; k < 6; k++) {
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 4; i++) {
mag[j][i] = deltamag[j][0] * mag_softiron_matrix[k][0][0] + deltamag[j][1] * mag_softiron_matrix[k][0][1] + deltamag[j][2] * mag_softiron_matrix[k][0][2];
}
}
}
// Apply gyroscope offsets
for (int j = 0; j < 6; j++) {
for (int i = 0; i < 4; i++) {
gyro[j][i] = getGyro[j][i] - gyro_zero_offsets[j][i];
}
}
// Update Madgwick filters
filter0.update(gyro[0][0], gyro[0][1], gyro[0][2], accel[0][0], accel[0][1], accel[0][2], mag[0][0], mag[0][1], -1 * mag[0][2]);
filter1.update(gyro[1][0], gyro[1][1], gyro[1][2], accel[1][0], accel[1][1], accel[1][2], mag[1][0], mag[1][1], -1 * mag[1][2]);
filter2.update(gyro[2][0], gyro[2][1], gyro[2][2], accel[2][0], accel[2][1], accel[2][2], mag[2][0], mag[2][1], -1 * mag[2][2]);
filter3.update(gyro[3][0], gyro[3][1], gyro[3][2], accel[3][0], accel[3][1], accel[3][2], mag[3][0], mag[3][1], -1 * mag[3][2]);
filter4.update(gyro[4][0], gyro[4][1], gyro[4][2], accel[4][0], accel[4][1], accel[4][2], mag[4][0], mag[4][1], -1 * mag[4][2]);
filter5.update(gyro[5][0], gyro[5][1], gyro[5][2], accel[5][0], accel[5][1], accel[5][2], mag[5][0], mag[5][1], -1 * mag[5][2]);
// Call All Euler Angle Rotations around {X,Y,Z} or {gamma, delta, epsilon}
float eulerAngles[6][3] = {
{filter0.getRoll(), filter0.getPitch(), filter0.getYaw()},
{filter1.getRoll(), filter1.getPitch(), filter1.getYaw()},
{filter2.getRoll(), filter2.getPitch(), filter2.getYaw()},
{filter3.getRoll(), filter3.getPitch(), filter3.getYaw()},
{filter4.getRoll(), filter4.getPitch(), filter4.getYaw()},
{filter5.getRoll(), filter5.getPitch(), filter5.getYaw()},
};
Serial.print(eulerAngles[0][0]);
Serial.print(eulerAngles[0][1]);
Serial.print(eulerAngles[0][2]);
}
Though the code seems to work the way I expected it, I am confident this is the wrong method to store this data...namely in the getAccel, getGyro, getMag arrays, or to call them like in eulerAngles .
My hunch on this was during initial testing some sensors data that I receive are have an oscillating error being applied to them, which makes me think i'm receiving junk data from the memory somewhere
...I would have used a for loop, but since each object name is individual and do not have indices, I am unsure the best practice, nor the fastest way to call and work with such a large data set. I have found a similar question, though I'm unfortunately too dumb to apply it to my situation.
So the question is what is the proper method to call and store so many objects (and their data) in arrays for further calculations? I would like to avoid having over a hundred variables (when using all 16 IMUs and in-between variables to carry out all the appropriate maths. My apologies for probably terribly written code, my c++/Wiring is not the best.
Research object-oriented programming. Apply encapsulation. Group data depending on object, not similarities - just like you think about them.
Use standard library objects - std::array. Save memory, allow optimization - apply const whenever possible, use constexpr when possible. Research code guidelines and style guides - like https://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#S-philosophy and https://google.github.io/styleguide/cppguide.html .
Let's say in pseudocode, you could encapsulate all variables within one object and use member function to calculate the relevant stuff:
class MyStuff { // pick more meaningfull name
// maybe be more verbose
using axisvals = std::array<float, 3>;
private:
// apply constness to save RAM memory
static const std::array<float, 3> mag_field_strength = { 38.52F, 37.24F , 38.58F };
MPU9250 mpu;
FILTER filter;
const std::array<float, 3> mag_offsets;
const std::array<std::array<float, 3> , 6> mag_softiron_matrix;
std::array<float, 3> gyros{}; // maybe some internal state?
public:
MyStuff(int gpionum,
const std::array<float, 3>& mag_offsets,
const std::array<std::array<float, 3> , 6> mag_softiron_matrix) :
mpu{SPI, gpionum},
filter{some, params, for, filter, constructor},
mag_offsets{mag_offsets},
mag_softiron_matrix{mag_softiron_matrix} {
}
void setup() {
// do some setuping stuff
}
axisvals calculate_stuff() {
mpu.readSensor();
// use const as much as possible
const std::array<float, 3> guro = {
something * mpu.getGyroX_rads(),
something * mpu.getGyroY_rads(),
something * mpu.getGyroZ_rads(),
};
// ...
filter.update(
gyro[0], gyro[1], gyro[2],
accel[0], accel[1], accel[0][2],
mag[0], mag[1], -1 * mag[2]);
// ...
return {filter.getRoll(), filter.getPitch(), filter.getYaw()};
}
};
std::array<MyStuff, 6> imus = {
{ 21, {10.44F, 34.76F, -49.86F}, {{1.036F, 0.017F, -0.001F }, {...}, {...} }, // Header P5
{25, {....} {{...},{..}{...} }, // Header P6
// etc....
};
void setup() {
for (auto&& imu : imus) {
imu.setup();
}
}
void loop() {
for (auto&& imu : imus) {
const auto&vals = imu.calculate_stuff();
for (auto&& v : vals) {
Serial.print(v);
}
}
}
I'm trying to write a simple 3D soft engine, but I've got a little problem..
I have a class called Mesh which contains vertex and edge data:
struct Vertex { float x, y, z; };
struct Edge { int from, to; };
template <int vs, int es>
class Mesh {
public:
Vertex vertices[vs];
int vSize = vs;
Edge edges[es];
int eSize = es;
};
then a derived class called Cube which specifies the vertex and edge data for a cube (I will later on add more shapes of course):
class Cube : public Mesh<8, 12> {
public:
inline Cube() {
Vertex v[] = {
{ -1.0f, -1.0f, -1.0f },
{ 1.0f, -1.0f, -1.0f },
{ 1.0f, 1.0f, -1.0f },
{ -1.0f, 1.0f, -1.0f },
{ -1.0f, -1.0f, 1.0f },
{ 1.0f, -1.0f, 1.0f },
{ 1.0f, 1.0f, 1.0f },
{ -1.0f, 1.0f, 1.0f }
};
for (int i = 0; i < 8; i++)
this->vertices[i] = v[i];
Edge e[] = {
{ 0,1 },{ 1,2 },{ 2,3 },{ 3,0 },
{ 4,5 },{ 5,6 },{ 6,7 },{ 7,4 },
{ 0,4 },{ 1,5 },{ 2,6 },{ 3,7 }
};
for (int i = 0; i < 12; i++)
this->edges[i] = e[i];
}
};
And after that a class called Engine, which has an array of Mesh parent classes, which should be able to hold Cube and later Triangle etc..
template <int w, int h, int mSize>
class Engine {
private:
int width = w;
int height = h;
Mesh meshes[mSize]; <-- problem
int mCount = 0;
byte fBuffer[w][h];
byte bBuffer[w][h];
public:
inline Engine() {};
inline void addMesh(Mesh mesh) { this->meshes[this->mCount++] = mesh; }
};
which yields this error:
Engine.h: 19:3: error: invalid use of template-name 'Mesh' without an argument list
Mesh* meshes = new Mesh[m]
Engine.h: 25:23: error: 'Mesh' is not a type
inline void addMesh(Mesh mesh) { this->meshes[this->mCount++] = mesh; }
I know it's because the Mesh meshes[mSize]; should have Mesh<a, b> values but of course I don't know that for every possible Mesh.
What's a better way of storing these?
I suppose you could add a not-template base for Mesh, say mBase
struct mBase { };
template <std::size_t vs, std::size_t es>
struct Mesh : public mBase
{ };
and define meshes as an array of mBases
mBase meshes[mSize]; // <-- no more problem
void addMesh(mBase mesh) { this->meshes[this->mCount++] = mesh; }
You don't have to use templates for what you're trying to achieve here. So why don't declare your Mesh.vertices and Mesh.edges as std::vectors (for instance), and fill them as you construct your derived objects?
Like so:
#include <vector>
class Mesh {
public:
std::vector<Vertex> vertices;
std::vector<Edge> edges;
};
class Cube : public Mesh {
public:
Cube() {
// Following stuff is only allowed since c++11
// But there's other tricks for vector's initializations before c++11
this->vertices = {
{ -1.0f, -1.0f, -1.0f },
{ 1.0f, -1.0f, -1.0f },
{ 1.0f, 1.0f, -1.0f },
{ -1.0f, 1.0f, -1.0f },
{ -1.0f, -1.0f, 1.0f },
{ 1.0f, -1.0f, 1.0f },
{ 1.0f, 1.0f, 1.0f },
{ -1.0f, 1.0f, 1.0f }
};
this->edges = {
{ 0,1 },{ 1,2 },{ 2,3 },{ 3,0 },
{ 4,5 },{ 5,6 },{ 6,7 },{ 7,4 },
{ 0,4 },{ 1,5 },{ 2,6 },{ 3,7 }
};
}
};
Note that you don't need to store the size of these vectors, since you can get it with: std::vector<T>.size() (Mesh.edges.size())
Make sure to be familiar with the templated objects from the STL before creating your own ;)
(In facts, here your classes should be structs... But that is out of scope of the problem I guess...)
I'm doing something with DirectX 11 and came to a rectagle drawing (empty, non-colored), seemed simple for me at start (linelist_topology, 8 indices) but when I have it on the screen I see that my rectangle is kinda incomleted at left-top coordinate, there is a point of a background color there, the code is not complicated at all, vertices are 2D space:
SIMPLEVERTEX gvFrameVertices[4]=
{XMFLOAT3(0.0f,0.0f,1.0f),XMFLOAT2(0.0f, 0.0f),
XMFLOAT3(1.0f, 0.0f, 1.0f), XMFLOAT2(1.0f, 0.0f),
XMFLOAT3(1.0f, -1.0f, 1.0f), XMFLOAT2(1.0f, 1.0f),
XMFLOAT3(0.0f, -1.0f, 1.0f), XMFLOAT2(0.0f, 1.0f)};
indices:
WORD gvRectangularIndices[8] = { 0, 1, 1, 2, 2, 3, 3, 0 };
Shader just returns given color in constant buffer:
float4 PS_PANEL(PS_INPUT input) : SV_Target
{
return fontcolor;
}
Function code itself:
VOID rectangle(INT _left, INT _top, INT _width, INT _height, XMFLOAT4 _color)
{
XMMATRIX scale;
XMMATRIX translate;
XMMATRIX world;
scale = XMMatrixScaling( _width, _height, 1.0f );
translate = XMMatrixTranslation(_left, gvHeight - _top, 1.0f);
world = scale * translate;
gvConstantBufferData.world = XMMatrixTranspose(world);
gvConstantBufferData.index = 1.0f;
gvConstantBufferData.color = _color;
gvContext->PSSetShader(gvPanelPixelshader, NULL, 0);
gvContext->UpdateSubresource(gvConstantBuffer, 0, NULL, &gvConstantBufferData, 0, 0 );
gvContext->IASetIndexBuffer(gvLinelistIndexBuffer, DXGI_FORMAT_R16_UINT, 0);
gvContext->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_LINELIST );
gvContext->DrawIndexed(8, 0, 0);
gvContext->IASetIndexBuffer(gvTriangleslistIndexBuffer, DXGI_FORMAT_R16_UINT, 0);
gvContext->IASetPrimitiveTopology( D3D11_PRIMITIVE_TOPOLOGY_TRIANGLELIST );
};
gvHeight - _top - I'm using orthographic matrix for projection, so the coordinate center is at left-bottom, that why need to substract for proper Y coordinate.
gvOrthographicProjection = XMMatrixOrthographicOffCenterLH( 0.0f, gvWidth, 0.0f, gvHeight, 0.01f, 100.0f );
Do you have any idea what can cause this pointal incompletness of a rectangle in my case or I need to supply more code info (don't really want to link lines of the whole initializations cause they seem very obvious and simple for me, done for /at c++ and directx amateur level :)
Thank you:)