remove fw_att_control base classes: as long as they are not integrated into fw_att_control_main they are useless

2 files changed, 0 insertions, 476 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
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 <bapstr@ethz.ch>
- *
- */
-
-#include "fw_att_control_base.h"
-#include <math.h>
-#include <mathlib/mathlib.h>
-#include <drivers/drv_hrt.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
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 <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_ */