-
-
Save thesummer/1d46580527424d63aa8c to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
}; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Wait... Just figured it out: you posted your alterations to David's code. Now I get it... Sorry.