fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 01:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw_att_pos_estimator: use new global map projection

Browse Source
This commit is contained in:
Thomas Gubler
2014-04-29 14:40:01 +02:00
parent 3ec818ce1e
commit 4f84cdc8b8
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+4 -4
View File
@@ -797,7 +797,7 @@ FixedwingEstimator::task_main()
if (hrt_elapsed_time(&start_time) > 100000) { if (hrt_elapsed_time(&start_time) > 100000) {
if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_initialized()) { if (!_gps_initialized && (_ekf->GPSstatus == 3) && map_projection_global_initialized()) {
_ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s; _ekf->velNED[2] = _gps.vel_d_m_s;
@@ -809,9 +809,9 @@ FixedwingEstimator::task_main()
_ekf->InitialiseFilter(_ekf->velNED); _ekf->InitialiseFilter(_ekf->velNED);
// Initialize projection // Initialize projection
map_projection_reference(&_local_pos.ref_lat, &_local_pos.ref_lon); map_projection_global_reference(&_local_pos.ref_lat, &_local_pos.ref_lon);
_local_pos.ref_alt = alt; _local_pos.ref_alt = alt;
_local_pos.ref_timestamp = map_projection_timestamp(); _local_pos.ref_timestamp = map_projection_global_timestamp();
// Store // Store
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
@@ -1041,7 +1041,7 @@ FixedwingEstimator::task_main()
if (_local_pos.xy_global) { if (_local_pos.xy_global) {
double est_lat, est_lon; double est_lat, est_lon;
map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); map_projection_global_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon);
_global_pos.lat = est_lat; _global_pos.lat = est_lat;
_global_pos.lon = est_lon; _global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec; _global_pos.time_gps_usec = _gps.time_gps_usec;