From 4fdf8e1ff26d37513c003ea1e92445fae81cc2cb Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 23 Oct 2014 16:59:21 +0200 Subject: updated from remote --- src/modules/mc_att_control/mc_att_control_base.cpp | 283 +++++++++++++++++++++ src/modules/mc_att_control/mc_att_control_base.h | 172 +++++++++++++ 2 files changed, 455 insertions(+) create mode 100644 src/modules/mc_att_control/mc_att_control_base.cpp create mode 100644 src/modules/mc_att_control/mc_att_control_base.h (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp new file mode 100644 index 000000000..d4270b153 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -0,0 +1,283 @@ +/* + * mc_att_control_base.cpp + * + * Created on: Sep 25, 2014 + * Author: roman + */ + +#include "mc_att_control_base.h" +#include +#include + +#ifdef CONFIG_ARCH_ARM +#else +#include +using namespace std; +#endif + +MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : + + _task_should_exit(false), _control_task(-1), + + _actuators_0_circuit_breaker_enabled(false), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + +{ + memset(&_v_att, 0, sizeof(_v_att)); + memset(&_v_att_sp, 0, sizeof(_v_att_sp)); + memset(&_v_rates_sp, 0, sizeof(_v_rates_sp)); + memset(&_manual_control_sp, 0, sizeof(_manual_control_sp)); + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + memset(&_actuators, 0, sizeof(_actuators)); + memset(&_armed, 0, sizeof(_armed)); + + _params.att_p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + _params.yaw_ff = 0.0f; + _params.yaw_rate_max = 0.0f; + _params.man_roll_max = 0.0f; + _params.man_pitch_max = 0.0f; + _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); + + _rates_prev.zero(); + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + + _I.identity(); +} + +MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { +} + +void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() { +} + +void MulticopterAttitudeControlBase::control_attitude(float dt) { + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + if (_v_control_mode.flag_control_manual_enabled) { + /* manual input, set or modify attitude setpoint */ + + if (_v_control_mode.flag_control_velocity_enabled + || _v_control_mode.flag_control_climb_rate_enabled) { + /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ + vehicle_attitude_setpoint_poll(); + } + + if (!_v_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude stabilized mode */ + _v_att_sp.thrust = _manual_control_sp.z; + publish_att_sp = true; + } + + if (!_armed.armed) { + /* reset yaw setpoint when disarmed */ + _reset_yaw_sp = true; + } + + /* move yaw setpoint in all modes */ + if (_v_att_sp.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.r * _params.man_yaw_max; + _v_att_sp.yaw_body = _wrap_pi( + _v_att_sp.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.yaw_body - _v_att.yaw); + if (yaw_offs < -yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); + + } else if (yaw_offs > yaw_offs_max) { + _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); + } + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + /* reset yaw setpint to current position if needed */ + if (_reset_yaw_sp) { + _reset_yaw_sp = false; + _v_att_sp.yaw_body = _v_att.yaw; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_v_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x + * _params.man_pitch_max; + _v_att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ + vehicle_attitude_setpoint_poll(); + + /* reset yaw setpoint after non-manual control mode */ + _reset_yaw_sp = true; + } + + _thrust_sp = _v_att_sp.thrust; + + /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + + if (_v_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_v_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, + _v_att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], + sizeof(_v_att_sp.R_body)); + _v_att_sp.R_valid = true; + } + +// /* publish the attitude setpoint if needed */ +// if (publish_att_sp) { +// _v_att_sp.timestamp = hrt_absolute_time(); +// +// if (_att_sp_pub > 0) { +// orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, +// &_v_att_sp); +// +// } else { +// _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), +// &_v_att_sp); +// } +// } + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.set(_v_att.R); + + /* all input data is ready, run controller itself */ + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R + * (_I + e_R_cp * e_R_z_sin + + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + math::Quaternion q; + q.from_dcm(R.transposed() * R_sp); + math::Vector < 3 > e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* calculate angular rates setpoint */ + _rates_sp = _params.att_p.emult(e_R); + + /* limit yaw rate */ + _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, + _params.yaw_rate_max); + + /* feed forward yaw setpoint rate */ + _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; +} + +void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { + /* reset integral if disarmed */ + if (!_armed.armed) { + _rates_int.zero(); + } + + /* current body angular rates */ + math::Vector < 3 > rates; + rates(0) = _v_att.rollspeed; + rates(1) = _v_att.pitchspeed; + rates(2) = _v_att.yawspeed; + + /* angular rates error */ + math::Vector < 3 > rates_err = _rates_sp - rates; + _att_control = _params.rate_p.emult(rates_err) + + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int; + _rates_prev = rates; + + /* update integral only if not saturated on low limit */ + if (_thrust_sp > MIN_TAKEOFF_THRUST) { + for (int i = 0; i < 3; i++) { + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite( + rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } + } + } + } + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h new file mode 100644 index 000000000..d75088b85 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -0,0 +1,172 @@ +/* + * mc_att_control_base.h + * + * Created on: Sep 25, 2014 + * Author: roman + */ + +#ifndef MC_ATT_CONTROL_BASE_H_ +#define MC_ATT_CONTROL_BASE_H_ + +/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_main.cpp + * Multicopter attitude controller. + * + * @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include + + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ + +#define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f +#define RATES_I_LIMIT 0.3f + +class MulticopterAttitudeControlBase { +public: + /** + * Constructor + */ + MulticopterAttitudeControlBase(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControlBase(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + + + + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ + struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ + struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */ + struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator controls */ + struct actuator_armed_s _armed; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + math::Vector<3> _rates_sp; /**< angular rates setpoint */ + math::Vector<3> _rates_int; /**< angular rates integral error */ + float _thrust_sp; /**< thrust setpoint */ + math::Vector<3> _att_control; /**< attitude control vector */ + + math::Matrix<3, 3> _I; /**< identity matrix */ + + bool _reset_yaw_sp; /**< reset yaw setpoint flag */ + + + + struct { + math::Vector<3> att_p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + float yaw_ff; /**< yaw control feed-forward */ + float yaw_rate_max; /**< max yaw rate */ + + float man_roll_max; + float man_pitch_max; + float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ + } _params; + + + /** + * Attitude controller. + */ + //void control_attitude(float dt); + + /** + * Attitude rates controller. + */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + + void vehicle_attitude_setpoint_poll(); //provisional + + +}; + +#endif /* MC_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From b168ba92e0be91aec9db5aaccbcc427cfd067c7b Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 29 Oct 2014 15:57:45 +0100 Subject: added setter functions into base class. used when integrating the base class into a gazebo environment --- src/modules/mc_att_control/mc_att_control_base.cpp | 57 ++++++++++++++++++++++ src/modules/mc_att_control/mc_att_control_base.h | 7 +++ 2 files changed, 64 insertions(+) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d4270b153..d39bae4dc 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -281,3 +281,60 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } } + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { + // check if this is consistent !!! + math::Vector<3> quat; + quat(0) = attitude(0); + quat(1) = attitude(1); + quat(2) = attitude(2); + quat(3) = attitude(3); + + _v_att.q[0] = quat(0); + _v_att.q[1] = quat(1); + _v_att.q[2] = quat(2); + _v_att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _v_att.R[0][0] = Rot(0,0); + _v_att.R[1][0] = Rot(1,0); + _v_att.R[2][0] = Rot(2,0); + _v_att.R[0][1] = Rot(0,1); + _v_att.R[1][1] = Rot(1,1); + _v_att.R[2][1] = Rot(2,1); + _v_att.R[0][2] = Rot(0,2); + _v_att.R[1][2] = Rot(1,2); + _v_att.R[2][2] = Rot(2,2); + + _v_att.R_valid = true; + +} + +void MulticopterAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + // check if this is consistent !!! + _v_att.rollspeed = angular_rate(0); + _v_att.pitchspeed = angular_rate(1); + _v_att.yawspeed = angular_rate(2); + +} + +void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _v_att_sp.roll_body = control_attitude_thrust_reference(0); + _v_att_sp.pitch_body = control_attitude_thrust_reference(1); + _v_att_sp.yaw_body = control_attitude_thrust_reference(2); + _v_att_sp.thrust = control_attitude_thrust_reference(3); + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + _v_att_sp.R_body[0][0] = Rot_sp(0,0); + _v_att_sp.R_body[1][0] = Rot_sp(1,0); + _v_att_sp.R_body[2][0] = Rot_sp(2,0); + _v_att_sp.R_body[0][1] = Rot_sp(0,1); + _v_att_sp.R_body[1][1] = Rot_sp(1,1); + _v_att_sp.R_body[2][1] = Rot_sp(2,1); + _v_att_sp.R_body[0][2] = Rot_sp(0,2); + _v_att_sp.R_body[1][2] = Rot_sp(1,2); + _v_att_sp.R_body[2][2] = Rot_sp(2,2); + +} diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index d75088b85..995838bb2 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -166,6 +166,13 @@ protected: void vehicle_attitude_setpoint_poll(); //provisional + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion& attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + + + }; -- cgit v1.2.3 From c51ec3411850d9de0794d4e584838d4a137dafcf Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 08:50:20 +0100 Subject: added initialisation of parameters, added assignment of actuator controls, cleaned up --- src/modules/mc_att_control/mc_att_control_base.cpp | 53 ++++++++++++++++++---- 1 file changed, 45 insertions(+), 8 deletions(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index d39bae4dc..fee117458 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -51,6 +51,27 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() : _att_control.zero(); _I.identity(); + + // setup standard gains + _params.att_p(0) = 5.0; + _params.rate_p(0) = 0.05; + _params.rate_i(0) = 0.0; + _params.rate_d(0) = 0.003; + /* pitch gains */ + _params.att_p(1) = 5.0; + _params.rate_p(1) = 0.05; + _params.rate_i(1) = 0.0; + _params.rate_d(1) = 0.003; + /* yaw gains */ + _params.att_p(2) = 2.8; + _params.rate_p(2) = 0.2; + _params.rate_i(2) = 0.1; + _params.rate_d(2) = 0.0; + _params.yaw_rate_max = 0.5; + _params.yaw_ff = 0.5; + _params.man_roll_max = 0.6; + _params.man_pitch_max = 0.6; + _params.man_yaw_max = 0.6; } MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() { @@ -282,13 +303,21 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt) { } -void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion& attitude) { +void MulticopterAttitudeControlBase::set_actuator_controls() { + _actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f; + _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; + _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; + _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; + //_actuators.timestamp = hrt_absolute_time(); +} + +void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { // check if this is consistent !!! - math::Vector<3> quat; - quat(0) = attitude(0); - quat(1) = attitude(1); - quat(2) = attitude(2); - quat(3) = attitude(3); + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); _v_att.q[0] = quat(0); _v_att.q[1] = quat(1); @@ -322,11 +351,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = control_attitude_thrust_reference(3); + _v_att_sp.thrust = (control_attitude_thrust_referenc(3) -30)*(-1)/30; // setup rotation matrix math::Matrix<3,3> Rot_sp; - Rot_sp = R.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); + Rot_sp.from_euler(_v_att_sp.roll_body,_v_att_sp.pitch_body,_v_att_sp.yaw_body); _v_att_sp.R_body[0][0] = Rot_sp(0,0); _v_att_sp.R_body[1][0] = Rot_sp(1,0); _v_att_sp.R_body[2][0] = Rot_sp(2,0); @@ -338,3 +367,11 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.R_body[2][2] = Rot_sp(2,2); } + +void MulticopterAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) { + motor_inputs(0) = _actuators.control[0]; + motor_inputs(1) = _actuators.control[1]; + motor_inputs(2) = _actuators.control[2]; + motor_inputs(3) = _actuators.control[3]; +} + -- cgit v1.2.3 From 48cdbf3d0cb8aed7dcdddde872a64096e7c9a595 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 08:51:12 +0100 Subject: added more setter and getter functions --- src/modules/mc_att_control/mc_att_control_base.h | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 995838bb2..8f1cf015e 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -76,6 +76,8 @@ #include //#include #include +#include + /** @@ -105,6 +107,14 @@ public: * * @return OK on success. */ + void control_attitude(float dt); + void control_attitude_rates(float dt); + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion attitude); + void set_attitude_rates(const Eigen::Vector3d& angular_rate); + void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + void get_mixer_input(Eigen::Vector4d& motor_inputs); + void set_actuator_controls(); protected: @@ -161,15 +171,10 @@ protected: /** * Attitude rates controller. */ - void control_attitude(float dt); - void control_attitude_rates(float dt); - + void vehicle_attitude_setpoint_poll(); //provisional - // setters and getters for interface with euroc-gazebo simulator - void set_attitude(const Eigen::Quaternion& attitude); - void set_attitude_rates(const Eigen::Vector3d& angular_rate); - void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference); + -- cgit v1.2.3 From b5e34eac4faae4b0bc605c6d2d36ed65740c781d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 5 Nov 2014 15:40:22 +0100 Subject: fixed typo --- src/modules/mc_att_control/mc_att_control_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index fee117458..b07a1a6be 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -351,7 +351,7 @@ void MulticopterAttitudeControlBase::set_attitude_reference(const Eigen::Vector4 _v_att_sp.roll_body = control_attitude_thrust_reference(0); _v_att_sp.pitch_body = control_attitude_thrust_reference(1); _v_att_sp.yaw_body = control_attitude_thrust_reference(2); - _v_att_sp.thrust = (control_attitude_thrust_referenc(3) -30)*(-1)/30; + _v_att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; // setup rotation matrix math::Matrix<3,3> Rot_sp; -- cgit v1.2.3 From f72f0db9963049879eb0dce9a9ef37baadd45dd2 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:10:43 +0100 Subject: include correct eigen lib header --- src/modules/mc_att_control/mc_att_control_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 8f1cf015e..5a08bba79 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -76,7 +76,7 @@ #include //#include #include -#include +//#include -- cgit v1.2.3 From a2ec2ec857f5b7584628c2a923c85d0b62b082dd Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 16:58:56 +0100 Subject: cleaned up --- src/modules/mc_att_control/mc_att_control_base.h | 58 +++--------------------- 1 file changed, 6 insertions(+), 52 deletions(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h index 5a08bba79..515fb0c14 100644 --- a/src/modules/mc_att_control/mc_att_control_base.h +++ b/src/modules/mc_att_control/mc_att_control_base.h @@ -1,14 +1,7 @@ -/* - * mc_att_control_base.h - * - * Created on: Sep 25, 2014 - * Author: roman - */ - #ifndef MC_ATT_CONTROL_BASE_H_ #define MC_ATT_CONTROL_BASE_H_ -/* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,21 +33,10 @@ ****************************************************************************/ /** - * @file mc_att_control_main.cpp - * Multicopter attitude controller. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin + * @file mc_att_control_base.h + * + * @author Roman Bapst * - * The controller has two loops: P loop for angular error and PD loop for angular rate error. - * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. - * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, - * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. - * These two approaches fused seamlessly with weight depending on angular error. - * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. - * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. - * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. */ #include @@ -74,18 +56,10 @@ #include #include #include -//#include #include -//#include -/** - * Multicopter attitude control app start / stop handling function - * - * @ingroup apps - */ - #define YAW_DEADZONE 0.05f #define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f @@ -98,7 +72,7 @@ public: MulticopterAttitudeControlBase(); /** - * Destructor, also kills the sensors task. + * Destructor */ ~MulticopterAttitudeControlBase(); @@ -109,6 +83,7 @@ public: */ void control_attitude(float dt); void control_attitude_rates(float dt); + // setters and getters for interface with euroc-gazebo simulator void set_attitude(const Eigen::Quaternion attitude); void set_attitude_rates(const Eigen::Vector3d& angular_rate); @@ -120,10 +95,6 @@ protected: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - - - - bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ struct vehicle_attitude_s _v_att; /**< vehicle attitude */ @@ -146,8 +117,6 @@ protected: bool _reset_yaw_sp; /**< reset yaw setpoint flag */ - - struct { math::Vector<3> att_p; /**< P gain for angular error */ math::Vector<3> rate_p; /**< P gain for angular rate error */ @@ -161,24 +130,9 @@ protected: float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; - - - /** - * Attitude controller. - */ - //void control_attitude(float dt); - - /** - * Attitude rates controller. - */ void vehicle_attitude_setpoint_poll(); //provisional - - - - - }; #endif /* MC_ATT_CONTROL_BASE_H_ */ -- cgit v1.2.3 From 0acdf5927b9e2dd2cc7375008af2d00cd7f9ba0e Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Mon, 10 Nov 2014 17:01:59 +0100 Subject: cleaned up --- src/modules/mc_att_control/mc_att_control_base.cpp | 44 +++++++++++++++++----- 1 file changed, 35 insertions(+), 9 deletions(-) (limited to 'src/modules/mc_att_control') diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp index b07a1a6be..baf2bfe65 100644 --- a/src/modules/mc_att_control/mc_att_control_base.cpp +++ b/src/modules/mc_att_control/mc_att_control_base.cpp @@ -1,8 +1,39 @@ -/* - * mc_att_control_base.cpp +/* Copyright (c) 2014 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** + * @file mc_att_control_base.h + * + * @author Roman Bapst * - * Created on: Sep 25, 2014 - * Author: roman */ #include "mc_att_control_base.h" @@ -308,11 +339,9 @@ void MulticopterAttitudeControlBase::set_actuator_controls() { _actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; - //_actuators.timestamp = hrt_absolute_time(); } void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion attitude) { - // check if this is consistent !!! math::Quaternion quat; quat(0) = (float)attitude.w(); quat(1) = (float)attitude.x(); @@ -336,7 +365,6 @@ void MulticopterAttitudeControlBase::set_attitude(const Eigen::Quaternion