px4_autopilot/apps/controllib/fixedwing.hpp

/****************************************************************************
 *
 *   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.h
 *
 * Controller library code
 */

#pragma once

#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <drivers/drv_hrt.h>
#include <poll.h>

#include "blocks.hpp"
#include "block/UOrbSubscription.hpp"
#include "block/UOrbPublication.hpp"

extern "C" {
#include <systemlib/geo/geo.h>
}

namespace control
{

namespace fixedwing
{

/**
 * BlockYawDamper
 *
 * This block has more explations to help new developers
 * add their own blocks. It includes a limited explanation
 * of some C++ basics.
 *
 * Block: The generic class describing a typical block as you
 * would expect in Simulink or ScicosLab. A block can have
 * parameters. It cannot have other blocks.
 *
 * SuperBlock: A superblock is a block that can have other
 * blocks. It has methods that manage the blocks beneath it.
 *
 * BlockYawDamper inherits from SuperBlock publically, this
 * means that any public function in SuperBlock are public within
 * BlockYawDamper and may be called from outside the
 * class methods. Any protected function within block
 * are private to the class and may not be called from
 * outside this class. Protected should be preferred
 * where possible to public as it is important to
 * limit access to the bare minimum to prevent
 * accidental errors.
 */
class __EXPORT BlockYawDamper : public SuperBlock
{
private:
	/**
	 * Declaring other blocks used by this block
	 *
	 * In this section we declare all child blocks that
	 * this block is composed of. They are private
	 * members so only this block has direct access to
	 * them.
	 */
	BlockLowPass _rLowPass;
	BlockHighPass _rWashout;
	BlockP _r2Rdr;

	/**
	 * Declaring output values and accessors
	 *
	 * If we have any output values for the block we
	 * declare them here. Output can be directly returned
	 * through the update function, but outputs should be
	 * declared here if the information will likely be requested
	 * again, or if there are multiple outputs.
	 *
	 * You should only be able to set outputs from this block,
	 * so the outputs are declared in the private section.
	 * A get accessor is provided
	 * in the public section for other blocks to get the
	 * value of the output.
	 */
	float _rudder;
public:
	/**
	 * BlockYawDamper Constructor
	 *
	 * The job of the constructor is to initialize all
	 * parameter in this block and initialize all child
	 * blocks. Note also, that the output values must be
	 * initialized here. The order of the members in the
	 * member initialization list should follow the
	 * order in which they are declared within the class.
	 * See the private member declarations above.
	 *
	 * Block Construction
	 *
	 * All blocks are constructed with their parent block
	 * and their name. This allows parameters within the
	 * block to construct a fully qualified name from
	 * concatenating the two. If the name provided to the
	 * block is "", then the block will use the parent
	 * name as it's name. This is useful in cases where
	 * you have a block that has parameters "MIN", "MAX",
	 * such as BlockLimit and you do not want an extra name
	 * to qualify them since the parent block has no "MIN",
	 * "MAX" parameters.
	 *
	 * Block Parameter Construction
	 *
	 * Block parameters are named constants, they are
	 * constructed using:
	 * BlockParam::BlockParam(Block * parent, const char * name)
	 * This funciton takes both a parent block and a name.
	 * The constructore then uses the parent name and the name of
	 * the paramter to ask the px4 param library if it has any
	 * parameters with this name. If it does, a handle to the
	 * parameter is retrieved.
	 *
	 * Block/ BlockParam Naming
	 *
	 * When desigining new blocks, the naming of the parameters and
	 * blocks determines the fully qualified name of the parameters
	 * within the ground station, so it is important to choose
	 * short, easily understandable names. Again, when a name of
	 * "" is passed, the parent block name is used as the value to
	 * prepend to paramters names.
	 */
	BlockYawDamper(SuperBlock *parent, const char *name);
	/**
	 * Block deconstructor
	 *
	 * It is always a good idea to declare a virtual
	 * deconstructor so that upon calling delete from
	 * a class derived from this, all of the
	 * deconstructors all called, the derived class first, and
	 * then the base class
	 */
	virtual ~BlockYawDamper();

	/**
	 * Block update function
	 *
	 * The job of the update function is to compute the output
	 * values for the block. In a simple block with one output,
	 * the output may be returned directly. If the output is
	 * required frequenly by other processses, it might be a
	 * good idea to declare a member to store the temporary
	 * variable.
	 */
	void update(float rCmd, float r);

	/**
	 * Rudder output value accessor
	 *
	 * This is a public accessor function, which means that the
	 * private value _rudder is returned to anyone calling
	 * BlockYawDamper::getRudder(). Note thate a setRudder() is
	 * not provided, this is because the updateParams() call
	 * for a block provides the mechanism for updating the
	 * paramter.
	 */
	float getRudder() { return _rudder; }
};

/**
 * Stability augmentation system.
 * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
 */
class __EXPORT BlockStabilization : public SuperBlock
{
private:
	BlockYawDamper _yawDamper;
	BlockLowPass _pLowPass;
	BlockLowPass _qLowPass;
	BlockP _p2Ail;
	BlockP _q2Elv;
	float _aileron;
	float _elevator;
public:
	BlockStabilization(SuperBlock *parent, const char *name);
	virtual ~BlockStabilization();
	void update(float pCmd, float qCmd, float rCmd,
		    float p, float q, float r);
	float getAileron() { return _aileron; }
	float getElevator() { return _elevator; }
	float getRudder() { return _yawDamper.getRudder(); }
};

/**
 * Heading hold autopilot block.
 * Aircraft Control and Simulation, Stevens and Lewis
 * Heading hold, pg. 348
 */
class __EXPORT BlockHeadingHold : public SuperBlock
{
private:
	BlockP _psi2Phi;
	BlockP _phi2P;
	BlockLimitSym _phiLimit;
public:
	BlockHeadingHold(SuperBlock *parent, const char *name);
	virtual ~BlockHeadingHold();
	/**
	 * returns pCmd
	 */
	float update(float psiCmd, float phi, float psi, float p);
};

/**
 * Frontside/ Backside Control Systems
 *
 * Frontside :
 *   velocity error -> throttle
 *   altitude error -> elevator
 *
 * Backside :
 *   velocity error -> elevator
 *   altitude error -> throttle
 *
 * Backside control systems are more resilient at
 * slow speeds on the back-side of the power
 * required curve/ landing etc. Less performance
 * than frontside at high speeds.
 */

/**
 * Backside velocity hold autopilot block.
 * v -> theta -> q -> elevator
 */
class __EXPORT BlockVelocityHoldBackside : public SuperBlock
{
private:
	BlockPID _v2Theta;
	BlockPID _theta2Q;
	BlockLimit _theLimit;
	BlockLimit _vLimit;
public:
	BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
	virtual ~BlockVelocityHoldBackside();
	/**
	 * returns qCmd
	 */
	float update(float vCmd, float v, float theta, float q);
};

/**
 * Frontside velocity hold autopilot block.
 * v -> throttle
 */
class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
{
private:
	BlockPID _v2Thr;
public:
	BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
	virtual ~BlockVelocityHoldFrontside();
	/**
	 * returns throttle
	 */
	float update(float vCmd, float v);
};

/**
 * Backside altitude hold autopilot block.
 * h -> throttle
 */
class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
{
private:
	BlockPID _h2Thr;
	float _throttle;
public:
	BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
	virtual ~BlockAltitudeHoldBackside();
	void update(float hCmd, float h);
	float getThrottle() { return _throttle; }
};

/**
 * Frontside altitude hold autopilot block.
 * h -> theta > q -> elevator
 */
class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
{
private:
	BlockPID _h2Theta;
	BlockPID _theta2Q;
public:
	BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
	virtual ~BlockAltitudeHoldFrontside();
	/**
	 * return qCmd
	 */
	float update(float hCmd, float h, float theta, float q);
};

/**
 * Backside autopilot
 */
class __EXPORT BlockBacksideAutopilot : public SuperBlock
{
private:
	BlockStabilization *_stabilization;
	BlockHeadingHold _headingHold;
	BlockVelocityHoldBackside _velocityHold;
	BlockAltitudeHoldBackside _altitudeHold;
	BlockParam<float> _trimAil;
	BlockParam<float> _trimElv;
	BlockParam<float> _trimRdr;
	BlockParam<float> _trimThr;
public:
	BlockBacksideAutopilot(SuperBlock *parent,
			       const char *name,
			       BlockStabilization *stabilization);
	virtual ~BlockBacksideAutopilot();
	void 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);
	float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
	float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
	float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
	float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
};

/**
 * Waypoint Guidance block
 */
class __EXPORT BlockWaypointGuidance : public SuperBlock
{
private:
	BlockLimitSym _xtYawLimit;
	BlockP _xt2Yaw;
	float _psiCmd;
public:
	BlockWaypointGuidance(SuperBlock *parent, const char *name);
	virtual ~BlockWaypointGuidance();
	void update(vehicle_global_position_s &pos,
		    vehicle_attitude_s &att,
		    vehicle_global_position_setpoint_s &posCmd,
		    vehicle_global_position_setpoint_s &lastPosCmd);
	float getPsiCmd() { return _psiCmd; }
};

/**
 * UorbEnabledAutopilot
 */
class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
	// subscriptions
	UOrbSubscription<vehicle_attitude_s> _att;
	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
	UOrbSubscription<vehicle_global_position_s> _pos;
	UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
	UOrbSubscription<manual_control_setpoint_s> _manual;
	UOrbSubscription<vehicle_status_s> _status;
	UOrbSubscription<parameter_update_s> _param_update;
	// publications
	UOrbPublication<actuator_controls_s> _actuators;
public:
	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
	virtual ~BlockUorbEnabledAutopilot();
};

/**
 * Multi-mode Autopilot
 */
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
{
private:
	BlockStabilization _stabilization;
	BlockBacksideAutopilot _backsideAutopilot;
	BlockWaypointGuidance _guide;
	BlockParam<float> _vCmd;

	struct pollfd _attPoll;
	vehicle_global_position_setpoint_s _lastPosCmd;
	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
	uint64_t _timeStamp;
public:
	BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
	void update();
	virtual ~BlockMultiModeBacksideAutopilot();
};


} // namespace fixedwing

} // namespace control