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

mc pos ctrl: revert to local map projection

Browse Source
This commit is contained in:
Thomas Gubler
2014-04-29 15:40:34 +02:00
parent 5a868751b5
commit 53d23c67d7
1 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -3
View File
@@ -468,15 +468,17 @@ MulticopterPositionControl::update_ref()
if (_ref_timestamp != 0) {
/* calculate current position setpoint in global frame */
map_projection_reproject(_pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_ref_timestamp != 0) {
/* reproject position setpoint to new reference */
map_projection_project(lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
@@ -675,7 +677,8 @@ MulticopterPositionControl::task_main()
_reset_alt_sp = true;
/* project setpoint to local frame */
map_projection_project(_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);