diff options
| author | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-12 13:17:51 +0100 |
|---|---|---|
| committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-03-12 13:17:51 +0100 |
| commit | 67695f191ea6f528971c6d2da3dcac66540d7be9 (patch) | |
| tree | 67522fc213bd41c9a4ff7c7b07dd00d5e64b2b5f /src/modules/ekf_att_pos_estimator | |
| parent | 20592ce4d81ff533510698391d163b9ec5819c9d (diff) | |
| download | px4-firmware-67695f191ea6f528971c6d2da3dcac66540d7be9.tar.gz px4-firmware-67695f191ea6f528971c6d2da3dcac66540d7be9.tar.bz2 px4-firmware-67695f191ea6f528971c6d2da3dcac66540d7be9.zip | |
AttPosEKF: Use Geolib lat/lon position projection
Diffstat (limited to 'src/modules/ekf_att_pos_estimator')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 8 |
1 files changed, 2 insertions, 6 deletions
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(); |