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

mc_pos_control: reproject local position setpoint on local reference updates

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-02 11:31:30 +04:00
parent a991ebd8ca
commit fdb17c9776
1 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+8 -1
View File
@@ -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;