dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AttPosEKF: Use Geolib lat/lon position projection

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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();