diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-02 11:31:30 +0400 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-02 11:31:30 +0400 |
| commit | fdb17c9776d573c46358684a6c6bd19afd2e1df2 (patch) | |
| tree | 9f3e82f76f297f6cc24235a3313fb40b041f7516 /src/modules/mc_pos_control/mc_pos_control_main.cpp | |
| parent | a991ebd8ca4aa1dc6fd69e307a74be6a93f4e6ff (diff) | |
| download | px4-firmware-fdb17c9776d573c46358684a6c6bd19afd2e1df2.tar.gz px4-firmware-fdb17c9776d573c46358684a6c6bd19afd2e1df2.tar.bz2 px4-firmware-fdb17c9776d573c46358684a6c6bd19afd2e1df2.zip | |
mc_pos_control: reproject local position setpoint on local reference updates
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; |