diff options
Diffstat (limited to 'apps/systemlib/control/fixedwing.hpp')
| -rw-r--r-- | apps/systemlib/control/fixedwing.hpp | 438 |
1 files changed, 438 insertions, 0 deletions
diff --git a/apps/systemlib/control/fixedwing.hpp b/apps/systemlib/control/fixedwing.hpp new file mode 100644 index 000000000..ea742641c --- /dev/null +++ b/apps/systemlib/control/fixedwing.hpp @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * 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 + |