From f86c0ed892c0b63af11d23e860c815d1c087ff8e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 8 Jan 2015 09:17:22 +0100 Subject: remove fw_att_control base classes: as long as they are not integrated into fw_att_control_main they are useless --- src/modules/fw_att_control/fw_att_control_base.cpp | 325 --------------------- src/modules/fw_att_control/fw_att_control_base.h | 151 ---------- 2 files changed, 476 deletions(-) delete mode 100644 src/modules/fw_att_control/fw_att_control_base.cpp delete mode 100644 src/modules/fw_att_control/fw_att_control_base.h (limited to 'src/modules') diff --git a/src/modules/fw_att_control/fw_att_control_base.cpp b/src/modules/fw_att_control/fw_att_control_base.cpp deleted file mode 100644 index f543c02f9..000000000 --- a/src/modules/fw_att_control/fw_att_control_base.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/* 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 - * - */ - -#include "fw_att_control_base.h" -#include -#include -#include - -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 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 deleted file mode 100644 index bde1b755f..000000000 --- a/src/modules/fw_att_control/fw_att_control_base.h +++ /dev/null @@ -1,151 +0,0 @@ -#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 - * - */ - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -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 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_ */ -- cgit v1.2.3