From 178b9b0a697eb0325689b271f8ffbba528dede14 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Nov 2013 23:03:19 +0400 Subject: [PATCH] mc_att_control_vector: initial rewrite, WIP --- .../ecl_mc_att_control_vector.cpp | 147 ------------- .../ecl_mc_att_control_vector.h | 76 ------- .../mc_att_control_vector_main.cpp | 204 ++++++------------ src/modules/mc_att_control_vector/module.mk | 3 +- 4 files changed, 66 insertions(+), 364 deletions(-) delete mode 100644 src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp delete mode 100644 src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp deleted file mode 100644 index 589c31c0d6..0000000000 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * - * 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 ecl_mc_att_control_vector.cpp - * - * Multirotor attitude controller based on concepts in: - * - * Minimum Snap Trajectory Generation and Control for Quadrotors - * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf - * - * @author Tobias Naegeli - * @author Lorenz Meier - * - */ - -#include -#include "ecl_mc_att_control_vector.h" - -ECL_MCAttControlVector::ECL_MCAttControlVector() : - _integral_error(0.0f, 0.0f), - _integral_max(1000.0f, 1000.0f), - _integral_lock(false) - { - - } - -void ECL_MCAttControlVector::control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des_in, - float Kp, float Kd, float Ki, const math::Vector &angular_rates, - math::Vector &rates_des, float &thrust) -{ - // XXX - bool earth = true; - bool integral_reset = false; - - math::Matrix R_bn = R_nb.transpose(); - - float cy = cosf(yaw); - float sy = sinf(yaw); - - math::Matrix RYaw = math::Dcm(cy,-sy,0.0f,sy,cy,0.0f,0.0f,0.0f,1.0f); - math::Vector z_b = math::Vector3(R_bn(0,2), R_bn(1,2), R_bn(2,2)); - - math::Vector3 F_des = F_des_in; - - // desired thrust in body frame - // avoid division by zero - // compensates for thrust loss due to roll/pitch - //XXX disable this for first time - //if (F_des(2) >= 0.1f) { - // thrust = F_des(2) / R_bn(2, 2); - //} else { - // F_des(2) = 0.1f; - // thrust= F_des(2) / R_bn(2, 2); - //} - - math::Vector3 x_B_des; - math::Vector3 y_B_des; - math::Vector3 z_B_des; - - // desired body z axis - if (earth) { - z_B_des = (F_des / F_des.norm()); - } else { - z_B_des = RYaw * (F_des / F_des.norm()); - } - - // desired direction in world coordinates (yaw angle) - math::Vector3 x_C(cy, sy, 0.0f); - // desired body y axis - y_B_des = z_B_des.cross(x_C) / (z_B_des.cross(x_C)).norm(); - // desired body x axis - x_B_des = y_B_des.cross(z_B_des); - // desired Rotation Matrix - math::Matrix R_des = math::Dcm(x_B_des(0), x_B_des(1), x_B_des(2), - y_B_des(0), y_B_des(1), y_B_des(2), - z_B_des(0), z_B_des(1), z_B_des(2)); - - - // Attitude Controller - // P controller - - // error rotation matrix - math::Matrix e_R = (R_des.transpose() * R_bn - R_bn.transpose() * R_des) * 0.5f; - - // error rotation vector - math::Vector e_R_v(3); - e_R_v(0) = e_R(1,2); - e_R_v(1) = e_R(0,2); - e_R_v(2) = e_R(0,1); - - - // include an integral part - math::Vector intError = math::Vector3(0.0f, 0.0f, 0.0f); - if (!_integral_lock) { - if (thrust > 0.3f && !integral_reset) { - if (fabsf(_integral_error(0)) < _integral_max(0)) { - _integral_error(0) = _integral_error(0) + e_R_v(0) * dt; - } - - if (fabsf(_integral_error(1)) < _integral_max(1)) { - _integral_error(1) = _integral_error(1) + e_R_v(1) * dt; - } - - } else { - _integral_error(0) = 0.0f; - _integral_error(1) = 0.0f; - } - - intError(0) = _integral_error(0); - intError(1) = _integral_error(1); - intError(2) = 0.0f; - } - - rates_des = (e_R_v * Kp + angular_rates * Kd + intError * Ki); -} diff --git a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h deleted file mode 100644 index a3440a06b2..0000000000 --- a/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * - * 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 ecl_mc_att_control_vector.cpp - * - * Multirotor attitude controller based on concepts in: - * - * Minimum Snap Trajectory Generation and Control for Quadrotors - * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf - * - * @author Tobias Naegeli - * @author Lorenz Meier - * - */ - -#include - -class ECL_MCAttControlVector { - -public: - ECL_MCAttControlVector(); - void control(float dt, const math::Dcm &R_nb, float yaw, const math::Vector &F_des, - float Kp, float Kd, float Ki, const math::Vector &angular_rates, - math::Vector &rates_des, float &thrust); - - void set_imax(float integral_max) { - _integral_max(0) = integral_max; - _integral_max(1) = integral_max; - } - - void reset_integral() { - _integral_error(0) = 0.0f; - _integral_error(1) = 0.0f; - } - - void lock_integrator(bool lock) { - _integral_lock = lock; - } - -protected: - math::Vector2f _integral_error; - math::Vector2f _integral_max; - bool _integral_lock; -}; diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp index e0b36067c7..1783efc0e0 100644 --- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -1,7 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Author: Tobias Naegeli + * Lorenz Meier + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,13 +36,16 @@ /** * @file mc_att_control_vector_main.c - * Implementation of a multicopter attitude controller based on desired thrust vector. + * Implementation of a multicopter attitude controller based on desired rotation matrix. + * + * References + * [1] Daniel Mellinger and Vijay Kumar, "Minimum Snap Trajectory Generation and Control for Quadrotors", + * http://www.seas.upenn.edu/~dmel/mellingerICRA11.pdf * * @author Tobias Naegeli * @author Lorenz Meier + * @author Anton Babushkin * - * Please refer to the library files for the authors and acknowledgements of - * the used control library functions. */ #include @@ -54,7 +59,6 @@ #include #include #include -#include #include #include #include @@ -72,8 +76,6 @@ #include #include -#include "ecl_mc_att_control_vector.h" - /** * Multicopter attitude control app start / stop handling function * @@ -107,7 +109,6 @@ private: int _control_task; /**< task handle for sensor task */ int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ @@ -119,52 +120,27 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_rates_setpoint_s _rates_setpoint; /**< vehicle rates setpoint */ perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - struct { - float yaw_p; - float yaw_i; - float yaw_d; - float yaw_imax; - - float att_p; - float att_i; - float att_d; - float att_imax; - - float att_rate_p; - - float yaw_rate_p; - } _parameters; /**< local copies of interesting parameters */ + math::Matrix _K; /**< diagonal gain matrix for position error */ + math::Matrix _K_rate; /**< diagonal gain matrix for angular rate error */ struct { - param_t yaw_p; - param_t yaw_i; - param_t yaw_d; - param_t yaw_imax; - param_t att_p; - param_t att_i; - param_t att_d; - param_t att_imax; - param_t att_rate_p; - + param_t yaw_p; param_t yaw_rate_p; } _parameter_handles; /**< handles for interesting parameters */ - - ECL_MCAttControlVector _att_control; - /** * Update our local parameter cache. */ @@ -186,11 +162,6 @@ private: */ void vehicle_manual_poll(); - /** - * Check for accel updates. - */ - void vehicle_accel_poll(); - /** * Check for set triplet updates. */ @@ -231,7 +202,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* subscriptions */ _att_sub(-1), - _accel_sub(-1), _control_mode_sub(-1), _params_sub(-1), _manual_sub(-1), @@ -244,19 +214,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false) -{ - _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); - _parameter_handles.yaw_i = param_find("MC_YAWPOS_I"); - _parameter_handles.yaw_d = param_find("MC_YAWPOS_D"); - _parameter_handles.yaw_imax = param_find("MC_YAWPOS_IMAX"); + _setpoint_valid(false), +/* gain matrices */ + _K(3, 3), + _K_rate(3, 3) +{ _parameter_handles.att_p = param_find("MC_ATT_P"); - _parameter_handles.att_i = param_find("MC_ATT_I"); - _parameter_handles.att_d = param_find("MC_ATT_D"); - _parameter_handles.att_imax = param_find("MC_ATT_IMAX"); - _parameter_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _parameter_handles.yaw_p = param_find("MC_YAWPOS_P"); _parameter_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); /* fetch initial parameter values */ @@ -291,29 +257,22 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() int MulticopterAttitudeControl::parameters_update() { - - //param_get(_parameter_handles.tconst, &(_parameters.tconst)); - - param_get(_parameter_handles.yaw_p, &(_parameters.yaw_p)); - param_get(_parameter_handles.yaw_i, &(_parameters.yaw_i)); - param_get(_parameter_handles.yaw_d, &(_parameters.yaw_d)); - param_get(_parameter_handles.yaw_imax, &(_parameters.yaw_imax)); - - param_get(_parameter_handles.att_p, &(_parameters.att_p)); - param_get(_parameter_handles.att_i, &(_parameters.att_i)); - param_get(_parameter_handles.att_d, &(_parameters.att_d)); - param_get(_parameter_handles.att_imax, &(_parameters.att_imax)); - - param_get(_parameter_handles.yaw_rate_p, &(_parameters.yaw_rate_p)); - - param_get(_parameter_handles.att_rate_p, &(_parameters.att_rate_p)); - - /* att control parameters */ - _att_control.set_imax(_parameters.att_imax); - //_att_control.set_tau(_parameters.p_tconst); - // _att_control.set_k_p(math::radians(_parameters.p_p)); - // _att_control.set_k_i(math::radians(_parameters.p_i)); - // _att_control.set_k_d(math::radians(_parameters.p_d)); + float att_p; + float att_rate_p; + float yaw_p; + float yaw_rate_p; + + param_get(_parameter_handles.att_p, &att_p); + param_get(_parameter_handles.att_rate_p, &att_rate_p); + param_get(_parameter_handles.yaw_p, &yaw_p); + param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p); + + _K(0, 0) = att_p; + _K(1, 1) = att_p; + _K(2, 2) = yaw_p; + _K_rate(0, 0) = att_rate_p; + _K_rate(1, 1) = att_rate_p; + _K_rate(2, 2) = yaw_rate_p; return OK; } @@ -346,18 +305,6 @@ MulticopterAttitudeControl::vehicle_manual_poll() } } -void -MulticopterAttitudeControl::vehicle_accel_poll() -{ - /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); - - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - } -} - void MulticopterAttitudeControl::vehicle_setpoint_poll() { @@ -402,7 +349,6 @@ MulticopterAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -418,7 +364,6 @@ MulticopterAttitudeControl::task_main() /* get an initial update for all sensor and status data */ vehicle_setpoint_poll(); - vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); arming_status_poll(); @@ -451,11 +396,10 @@ MulticopterAttitudeControl::task_main() /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ + /* copy the topic to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - /* update parameters from storage */ parameters_update(); } @@ -469,29 +413,20 @@ MulticopterAttitudeControl::task_main() if (deltaT > 1.0f) deltaT = 0.01f; - /* load local copies */ + /* copy attitude topic */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); vehicle_setpoint_poll(); - vehicle_accel_poll(); vehicle_control_mode_poll(); arming_status_poll(); vehicle_manual_poll(); - /* lock integrator until armed */ - if (_arming.armed) { - _att_control.lock_integrator(false); - } else { - _att_control.reset_integral(); - _att_control.lock_integrator(true); - } - - /* decide if in auto or full manual control */ float roll_sp = 0.0f; float pitch_sp = 0.0f; float throttle_sp = 0.0f; float yaw_sp = 0.0f; + /* decide if in auto or manual control */ if (_control_mode.flag_control_manual_enabled) { roll_sp = _manual.roll; pitch_sp = _manual.pitch; @@ -505,51 +440,42 @@ MulticopterAttitudeControl::task_main() throttle_sp = _att_sp.thrust; } - // XXX take rotation matrix directly from att_sp for auto mode - math::Vector3 F_des(roll_sp, pitch_sp, yaw_sp); - - math::Vector3 rates_des; - - math::Dcm R_nb(_att.R); - // XXX actually it's not euler angles rates, but body rates, rename it (?) - math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); + /* construct rotation matrix from euler angles */ + math::EulerAngles euler(roll_sp, pitch_sp, yaw_sp); + math::Dcm R_des(euler); - _att_control.control(deltaT, R_nb, _att.yaw, F_des, - _parameters.att_p, _parameters.att_d, _parameters.att_i, - angular_rates, rates_des, throttle_sp); + /* rotation matrix for current state */ + math::Dcm R(_att.R); + /* current body angular rates */ + math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); - float roll_out = _parameters.att_rate_p * rates_des(0); - float pitch_out = _parameters.att_rate_p * rates_des(1); - float yaw_out = _parameters.yaw_rate_p * rates_des(2); - - _actuators.control[0] = (isfinite(roll_out)) ? roll_out : 0.0f; - _actuators.control[1] = (isfinite(pitch_out)) ? pitch_out : 0.0f; - _actuators.control[2] = (isfinite(yaw_out)) ? yaw_out : 0.0f; + /* orientation error between R and R_des */ + math::Matrix e_R_m = (R_des.transpose() * R - R.transpose() * R_des).transpose() * 0.5f; + math::Vector3 e_R(-e_R_m(1, 2), e_R_m(0, 2), -e_R_m(0, 1)); - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; - - /* - * Lazily publish the rate setpoint (for analysis, the actuators are published below) - * only once available - */ - vehicle_rates_setpoint_s rates_sp; - rates_sp.roll = rates_des(0); - rates_sp.pitch = rates_des(1); - rates_sp.yaw = rates_des(2); + /* angular rates setpoint*/ + math::Vector3 rates_sp = K_p * e_R; + math::Vector3 control = K_r_p * (rates_sp - rates); - rates_sp.timestamp = hrt_absolute_time(); + /* publish the attitude rates setpoint */ + _rates_setpoint.roll = rates_sp(0); + _rates_setpoint.pitch = rates_sp(1); + _rates_setpoint.yaw = rates_sp(2); + _rates_setpoint.thrust = throttle_sp; + _rates_setpoint.timestamp = hrt_absolute_time(); if (_rate_sp_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), _actuators_0_pub, &_rates_setpoint); } else { - /* advertise and publish */ - _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_setpoint); } - /* lazily publish the setpoint only once available */ + /* publish the attitude controls */ + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { @@ -566,7 +492,7 @@ MulticopterAttitudeControl::task_main() perf_end(_loop_perf); } - warnx("exiting.\n"); + warnx("exit"); _control_task = -1; _exit(0); diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control_vector/module.mk index 61eb3b3fdf..8d380d719f 100644 --- a/src/modules/mc_att_control_vector/module.mk +++ b/src/modules/mc_att_control_vector/module.mk @@ -37,5 +37,4 @@ MODULE_COMMAND = mc_att_control_vector -SRCS = mc_att_control_vector_main.cpp \ - ecl_mc_att_control_vector.cpp +SRCS = mc_att_control_vector_main.cpp