dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1822 from PX4/mcposcontrolmultiplatform1752 

port #1752 to multiplatform
This commit is contained in:
Thomas Gubler 2015-02-24 20:31:24 +01:00
parent eb5278d12c 6c3f57f87e
commit e91dabd3d5
1 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@ -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;