diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-02 16:52:38 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-02 16:52:38 +0100 |
commit | ad499a594447d807b3702c64fdfaad51ac579e2b (patch) | |
tree | 73c3a2b96b488a246407f9a377b6bac5830965c4 /src/modules/fw_att_control | |
parent | 05a87a706a122b8a83becaaa94c70161fb69c82a (diff) | |
parent | efe0938e9990225636e5fed78e945ded5f24220f (diff) | |
download | px4-firmware-ad499a594447d807b3702c64fdfaad51ac579e2b.tar.gz px4-firmware-ad499a594447d807b3702c64fdfaad51ac579e2b.tar.bz2 px4-firmware-ad499a594447d807b3702c64fdfaad51ac579e2b.zip |
Merge remote-tracking branch 'upstream/ROS_shared_lib_base_class' into dev_ros_rossharedlib
Conflicts:
src/modules/uORB/topics/vehicle_attitude.h
src/platforms/px4_defines.h
Diffstat (limited to 'src/modules/fw_att_control')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_base.cpp | 326 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_base.h | 151 |
2 files changed, 477 insertions, 0 deletions
diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp new file mode 100644 index 000000000..46fef3e67 --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.cpp @@ -0,0 +1,326 @@ +/* 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.cpp + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include "fw_att_control_base.h" +#include <math.h> +#include <mathlib/mathlib.h> +#include <drivers/drv_hrt.h> +#include <systemlib/err.h> + +using namespace std; + +FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() : + + _task_should_exit(false), _task_running(false), _control_task(-1), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf( + perf_alloc(PC_COUNT, "fw att control nonfinite output")), + /* states */ + _setpoint_valid(false), _debug(false) { + /* safely initialize structs */ + _att = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; + _vehicle_status = {}; + +} + +FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() { + +} + +void FixedwingAttitudeControlBase::control_attitude() { + bool lock_integrator = false; + static int loop_counter = 0; + /* scale around tuning airspeed */ + + float airspeed; + + /* if airspeed is not updating, we assume the normal average speed */ + if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) + || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { + airspeed = _parameters.airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); + } + + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim + / ((airspeed < _parameters.airspeed_min) ? + _parameters.airspeed_min : airspeed); + + float roll_sp = _parameters.rollsp_offset_rad; + float pitch_sp = _parameters.pitchsp_offset_rad; + float throttle_sp = 0.0f; + + if (_vcontrol_mode.flag_control_velocity_enabled + || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ + roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; + pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; + throttle_sp = _att_sp.thrust; + + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) { + _roll_ctrl.reset_integrator(); + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } + } else { + /* + * Scale down roll and pitch as the setpoints are radians + * and a typical remote can only do around 45 degrees, the mapping is + * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch) + * + * With this mapping the stick angle is a 1:1 representation of + * the commanded attitude. + * + * The trim gets subtracted here from the manual setpoint to get + * the intended attitude setpoint. Later, after the rate control step the + * trim is added again to the control signal. + */ + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + + _parameters.rollsp_offset_rad; + pitch_sp = -(_manual.x * _parameters.man_pitch_max + - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; + _actuators.control[4] = _manual.flaps; + + /* + * in manual mode no external source should / does emit attitude setpoints. + * emit the manual setpoint here to allow attitude controller tuning + * in attitude control mode. + */ + struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = roll_sp; + att_sp.pitch_body = pitch_sp; + att_sp.yaw_body = 0.0f - _parameters.trim_yaw; + att_sp.thrust = throttle_sp; + + } + + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + + /* Prepare speed_body_u and speed_body_w */ + float speed_body_u = 0.0f; + float speed_body_v = 0.0f; + float speed_body_w = 0.0f; + if (_att.R_valid) { + speed_body_u = _att.R[0][0] * _global_pos.vel_n + + _att.R[1][0] * _global_pos.vel_e + + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + + _att.R[1][1] * _global_pos.vel_e + + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + + _att.R[1][2] * _global_pos.vel_e + + _att.R[2][2] * _global_pos.vel_d; + } else { + if (_debug && loop_counter % 10 == 0) { + warnx("Did not get a valid R\n"); + } + } + + /* Run attitude controllers */ + if (isfinite(roll_sp) && isfinite(pitch_sp)) { + _roll_ctrl.control_attitude(roll_sp, _att.roll); + _pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed); + _yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u, + speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(), + _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude + + /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ + float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed, + _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[0] = + (isfinite(roll_u)) ? + roll_u + _parameters.trim_roll : _parameters.trim_roll; + if (!isfinite(roll_u)) { + _roll_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + + if (_debug && loop_counter % 10 == 0) { + warnx("roll_u %.4f", (double) roll_u); + } + } + + float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[1] = + (isfinite(pitch_u)) ? + pitch_u + _parameters.trim_pitch : + _parameters.trim_pitch; + if (!isfinite(pitch_u)) { + _pitch_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," + " airspeed %.4f, airspeed_scaling %.4f," + " roll_sp %.4f, pitch_sp %.4f," + " _roll_ctrl.get_desired_rate() %.4f," + " _pitch_ctrl.get_desired_rate() %.4f" + " att_sp.roll_body %.4f", (double) pitch_u, + (double) _yaw_ctrl.get_desired_rate(), + (double) airspeed, (double) airspeed_scaling, + (double) roll_sp, (double) pitch_sp, + (double) _roll_ctrl.get_desired_rate(), + (double) _pitch_ctrl.get_desired_rate(), + (double) _att_sp.roll_body); + } + } + + float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, + _att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(), + _parameters.airspeed_min, _parameters.airspeed_max, airspeed, + airspeed_scaling, lock_integrator); + _actuators.control[2] = + (isfinite(yaw_u)) ? + yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + if (!isfinite(yaw_u)) { + _yaw_ctrl.reset_integrator(); + perf_count(_nonfinite_output_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("yaw_u %.4f", (double) yaw_u); + } + } + + /* throttle passed through */ + _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + if (!isfinite(throttle_sp)) { + if (_debug && loop_counter % 10 == 0) { + warnx("throttle_sp %.4f", (double) throttle_sp); + } + } + } else { + perf_count(_nonfinite_input_perf); + if (_debug && loop_counter % 10 == 0) { + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", + (double) roll_sp, (double) pitch_sp); + } + } + +} + +void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) { + // watch out, still need to see where we modify attitude for the tailsitter case + math::Quaternion quat; + quat(0) = (float)attitude.w(); + quat(1) = (float)attitude.x(); + quat(2) = (float)attitude.y(); + quat(3) = (float)attitude.z(); + + _att.q[0] = quat(0); + _att.q[1] = quat(1); + _att.q[2] = quat(2); + _att.q[3] = quat(3); + + math::Matrix<3,3> Rot = quat.to_dcm(); + _att.R[0][0] = Rot(0,0); + _att.R[1][0] = Rot(1,0); + _att.R[2][0] = Rot(2,0); + _att.R[0][1] = Rot(0,1); + _att.R[1][1] = Rot(1,1); + _att.R[2][1] = Rot(2,1); + _att.R[0][2] = Rot(0,2); + _att.R[1][2] = Rot(1,2); + _att.R[2][2] = Rot(2,2); + + _att.R_valid = true; +} +void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) { + _att.rollspeed = angular_rate(0); + _att.pitchspeed = angular_rate(1); + _att.yawspeed = angular_rate(2); +} +void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) { + _att_sp.roll_body = control_attitude_thrust_reference(0); + _att_sp.pitch_body = control_attitude_thrust_reference(1); + _att_sp.yaw_body = control_attitude_thrust_reference(2); + _att_sp.thrust = (control_attitude_thrust_reference(3) -30)*(-1)/30; + + // setup rotation matrix + math::Matrix<3,3> Rot_sp; + Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + _att_sp.R_body[0][0] = Rot_sp(0,0); + _att_sp.R_body[1][0] = Rot_sp(1,0); + _att_sp.R_body[2][0] = Rot_sp(2,0); + _att_sp.R_body[0][1] = Rot_sp(0,1); + _att_sp.R_body[1][1] = Rot_sp(1,1); + _att_sp.R_body[2][1] = Rot_sp(2,1); + _att_sp.R_body[0][2] = Rot_sp(0,2); + _att_sp.R_body[1][2] = Rot_sp(1,2); + _att_sp.R_body[2][2] = Rot_sp(2,2); +} +void FixedwingAttitudeControlBase::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]; +} diff --git a/src/modules/fw_att_control/fw_att_control_base.h b/src/modules/fw_att_control/fw_att_control_base.h new file mode 100644 index 000000000..bde1b755f --- /dev/null +++ b/src/modules/fw_att_control/fw_att_control_base.h @@ -0,0 +1,151 @@ +#ifndef FW_ATT_CONTROL_BASE_H_ +#define FW_ATT_CONTROL_BASE_H_ + +/* 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 fw_att_control_base.h + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ + +#include <ecl/attitude_fw/ecl_pitch_controller.h> +#include <ecl/attitude_fw/ecl_roll_controller.h> +#include <ecl/attitude_fw/ecl_yaw_controller.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_control_mode.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_status.h> + +#include <systemlib/perf_counter.h> + +class FixedwingAttitudeControlBase +{ +public: + /** + * Constructor + */ + FixedwingAttitudeControlBase(); + + /** + * Destructor + */ + ~FixedwingAttitudeControlBase(); + + +protected: + + bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_running; /**< if true, task is running in its mainloop */ + int _control_task; /**< task handle for sensor task */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + 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_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ + + bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ + + struct { + float tconst; + float p_p; + float p_d; + float p_i; + float p_ff; + float p_rmax_pos; + float p_rmax_neg; + float p_integrator_max; + float p_roll_feedforward; + float r_p; + float r_d; + float r_i; + float r_ff; + float r_integrator_max; + float r_rmax; + float y_p; + float y_i; + float y_d; + float y_ff; + float y_roll_feedforward; + float y_integrator_max; + float y_coordinated_min_speed; + float y_rmax; + + float airspeed_min; + float airspeed_trim; + float airspeed_max; + + float trim_roll; + float trim_pitch; + float trim_yaw; + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + } _parameters; /**< local copies of interesting parameters */ + + ECL_RollController _roll_ctrl; + ECL_PitchController _pitch_ctrl; + ECL_YawController _yaw_ctrl; + + void control_attitude(); + + // setters and getters for interface with euroc-gazebo simulator + void set_attitude(const Eigen::Quaternion<double> 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); + +}; + +#endif /* FW_ATT_CONTROL_BASE_H_ */ |