Added control library.

1 files changed, 351 insertions, 0 deletions

diff --git a/apps/systemlib/control/fixedwing.cpp b/apps/systemlib/control/fixedwing.cpp
new file mode 100644
index 000000000..695653714
--- /dev/null
+++ b/apps/systemlib/control/fixedwing.cpp
@@ -0,0 +1,351 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 fixedwing.cpp
+ *
+ * Controller library code
+ */
+
+#include "fixedwing.hpp"
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_rLowPass(this, "R_LP"),
+	_rWashout(this, "R_HP"),
+	_r2Rdr(this, "R2RDR"),
+	_rudder(0)
+{
+}
+
+BlockYawDamper::~BlockYawDamper() {};
+
+void BlockYawDamper::update(float rCmd, float r)
+{
+	_rudder = _r2Rdr.update(rCmd -
+				_rWashout.update(_rLowPass.update(r)));
+}
+
+BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_yawDamper(this, ""),
+	_pLowPass(this, "P_LP"),
+	_qLowPass(this, "Q_LP"),
+	_p2Ail(this, "P2AIL"),
+	_q2Elv(this, "Q2ELV"),
+	_aileron(0),
+	_elevator(0)
+{
+}
+
+BlockStabilization::~BlockStabilization() {};
+
+void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
+				float p, float q, float r)
+{
+	_aileron = _p2Ail.update(
+			   pCmd - _pLowPass.update(p));
+	_elevator = _q2Elv.update(
+			    qCmd - _qLowPass.update(q));
+	_yawDamper.update(rCmd, r);
+}
+
+BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM")
+{
+}
+
+BlockHeadingHold::~BlockHeadingHold() {};
+
+float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
+{
+	float psiError = _wrap_pi(psiCmd - psi);
+	float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+	return _phi2P.update(phiCmd - phi);
+}
+
+BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V")
+{
+}
+
+BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
+
+float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
+{
+	// negative sign because nose over to increase speed
+	float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Thr(this, "V2THR")
+{
+}
+
+BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
+
+float BlockVelocityHoldFrontside::update(float vCmd, float v)
+{
+	return _v2Thr.update(vCmd - v);
+}
+
+BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Thr(this, "H2THR"),
+	_throttle(0)
+{
+}
+
+BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
+
+void BlockAltitudeHoldBackside::update(float hCmd, float h)
+{
+	_throttle = _h2Thr.update(hCmd - h);
+}
+
+BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Theta(this, "H2THE"),
+	_theta2Q(this, "THE2Q")
+{
+}
+
+BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
+
+float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
+{
+	float thetaCmd = _h2Theta.update(hCmd - h);
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
+		const char *name,
+		BlockStabilization *stabilization) :
+	SuperBlock(parent, name),
+	_stabilization(stabilization),
+	_headingHold(this, ""),
+	_velocityHold(this, ""),
+	_altitudeHold(this, ""),
+	_trimAil(this, "TRIM_AIL"),
+	_trimElv(this, "TRIM_ELV"),
+	_trimRdr(this, "TRIM_RDR"),
+	_trimThr(this, "TRIM_THR")
+{
+}
+
+BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
+
+void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
+				    float h, float v,
+				    float phi, float theta, float psi,
+				    float p, float q, float r)
+{
+	_altitudeHold.update(hCmd, h);
+	_stabilization->update(
+		_headingHold.update(psiCmd, phi, psi, p),
+		_velocityHold.update(vCmd, v, theta, q),
+		rCmd,
+		p, q, r);
+}
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
+	BlockUorbEnabledAutopilot(parent, name),
+	_stabilization(this, ""), // no name needed, already unique
+	_backsideAutopilot(this, "", &_stabilization),
+	_guide(this, ""),
+	_vCmd(this, "V_CMD"),
+	_attPoll(),
+	_lastPosCmd(),
+	_timeStamp(0)
+{
+	_attPoll.fd = _att.getHandle();
+	_attPoll.events = POLLIN;
+}
+
+void BlockMultiModeBacksideAutopilot::update()
+{
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// store old position command before update if new command sent
+	if (_posCmd.updated()) {
+		_lastPosCmd = _posCmd.getData();
+	}
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_STABILIZED) {
+		_stabilization.update(
+			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+		_actuators.control[CH_AIL] = _stabilization.getAileron();
+		_actuators.control[CH_ELV] = _stabilization.getElevator();
+		_actuators.control[CH_RDR] = _stabilization.getRudder();
+		_actuators.control[CH_THR] = _manual.throttle;
+
+	} else if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+		// commands
+		float rCmd = 0;
+
+		_backsideAutopilot.update(
+			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
+			_pos.alt, v,
+			_att.roll, _att.pitch, _att.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed
+		);
+		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
+		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
+		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
+		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+		_actuators.control[CH_AIL] = _manual.roll;
+		_actuators.control[CH_ELV] = _manual.pitch;
+		_actuators.control[CH_RDR] = _manual.yaw;
+		_actuators.control[CH_THR] = _manual.throttle;
+	}
+
+	// update all publications
+	updatePublications();
+}
+
+BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
+{
+	// send one last publication when destroyed, setting
+	// all output to zero
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	updatePublications();
+}
+
+} // namespace fixedwing
+
+} // namespace control
+