diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-05 18:11:51 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-05 18:11:51 +0400 |
commit | 79d2247b44aaeb0f2211a1c66c994e0fa7ca4b9f (patch) | |
tree | 6be78e1a3020c149a4cf16d3c6b360b518fcaec8 /src/modules/mc_pos_control | |
parent | 0fd6fb53f33bbe923973ee519e2464655f2c2bc5 (diff) | |
download | px4-firmware-79d2247b44aaeb0f2211a1c66c994e0fa7ca4b9f.tar.gz px4-firmware-79d2247b44aaeb0f2211a1c66c994e0fa7ca4b9f.tar.bz2 px4-firmware-79d2247b44aaeb0f2211a1c66c994e0fa7ca4b9f.zip |
position_estimator_inav, mc_pos_control: precise position reprojection on home position changes
Diffstat (limited to 'src/modules/mc_pos_control')
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 22 |
1 files changed, 15 insertions, 7 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 21070e1e9..dc0aa172a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -473,18 +473,26 @@ void MulticopterPositionControl::update_ref() { if (_local_pos.ref_timestamp != _ref_timestamp) { + double lat_sp, lon_sp; + float alt_sp; + 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; + /* calculate current position setpoint in global frame */ + map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp); + alt_sp = _ref_alt - _pos_sp(2); } - _ref_timestamp = _local_pos.ref_timestamp; - + /* 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(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]); + _pos_sp(2) = -(alt_sp - _ref_alt); + } + + _ref_timestamp = _local_pos.ref_timestamp; } } |