mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-17 11:53:34 +08:00
Merge James's controllib bits into a separate library module.
Add a top-level mathlib header to avoid having to dig around for specific headers.
This commit is contained in:
px4dev
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user