Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
Trent Lukaczyk
2015-02-06 18:40:17 -08:00
parent 33141ae7a2 a2a0451094
commit 52d5d690ff
5 changed files with 53 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/px_generate_uorb_topic_headers.py
+6 -1
View File
@@ -42,7 +42,12 @@ import os
import shutil
import filecmp
import argparse
import genmsg.template_tools
try:
import genmsg.template_tools
except ImportError, e:
print("Package empy not installed. Please run 'sudo apt-get install python-empy' on a Debian/Ubuntu system, 'sudo pip install empy' on a Mac OS system or 'easy_install empy' on Windows system.")
exit(1)
__author__ = "Thomas Gubler"
__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh
+1 -3
View File
@@ -18,9 +18,7 @@ echo "source $WORKSPACE/devel/setup.bash" >> ~/.bashrc
# PX4 Firmware
cd $WORKSPACE/src
git clone https://github.com/PX4/Firmware.git \
&& cd Firmware \
&& git checkout ros
git clone https://github.com/PX4/Firmware.git
# euroc simulator
cd $WORKSPACE/src
Tools/ros/px4_workspace_setup.sh
-3
View File
@@ -7,9 +7,6 @@ cd src
# PX4 Firmware
git clone https://github.com/PX4/Firmware.git
cd Firmware
git checkout ros
cd ..
# euroc simulator
git clone https://github.com/PX4/euroc_simulator.git
src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+25 -26
View File
@@ -112,32 +112,6 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
_reset_yaw_sp = true;
}
/* move yaw setpoint in all modes */
if (_v_att_sp_mod.data().thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_v_att_sp_mod.data().yaw_body = _wrap_pi(
_v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
if (yaw_offs < -yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
}
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
/* reset yaw setpoint to current position if needed */
if (_reset_yaw_sp) {
_reset_yaw_sp = false;
@@ -147,6 +121,31 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
}
if (!_v_control_mode->data().flag_control_velocity_enabled) {
if (_v_att_sp_mod.data().thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_v_att_sp_mod.data().yaw_body = _wrap_pi(
_v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
if (yaw_offs < -yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
}
_v_att_sp_mod.data().R_valid = false;
// _publish_att_sp = true;
}
/* update attitude setpoint if not in position control mode */
_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+21 -2
View File
@@ -245,6 +245,10 @@ MulticopterPositionControl::reset_alt_sp()
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
//XXX hack until #1741 is in/ported
/* reset yaw sp */
_att_sp_msg.data().yaw_body = _att->data().yaw;
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
@@ -297,6 +301,22 @@ MulticopterPositionControl::control_manual(float dt)
_sp_move_rate /= sp_move_norm;
}
/* move yaw setpoint */
//XXX hardcoded hack until #1741 is in/ported (the values stem
//from default param values, see how yaw setpoint is moved in the attitude controller)
float yaw_sp_move_rate = _manual_control_sp->data().r * 120.0f * M_DEG_TO_RAD_F;
_att_sp_msg.data().yaw_body = _wrap_pi(
_att_sp_msg.data().yaw_body + yaw_sp_move_rate * dt);
float yaw_offs_max = 120.0f * M_DEG_TO_RAD_F / 2.0f;
float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
if (yaw_offs < -yaw_offs_max) {
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - yaw_offs_max);
} else if (yaw_offs > yaw_offs_max) {
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + yaw_offs_max);
}
/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp_msg.data().yaw_body);
@@ -627,8 +647,7 @@ void MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
_att_sp_msg.data().R_valid = true;
_att_sp_msg.data().roll_body = 0.0f;
_att_sp_msg.data().pitch_body = 0.0f;
_att_sp_msg.data().yaw_body = _att->data().yaw;
// _att_sp_msg.data().yaw_body = _att->data().yaw;
_att_sp_msg.data().thrust = 0.0f;
_att_sp_msg.data().timestamp = get_time_micros();