diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-11 16:11:04 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-11 16:11:04 +0100 |
commit | 9c09a9e8d56b69797b64e5fa1f17d2bcf760e2bb (patch) | |
tree | 9fdf579957abce2489e8226d5d334048006fb075 /src/modules/mc_att_control/mc_att_control.h | |
parent | 12df129da297e4d4c36d2ed0f7f3571b5fbf7e29 (diff) | |
download | px4-firmware-9c09a9e8d56b69797b64e5fa1f17d2bcf760e2bb.tar.gz px4-firmware-9c09a9e8d56b69797b64e5fa1f17d2bcf760e2bb.tar.bz2 px4-firmware-9c09a9e8d56b69797b64e5fa1f17d2bcf760e2bb.zip |
mc att control: prepare for ros integration, move class into spearate file
Diffstat (limited to 'src/modules/mc_att_control/mc_att_control.h')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control.h | 174 |
1 files changed, 174 insertions, 0 deletions
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h new file mode 100644 index 000000000..48df88771 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control.h @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * 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.h + * Multicopter attitude controller. + * + * @author Tobias Naegeli <naegelit@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Julian Oes <julian@oes.ch> + * @author Roman Bapst <bapstr@ethz.ch> + * + * 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 <px4.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <systemlib/perf_counter.h> +#include <systemlib/systemlib.h> +#include <systemlib/circuit_breaker.h> +#include <lib/mathlib/mathlib.h> +#include <lib/geo/geo.h> + +#include "mc_att_control_base.h" + +class MulticopterAttitudeControl : + public MulticopterAttitudeControlBase +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + void handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg); + + void spin() { n.spin(); } + +private: + 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 */ + + + int _v_att_sub; /**< vehicle attitude subscription */ + int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */ + int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */ + int _v_control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< parameter updates subscription */ + int _manual_control_sp_sub; /**< manual control setpoint subscription */ + int _armed_sub; /**< arming status subscription */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + + px4::NodeHandle n; + + struct { + param_t roll_p; + param_t roll_rate_p; + param_t roll_rate_i; + param_t roll_rate_d; + param_t pitch_p; + param_t pitch_rate_p; + param_t pitch_rate_i; + param_t pitch_rate_d; + param_t yaw_p; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + param_t yaw_ff; + param_t yaw_rate_max; + + param_t man_roll_max; + param_t man_pitch_max; + param_t man_yaw_max; + param_t acro_roll_max; + param_t acro_pitch_max; + param_t acro_yaw_max; + } _params_handles; /**< handles for interesting parameters */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Check for parameter update and handle it. + */ + void parameter_update_poll(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for attitude setpoint updates. + */ + void vehicle_attitude_setpoint_poll(); + + /** + * Check for rates setpoint updates. + */ + void vehicle_rates_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + +}; + |