fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 15:41:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

AttPosEKF: Use Geolib lat/lon position projection

Browse Source
This commit is contained in:
Johan Jansen
2015-03-12 13:17:51 +01:00
parent 20592ce4d8
commit 67695f191e
1 changed files with 2 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+2 -6
View File
@@ -908,8 +908,7 @@ void AttitudePositionEstimatorEKF::publishWindEstimate()
} }
void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag,
const bool fuseRangeSensor, const bool fuseRangeSensor, const bool fuseBaro, const bool fuseAirSpeed)
const bool fuseBaro, const bool fuseAirSpeed)
{ {
// Run the strapdown INS equations every IMU update // Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED(); _ekf->UpdateStrapdownEquationsNED();
@@ -1324,10 +1323,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (_gps_initialized) { if (_gps_initialized) {
//Convert from global frame to local frame //Convert from global frame to local frame
float posNED[3] = {0.0f, 0.0f, 0.0f}; map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]);
_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];
if (dtLastGoodGPS > POS_RESET_THRESHOLD) { if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
_ekf->ResetPosition(); _ekf->ResetPosition();