forked from Archive/PX4-Autopilot
port #1752 to multiplatform
This commit is contained in:
parent
eb5278d12c
commit
6c3f57f87e
|
@ -620,22 +620,6 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
|
||||||
control_auto(dt);
|
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) {
|
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 */
|
/* idle state, don't run controller and set zero thrust */
|
||||||
_R.identity();
|
_R.identity();
|
||||||
|
@ -935,6 +919,11 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
|
||||||
|
|
||||||
_att_sp_msg.data().thrust = thrust_abs;
|
_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();
|
_att_sp_msg.data().timestamp = get_time_micros();
|
||||||
|
|
||||||
/* publish attitude setpoint */
|
/* 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 {
|
} else {
|
||||||
/* position controller disabled, reset setpoints */
|
/* position controller disabled, reset setpoints */
|
||||||
_reset_alt_sp = true;
|
_reset_alt_sp = true;
|
||||||
|
|
Loading…
Reference in New Issue