diff options
5 files changed, 53 insertions, 35 deletions
diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index 3e24e66b2..694665592 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -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." diff --git a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh index 2de5f8bec..72ab5a609 100644 --- a/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh +++ b/Tools/ros/docker/px4-ros-full/scripts/setup-workspace.sh @@ -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 diff --git a/Tools/ros/px4_workspace_setup.sh b/Tools/ros/px4_workspace_setup.sh index 53568b4fb..3e4bf06a5 100755 --- a/Tools/ros/px4_workspace_setup.sh +++ b/Tools/ros/px4_workspace_setup.sh @@ -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 diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp index 1f0d88bcd..e779239ba 100644 --- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -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 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 a46163e6f..b7fce5029 100644 --- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -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(); |