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 | 12 |
1 files changed, 12 insertions, 0 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 4d0c3216a..b73becf3c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -685,6 +685,18 @@ MulticopterPositionControl::task_main() _pos_sp(1) = _offboard_control_pos_sp.y; _pos_sp(2) = _offboard_control_pos_sp.z; _att_sp.yaw_body = _offboard_control_pos_sp.yaw; + + /* Make sure position control is selected i.e. not only velocity control */ + if (_control_mode.flag_control_position_enabled) { + _pos_sp(0) = _offboard_control_pos_sp.x; + _pos_sp(1) = _offboard_control_pos_sp.y; + } + + if (_control_mode.flag_control_altitude_enabled) { + _pos_sp(2) = _offboard_control_pos_sp.z; + } + + _att_sp.yaw_body = _offboard_control_pos_sp.yaw; } else { /* AUTO */ |