diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-11 23:12:06 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-11 23:12:06 +0200 |
commit | feddb618c492c115933c8714ed158325af940203 (patch) | |
tree | 7044cb3f73d35b58805b19e286cd741ac402ae8f /src/modules | |
parent | 7a6a8ea53a94a71138e3cedcb0e3733ca15e0f10 (diff) | |
download | px4-firmware-feddb618c492c115933c8714ed158325af940203.tar.gz px4-firmware-feddb618c492c115933c8714ed158325af940203.tar.bz2 px4-firmware-feddb618c492c115933c8714ed158325af940203.zip |
Added rotation matrix based vector controller
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mathlib/math/Vector3.cpp | 10 | ||||
-rw-r--r-- | src/modules/mathlib/math/Vector3.hpp | 2 | ||||
-rw-r--r-- | src/modules/mathlib/math/arm/Vector.hpp | 2 | ||||
-rw-r--r-- | src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp | 146 | ||||
-rw-r--r-- | src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h | 60 | ||||
-rw-r--r-- | src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp | 692 | ||||
-rw-r--r-- | src/modules/mc_att_control_vector/module.mk | 41 |
7 files changed, 952 insertions, 1 deletions
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp index dcb85600e..3936650c6 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/modules/mathlib/math/Vector3.cpp @@ -84,6 +84,16 @@ Vector3 Vector3::cross(const Vector3 &b) const return result; } +Vector3 Vector3::operator %(const Vector3 &v) const +{ + return cross(v); +} + +float Vector3::operator *(const Vector3 &v) const +{ + return dot(v); +} + int __EXPORT vector3Test() { printf("Test Vector3\t\t: "); diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp index 568d9669a..2bf00f26b 100644 --- a/src/modules/mathlib/math/Vector3.hpp +++ b/src/modules/mathlib/math/Vector3.hpp @@ -54,6 +54,8 @@ public: Vector3(const float *data); virtual ~Vector3(); Vector3 cross(const Vector3 &b) const; + Vector3 operator %(const Vector3 &v) const; + float operator *(const Vector3 &v) const; /** * accessors diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp index 4155800e8..0f7722143 100644 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ b/src/modules/mathlib/math/arm/Vector.hpp @@ -141,7 +141,7 @@ public: getRows()); return result; } - inline Vector operator*(float right) const { + inline Vector operator*(const float &right) const { Vector result(getRows()); arm_scale_f32((float *)getData(), right, result.getData(), 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 new file mode 100644 index 000000000..b21b0cf67 --- /dev/null +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.cpp @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * 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 <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include <mathlib/mathlib.h> +#include "ecl_mc_att_control_vector.h" + +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) +{ + //function [rates_des,Thrust,ie_out] = attitudeController(R1,yaw,F_des,Kp,Kd,Ki,e_Rd_v,ie) + //% dt + //dt=1/50; + + //% shoult the controll be in the body frame or in the inertial frame: 0 is + //% body frame, 1 is inertial frame. + + // XXX + bool earth = true; + bool integral_freeze = false; + 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); + //%F_des=RYaw*F_des; + 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 + 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_freeze) { + 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 new file mode 100644 index 000000000..98689f0be --- /dev/null +++ b/src/modules/mc_att_control_vector/ecl_mc_att_control_vector.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + */ + +#include <mathlib/mathlib.h> + +class ECL_MCAttControlVector { + +public: + 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); + +protected: + math::Vector2 _integral_error; + math::Vector2 _integral_max; +}; 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 new file mode 100644 index 000000000..f3f8235f0 --- /dev/null +++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp @@ -0,0 +1,692 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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_vector_main.c + * Implementation of a multicopter attitude controller based on desired thrust vector. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Please refer to the library files for the authors and acknowledgements of + * the used control library functions. + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/airspeed.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/pid/pid.h> +#include <systemlib/geo/geo.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <mathlib/mathlib.h> + +//#include <ecl/attitude_mc/ecl_mc_att_control_vector.h> +#include "ecl_mc_att_control_vector.h" + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_att_control_vector_main(int argc, char *argv[]); + +class MulticopterAttitudeControl +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + 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 _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + + orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ + 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 airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_armed_s _arming; /**< actuator arming status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ + + // ECL_L1_Pos_Control _att_control; + + 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 */ + + 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_rate_p; + } _parameter_handles; /**< handles for interesting parameters */ + + + ECL_MCAttControlVector _att_control; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + + /** + * Check for airspeed updates. + */ + bool vehicle_airspeed_poll(); + + /** + * Check for accel updates. + */ + void vehicle_accel_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterAttitudeControl *g_control; +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _accel_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + +/* publications */ + _rate_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), +/* states */ + _setpoint_valid(false), + _airspeed_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"); + + _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_rate_p = param_find("MC_YAWRATE_P"); + + /* fetch initial parameter values */ + parameters_update(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + if (_control_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +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)); + + /* class control parameters */ + // _att_ctrl.set_tau(_parameters.p_tconst); + // _att_ctrl.set_k_p(math::radians(_parameters.p_p)); + // _att_ctrl.set_k_i(math::radians(_parameters.p_i)); + // _att_ctrl.set_k_d(math::radians(_parameters.p_d)); + + return OK; +} + +void +MulticopterAttitudeControl::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +bool +MulticopterAttitudeControl::vehicle_airspeed_poll() +{ + /* check if there is a new position */ + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + return true; + } + + return false; +} + +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() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + _setpoint_valid = true; + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool arming_updated; + orb_check(_arming_sub, &arming_updated); + + if (arming_updated) { + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } +} + +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +MulticopterAttitudeControl::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _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)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_att_sub, 100); + + parameters_update(); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + (void)vehicle_airspeed_poll(); + vehicle_setpoint_poll(); + vehicle_accel_poll(); + vehicle_status_poll(); + vehicle_manual_poll(); + arming_status_poll(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + _airspeed_valid = vehicle_airspeed_poll(); + + vehicle_setpoint_poll(); + + vehicle_accel_poll(); + + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + + /* check for arming status changes */ + arming_status_poll(); + + vehicle_manual_poll(); + + /* lock integrator until armed */ + bool lock_integrator; + if (_arming.armed) { + lock_integrator = false; + } else { + lock_integrator = true; + } + + /* decide if in auto or full manual control */ + float roll_sp, pitch_sp; + float throttle_sp = 0.0f; + float yaw_sp = 0.0f; + + if (_vstatus.state_machine == SYSTEM_STATE_MANUAL || + (_vstatus.state_machine == SYSTEM_STATE_STABILIZED)) { + + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do 45 degrees, the mapping is + * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians. + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. If more than 45 degrees are desired, + * a scaling parameter can be applied to the remote. + */ + roll_sp = _manual.roll * 0.75f; + pitch_sp = _manual.pitch * 0.75f; + yaw_sp = _manual.yaw; + throttle_sp = _manual.throttle; + + } else if (_vstatus.state_machine == SYSTEM_STATE_AUTO) { + + roll_sp = _att_sp.roll_body; + pitch_sp = _att_sp.pitch_body; + yaw_sp = _att_sp.yaw_body; + 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); + math::Vector3 angular_rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _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); + + 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; + + /* 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); + + rates_sp.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); + + } else { + /* advertise and publish */ + _rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + + /* lazily publish the setpoint only once available */ + _actuators.timestamp = hrt_absolute_time(); + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_att_control_vector", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_att_control_vector_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_att_control_vector {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new MulticopterAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/mc_att_control_vector/module.mk b/src/modules/mc_att_control_vector/module.mk new file mode 100644 index 000000000..61eb3b3fd --- /dev/null +++ b/src/modules/mc_att_control_vector/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Multirotor attitude controller (vector based, no Euler singularities) +# + +MODULE_COMMAND = mc_att_control_vector + +SRCS = mc_att_control_vector_main.cpp \ + ecl_mc_att_control_vector.cpp |