From 6c3f57f87e1dc3e7fd36c049599e1fdcd51f0a26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 22 Feb 2015 12:32:51 +0100 Subject: port #1752 to multiplatform --- .../mc_pos_control.cpp | 40 +++++++++++++--------- 1 file changed, 24 insertions(+), 16 deletions(-) (limited to 'src/modules/mc_pos_control_multiplatform') diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp index e6a7ee8a6..1fe75b90c 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -620,22 +620,6 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti control_auto(dt); } - /* fill local position setpoint */ - _local_pos_sp_msg.data().timestamp = get_time_micros(); - _local_pos_sp_msg.data().x = _pos_sp(0); - _local_pos_sp_msg.data().y = _pos_sp(1); - _local_pos_sp_msg.data().z = _pos_sp(2); - _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; - - /* publish local position setpoint */ - if (_local_pos_sp_pub != nullptr) { - _local_pos_sp_pub->publish(_local_pos_sp_msg); - - } else { - _local_pos_sp_pub = _n.advertise(); - } - - if (!_control_mode->data().flag_control_manual_enabled && _pos_sp_triplet->data().current.valid && _pos_sp_triplet->data().current.type == _pos_sp_triplet->data().current.SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ _R.identity(); @@ -935,6 +919,11 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti _att_sp_msg.data().thrust = thrust_abs; + /* save thrust setpoint for logging */ + _local_pos_sp_msg.data().acc_x = thrust_sp(0); + _local_pos_sp_msg.data().acc_x = thrust_sp(1); + _local_pos_sp_msg.data().acc_x = thrust_sp(2); + _att_sp_msg.data().timestamp = get_time_micros(); /* publish attitude setpoint */ @@ -950,6 +939,25 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti } } + /* fill local position setpoint */ + _local_pos_sp_msg.data().timestamp = get_time_micros(); + _local_pos_sp_msg.data().x = _pos_sp(0); + _local_pos_sp_msg.data().y = _pos_sp(1); + _local_pos_sp_msg.data().z = _pos_sp(2); + _local_pos_sp_msg.data().yaw = _att_sp_msg.data().yaw_body; + _local_pos_sp_msg.data().vx = _vel_sp(0); + _local_pos_sp_msg.data().vy = _vel_sp(1); + _local_pos_sp_msg.data().vz = _vel_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub != nullptr) { + _local_pos_sp_pub->publish(_local_pos_sp_msg); + + } else { + _local_pos_sp_pub = _n.advertise(); + } + + } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; -- cgit v1.2.3