Commit 283aff5c authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Fix declination math

parent 935a518a
......@@ -188,8 +188,12 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
velocity_current_W_xy.z = 0.0;
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
const double lat_zurich = 47.3667 * M_PI / 180 ; // rad
const double lon_zurich = 8.5500 * M_PI / 180; // rad
// Zurich Irchel Park
const double lat_zurich = 47.397742 * M_PI / 180 ; // rad
const double lon_zurich = 8.545594 * M_PI / 180; // rad
// Seattle downtown (15 deg declination): 47.592182, -122.316031
// const double lat_zurich = 47.592182 * M_PI / 180 ; // rad
// const double lon_zurich = -122.316031 * M_PI / 180; // rad
const float earth_radius = 6353000; // m
// reproject local position to gps coordinates
......@@ -340,7 +344,7 @@ void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) {
// TODO replace mag_W_ in the line below with mag_decl
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_W_); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 mag_I = C_W_I.RotateVectorReverse(mag_decl); // TODO: Add noise based on bais and variance like for imu and gyro
math::Vector3 body_vel = C_W_I.RotateVectorReverse(model_->GetWorldLinearVel());
standard_normal_distribution_ = std::normal_distribution<float>(0, 0.01f);
......
......@@ -43,7 +43,8 @@
*
*/
#include <stdint.h>
#include <cstdint>
#include <cmath>
#include "geo_mag_declination.h"
/** set this always to the sampling in degrees for the table below */
......@@ -72,8 +73,11 @@ static const int8_t declination_table[13][37] = \
static float get_lookup_table_val(unsigned lat, unsigned lon);
float get_mag_declination(float lat, float lon)
float get_mag_declination(float lat_rad, float lon_rad)
{
float lat = lat_rad / M_PI * 180.0f;
float lon = lon_rad / M_PI * 180.0f;
/*
* If the values exceed valid ranges, return zero as default
* as we have no way of knowing what the closest real value
......@@ -123,7 +127,9 @@ float get_mag_declination(float lat, float lon)
float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
float declination_ret = ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
return declination_ret / 180.0f * M_PI;
}
float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment