diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-22 12:32:51 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-24 20:31:02 +0100 |
commit | 6c3f57f87e1dc3e7fd36c049599e1fdcd51f0a26 (patch) | |
tree | bfe5990b38454266924ce8ab60e320773981ff9e /src | |
parent | eb5278d12cfe13f20619670ea2e0cf1602a12361 (diff) | |
download | px4-firmware-6c3f57f87e1dc3e7fd36c049599e1fdcd51f0a26.tar.gz px4-firmware-6c3f57f87e1dc3e7fd36c049599e1fdcd51f0a26.tar.bz2 px4-firmware-6c3f57f87e1dc3e7fd36c049599e1fdcd51f0a26.zip |
port #1752 to multiplatform
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp | 40 |
1 files changed, 24 insertions, 16 deletions
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<px4_vehicle_local_position_setpoint>(); - } - - 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<px4_vehicle_local_position_setpoint>(); + } + + } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; |