From 30d851284606476642c6cf8a54070bc08ab923c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Nov 2014 12:31:04 +0100 Subject: MC pos control offb: read altitude sp separately --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src/modules/mc_pos_control') 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 d52138522..f2c650ddd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt) _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } - if (_control_mode.flag_control_altitude_enabled) { + if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); -- cgit v1.2.3