diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 |
1 files changed, 6 insertions, 3 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index bf7b6a19c..6b797f222 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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); |