Skip to content

Commit

Permalink
Change magnetometer scale to match hmc5883 sensor.
Browse files Browse the repository at this point in the history
  • Loading branch information
@lovettchris
lovettchris committed Apr 6, 2017
1 parent 4be7128 commit e884603
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 15 deletions.
4 changes: 2 additions & 2 deletions AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,11 +17,11 @@ struct MagnetometerSimpleParams {
};

Vector3r noise_sigma = Vector3r(0.005f, 0.005f, 0.005f); //5 mgauss as per specs sheet (RMS is same as stddev) https://goo.gl/UOz6FT
real_T scale_factor = 2.0f;
real_T scale_factor = 1.0f;
Vector3r noise_bias = Vector3r(0.0f, 0.0f, 0.0f); //no offset as per specsheet (zero gauss level) https://goo.gl/UOz6FT
float ref_update_frequency = 0.2f; //Hz

//use dipole model is there is enough compute power available
//use dipole model if there is enough compute power available
bool dynamic_reference_source = true;
ReferenceSource ref_source = ReferenceSource::ReferenceSource_DipoleModel;
//bool dynamic_reference_source = false;
Expand Down
75 changes: 65 additions & 10 deletions Examples/StandAloneSensors.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,33 +112,38 @@ class StandALoneSensors {
}


static void generateMagnetometerDataLoc(std::ostream& output_stream, float period, float total_duration, GeoPoint loc)
static void generateMagnetometer2D(std::ostream& output_stream, float period, float total_duration, GeoPoint loc, float yawStart, bool ccw = false)
{
output_stream << std::fixed;

output_stream << "time\tx-mag\ty-mag\tz-mag\tlat\tlon\talt\tw\tx\ty\tz" << std::endl;
output_stream << "time\tx-mag\ty-mag\tz-mag" << std::endl;

float interations = total_duration / period;
double last = Utils::getTimeSinceEpoch();
for (float pitch = 0; pitch < 2.1*M_PIf; pitch += M_PIf/2) {
for (float roll = 0; roll < 2.1*M_PIf; roll += M_PIf/2) {
for (float yaw = 0; yaw < 2.1*M_PIf; yaw += M_PIf/2) {
for (float direction = 0; direction < 5; direction++) {

float yaw = yawStart;
float yawDelta = (direction * M_PIf / 2.0f);
if (ccw) {
yaw -= yawDelta;
}
else
{
yaw += yawDelta;
}

auto kinematics = Kinematics::State::zero();
kinematics.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
kinematics.pose.orientation = VectorMath::toQuaternion(0, 0, yaw);
msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0);
msr::airlib::Environment environment(initial_environment);
MagnetometerSimple mag;
mag.initialize(&kinematics, &environment);

for (auto i = 0; i < interations; ++i) {
const auto& output = mag.getOutput();
const auto& geo = environment.getState().geo_point;

output_stream << Utils::getTimeSinceEpoch() << "\t";
output_stream << output.magnetic_field_body.x() << "\t" << output.magnetic_field_body.y() << "\t" << output.magnetic_field_body.z();
output_stream << "\t" << geo.latitude << "\t" << geo.longitude << "\t" << geo.altitude;
output_stream << "\t" << kinematics.pose.orientation.w() << "\t" << kinematics.pose.orientation.x()<< "\t" << kinematics.pose.orientation.y() << "\t" << kinematics.pose.orientation.z();
output_stream << std::endl;

std::this_thread::sleep_for(std::chrono::duration<double>(period - (Utils::getTimeSinceEpoch() - last)));
Expand All @@ -147,8 +152,58 @@ class StandALoneSensors {
environment.update(dt);
mag.update(dt);
}
}}}
}
}



static void generateMagnetometer3D(std::ostream& output_stream, float period, float total_duration, GeoPoint loc, float yawStart = 0, bool ccw = false)
{
output_stream << std::fixed;

output_stream << "time\tx-mag\ty-mag\tz-mag\tlat\tlon\talt\tw\tx\ty\tz" << std::endl;

float interations = total_duration / period;
double last = Utils::getTimeSinceEpoch();
for (float pitch = 0; pitch < 2.1*M_PIf; pitch += M_PIf/2) {
for (float roll = 0; roll < 2.1*M_PIf; roll += M_PIf/2) {
for (float direction = 0; direction < 5; direction++) {

float yaw = yawStart;
float yawDelta = (direction * M_PIf / 2.0f);
if (ccw) {
yaw -= yawDelta;
}
else
{
yaw += yawDelta;
}

auto kinematics = Kinematics::State::zero();
kinematics.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
msr::airlib::Environment::State initial_environment(kinematics.pose.position, loc, 0);
msr::airlib::Environment environment(initial_environment);
MagnetometerSimple mag;
mag.initialize(&kinematics, &environment);

for (auto i = 0; i < interations; ++i) {
const auto& output = mag.getOutput();
const auto& geo = environment.getState().geo_point;

output_stream << Utils::getTimeSinceEpoch() << "\t";
output_stream << output.magnetic_field_body.x() << "\t" << output.magnetic_field_body.y() << "\t" << output.magnetic_field_body.z();
output_stream << "\t" << geo.latitude << "\t" << geo.longitude << "\t" << geo.altitude;
output_stream << "\t" << kinematics.pose.orientation.w() << "\t" << kinematics.pose.orientation.x() << "\t" << kinematics.pose.orientation.y() << "\t" << kinematics.pose.orientation.z();
output_stream << std::endl;

std::this_thread::sleep_for(std::chrono::duration<double>(period - (Utils::getTimeSinceEpoch() - last)));
float dt = static_cast<float>(Utils::getTimeSinceEpoch() - last);
last = Utils::getTimeSinceEpoch();
environment.update(dt);
mag.update(dt);
}
}}}
}
};


Expand Down
9 changes: 6 additions & 3 deletions Examples/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,11 +23,14 @@ int main(int argc, const char *argv[])

using namespace msr::airlib;

GeoPoint testLocation(47.763160, -122.068534, 120.6f);
//GeoPoint testLocation(47.763160, -122.068534, 120.6f);

GeoPoint testLocation(47.7037051477, -122.1415384809, 9.93f); // 60 acres
float yawOffset = static_cast<float>(91.27622 * M_PI / 180.0); // I was aligned with the road...

std::ofstream out_file(argv[1]);
//StandALoneSensors::createStaticData(out_file, period, total_duration, testLocation);
StandALoneSensors::generateBarometerStaticData(out_file, period, total_duration, testLocation);
//StandALoneSensors::generateBarometerStaticData(out_file, period, total_duration, testLocation);
//StandALoneSensors::generateBarometerDynamicData(out_file, period, total_duration, testLocation);
//StandALoneSensors::generateMagnetometerDataLoc(out_file, period, total_duration, testLocation);
StandALoneSensors::generateMagnetometer2D(out_file, period, total_duration, testLocation, yawOffset, true);
}

0 comments on commit e884603

Please sign in to comment.