From 67695f191ea6f528971c6d2da3dcac66540d7be9 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 12 Mar 2015 13:17:51 +0100 Subject: AttPosEKF: Use Geolib lat/lon position projection --- src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3bb395a87..dfe3f5357 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -908,8 +908,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate() } void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, - const bool fuseRangeSensor, - const bool fuseBaro, const bool fuseAirSpeed) + const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed) { // Run the strapdown INS equations every IMU update _ekf->UpdateStrapdownEquationsNED(); @@ -1324,10 +1323,7 @@ void AttitudePositionEstimatorEKF::pollData() if (_gps_initialized) { //Convert from global frame to local frame - float posNED[3] = {0.0f, 0.0f, 0.0f}; - _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - _ekf->posNE[0] = posNED[0]; - _ekf->posNE[1] = posNED[1]; + map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]); if (dtLastGoodGPS > POS_RESET_THRESHOLD) { _ekf->ResetPosition(); -- cgit v1.2.3