diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-28 18:34:33 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-04-28 18:34:33 +0200 |
| commit | 90569d22bcee65b102c9f55be71806d301829cee (patch) | |
| tree | 36e0a41693fceea1e1978811470fe589a923b0ab /src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | |
| parent | 2c37ec16dc4a8ba47d985dc489b5b322758457c2 (diff) | |
| download | px4-firmware-90569d22bcee65b102c9f55be71806d301829cee.tar.gz px4-firmware-90569d22bcee65b102c9f55be71806d301829cee.tar.bz2 px4-firmware-90569d22bcee65b102c9f55be71806d301829cee.zip | |
Added support for automatic mag declination setup
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp')
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 13 |
1 files changed, 8 insertions, 5 deletions
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d704..23f9e80bd 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; + + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } |