Skip to content

Instantly share code, notes, and snippets.

@thesummer

thesummer/IMU.h Secret

Created June 20, 2013 02:21
Show Gist options
  • Save thesummer/1d46580527424d63aa8c to your computer and use it in GitHub Desktop.
Save thesummer/1d46580527424d63aa8c to your computer and use it in GitHub Desktop.
class IMU {
public:
// Scaled readings
virtual vector readMag() = 0; // In body coords, scaled to -1..1 range
virtual vector readAcc() = 0; // In body coords, with units = g
virtual vector readGyro() = 0; // In body coords, with units = rad/sec
void read(){ readAcc(); readMag(); readGyro(); }
virtual void measureOffsets() = 0;
virtual void enable() = 0;
virtual void loadCalibration() = 0;
vector gyro_offset;
matrix calMatrix;
vector calOffset;
int_vector raw_m, raw_a, raw_g;
};
void MinIMU9::loadCalibration()
{
wordexp_t expansion_result;
wordexp("~/.ahrs-cal", &expansion_result, 0);
std::ifstream file(expansion_result.we_wordv[0]);
if (file.fail())
{
throw posix_error("Failed to open calibration file ~/.ahrs-cal.");
}
file >> calMatrix(0,0) >> calMatrix(0,1) >> calMatrix(0,2);
file >> calMatrix(1,0) >> calMatrix(1,1) >> calMatrix(1,2);
file >> calMatrix(2,0) >> calMatrix(2,1) >> calMatrix(2,2);
file >> calOffset(0) >> calOffset(1) >> calOffset(2);
if (file.fail() || file.bad())
{
throw std::runtime_error("Failed to parse calibration file ~/.minimu9-ahrs-cal.");
}
}
vector MinIMU9::readMag()
{
compass.readMag();
IMU::raw_m = int_vector_from_ints(&compass.m);
vector v;
v = IMU::raw_m.cast<float>() - calOffset;
v = calMatrix * v;
return v;
}
@ssokol
Copy link

ssokol commented Jul 2, 2013

Finally getting a chance to look into this. Warning: I'm far from proficient in C++ and never took a matrix algebra class so please forgive my ignorance.

Line 4 appears to be looking for a file called ".ahrs-cal" in the user's home directory. However, the calibration utility generates a file called ".minimu9-ahrs-cal". Is this an error or am I missing some clever C++ thing here?

David's calibrator results in a minimu9-ahrs-cal file that contains the following:

-578 687 -760 497 -676 417

Your calibration utility created a file that contains:

2.667792712353045902e-03 3.000263573718869696e-05 5.212993673986833325e-06
3.000263573718869696e-05 2.489844522975613074e-03 -7.086119139315628576e-05
5.212993673986833325e-06 -7.086119139315628576e-05 2.458301739549333641e-03

How do these items correlate?

Again, apologies for my mathematical ignorance.

Cheers,

-S

@ssokol
Copy link

ssokol commented Jul 2, 2013

Wait... Just figured it out: you posted your alterations to David's code. Now I get it... Sorry.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment