diff options
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 |
1 files changed, 8 insertions, 1 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 97357d07a..21070e1e9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -473,8 +473,15 @@ void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { + if (_ref_timestamp != 0) { + /* reproject local position setpoint to new reference */ + float dx, dy; + map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy); + _pos_sp(0) -= dx; + _pos_sp(1) -= dy; + } + _ref_timestamp = _local_pos.ref_timestamp; - // TODO mode position setpoint in assisted modes map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon); _ref_alt = _local_pos.ref_alt; |