Skip to content
Snippets Groups Projects
Commit 1e50875d authored by rmackay9's avatar rmackay9
Browse files

AP_InertialNav: correct lat/lon to cm

parent ebe33f1b
No related branches found
No related tags found
No related merge requests found
......@@ -166,10 +166,8 @@ void AP_InertialNav::correct_with_gps(int32_t lon, int32_t lat, float dt)
}
// calculate distance from base location
//x = (float)(lat - _base_lat) * 1.113195;
//y = (float)(lon - _base_lon) * _lon_to_m_scaling * 1.113195;
x = (float)(lat - _base_lat);
y = (float)(lon - _base_lon) * _lon_to_m_scaling;
x = (float)(lat - _base_lat) * AP_INERTIALNAV_LATLON_TO_CM;
y = (float)(lon - _base_lon) * _lon_to_m_scaling * AP_INERTIALNAV_LATLON_TO_CM;
// convert accelerometer readings to earth frame
Matrix3f dcm = _ahrs->get_dcm_matrix();
......@@ -213,8 +211,7 @@ int32_t AP_InertialNav::get_latitude()
return 0;
}
//return _base_lat - (int32_t)(_position.x / 1.113195);
return _base_lat - (int32_t)(_position_base.x + _position_correction.x);
return _base_lat - (int32_t)((_position_base.x + _position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM);
}
// get accel based longitude
......@@ -225,8 +222,7 @@ int32_t AP_InertialNav::get_longitude()
return 0;
}
//return _base_lon - (int32_t)(_position.y / (_lon_to_m_scaling * 1.113195) );
return _base_lon - (int32_t)((_position_base.y+_position_correction.y) / _lon_to_m_scaling );
return _base_lon - (int32_t)((_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM));
}
// set_current_position - all internal calculations are recorded as the distances from this point
......@@ -262,9 +258,7 @@ float AP_InertialNav::get_latitude_diff()
return 0;
}
//return _base_lat + (int32_t)_position.x;
//return -_position.x / 1.113195;
return -(_position_base.x+_position_correction.x);
return -((_position_base.x+_position_correction.x)/AP_INERTIALNAV_LATLON_TO_CM);
}
// get accel based longitude
......@@ -275,9 +269,7 @@ float AP_InertialNav::get_longitude_diff()
return 0;
}
//return _base_lon - (int32_t)(_position.x / _lon_to_m_scaling);
//return -_position.y / (_lon_to_m_scaling * 1.113195);
return -(_position_base.y+_position_correction.y) / _lon_to_m_scaling;
return -(_position_base.y+_position_correction.y) / (_lon_to_m_scaling*AP_INERTIALNAV_LATLON_TO_CM);
}
// get velocity in latitude & longitude directions
......
......@@ -17,6 +17,8 @@
#define AP_INTERTIALNAV_SAVE_POS_AFTER_ITERATIONS 10
#define AP_INTERTIALNAV_GPS_LAG_IN_10HZ_INCREMENTS 4 // must not be larger than size of _hist_position_estimate_x and _hist_position_estimate_y
#define AP_INERTIALNAV_LATLON_TO_CM 1.1113175
/*
* AP_InertialNav is an attempt to use accelerometers to augment other sensors to improve altitud e position hold
*/
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment