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

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