mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 12:57:34 +08:00
Merged master
This commit is contained in:
Lorenz Meier
@@ -0,0 +1,333 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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"
|
||||
|
||||
#if 0
|
||||
|
||||
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, float outputScale)
|
||||
{
|
||||
_rudder = outputScale*_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, float outputScale)
|
||||
{
|
||||
_aileron = outputScale*_p2Ail.update(
|
||||
pCmd - _pLowPass.update(p));
|
||||
_elevator = outputScale*_q2Elv.update(
|
||||
qCmd - _qLowPass.update(q));
|
||||
_yawDamper.update(rCmd, r, outputScale);
|
||||
}
|
||||
|
||||
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
|
||||
BlockUorbEnabledAutopilot(parent, name),
|
||||
_stabilization(this, ""), // no name needed, already unique
|
||||
|
||||
// heading hold
|
||||
_psi2Phi(this, "PSI2PHI"),
|
||||
_phi2P(this, "PHI2P"),
|
||||
_phiLimit(this, "PHI_LIM"),
|
||||
|
||||
// velocity hold
|
||||
_v2Theta(this, "V2THE"),
|
||||
_theta2Q(this, "THE2Q"),
|
||||
_theLimit(this, "THE"),
|
||||
_vLimit(this, "V"),
|
||||
|
||||
// altitude/climb rate hold
|
||||
_h2Thr(this, "H2THR"),
|
||||
_cr2Thr(this, "CR2THR"),
|
||||
|
||||
// guidance block
|
||||
_guide(this, ""),
|
||||
|
||||
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
|
||||
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
|
||||
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
|
||||
_trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
|
||||
_trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
|
||||
|
||||
_vCmd(this, "V_CMD"),
|
||||
_crMax(this, "CR_MAX"),
|
||||
_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;
|
||||
|
||||
#warning this if is incomplete, should be based on flags
|
||||
// only update guidance in auto mode
|
||||
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
|
||||
// update guidance
|
||||
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
|
||||
}
|
||||
|
||||
// XXX handle STABILIZED (loiter on spot) as well
|
||||
// once the system switches from manual or auto to stabilized
|
||||
// the setpoint should update to loitering around this position
|
||||
|
||||
#warning should be base on flags
|
||||
// handle autopilot modes
|
||||
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
|
||||
_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
|
||||
|
||||
// update guidance
|
||||
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
|
||||
|
||||
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
||||
// for the purpose of control we will limit the velocity feedback between
|
||||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vx * _pos.vx +
|
||||
_pos.vy * _pos.vy +
|
||||
_pos.vz * _pos.vz));
|
||||
|
||||
// limit velocity command between min/max velocity
|
||||
float vCmd = _vLimit.update(_vCmd.get());
|
||||
|
||||
// altitude hold
|
||||
float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
|
||||
|
||||
// heading hold
|
||||
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
|
||||
float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
|
||||
float pCmd = _phi2P.update(phiCmd - _att.roll);
|
||||
|
||||
// velocity hold
|
||||
// negative sign because nose over to increase speed
|
||||
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
|
||||
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
|
||||
|
||||
// yaw rate cmd
|
||||
float rCmd = 0;
|
||||
|
||||
// stabilization
|
||||
float velocityRatio = _trimV.get()/v;
|
||||
float outputScale = velocityRatio*velocityRatio;
|
||||
// this term scales the output based on the dynamic pressure change from trim
|
||||
_stabilization.update(pCmd, qCmd, rCmd,
|
||||
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
|
||||
outputScale);
|
||||
|
||||
// output
|
||||
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
|
||||
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
|
||||
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
|
||||
_actuators.control[CH_THR] = dThrottle + _trimThr.get();
|
||||
|
||||
// XXX limit throttle to manual setting (safety) for now.
|
||||
// If it turns out to be confusing, it can be removed later once
|
||||
// a first binary release can be targeted.
|
||||
// This is not a hack, but a design choice.
|
||||
|
||||
/* do not limit in HIL */
|
||||
#warning fix this
|
||||
// if (!_status.flag_hil_enabled) {
|
||||
// /* limit to value of manual throttle */
|
||||
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
|
||||
// _actuators.control[CH_THR] : _manual.throttle;
|
||||
// }
|
||||
|
||||
} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
|
||||
|
||||
#warning fix here too
|
||||
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||
// _actuators.control[CH_AIL] = _manual.roll;
|
||||
// _actuators.control[CH_ELV] = _manual.pitch;
|
||||
// _actuators.control[CH_RDR] = _manual.yaw;
|
||||
// _actuators.control[CH_THR] = _manual.throttle;
|
||||
//
|
||||
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
||||
|
||||
// calculate velocity, XXX should be airspeed, but using ground speed for now
|
||||
// for the purpose of control we will limit the velocity feedback between
|
||||
// the min/max velocity
|
||||
float v = _vLimit.update(sqrtf(
|
||||
_pos.vx * _pos.vx +
|
||||
_pos.vy * _pos.vy +
|
||||
_pos.vz * _pos.vz));
|
||||
|
||||
// pitch channel -> rate of climb
|
||||
// TODO, might want to put a gain on this, otherwise commanding
|
||||
// from +1 -> -1 m/s for rate of climb
|
||||
//float dThrottle = _cr2Thr.update(
|
||||
//_crMax.get()*_manual.pitch - _pos.vz);
|
||||
|
||||
// roll channel -> bank angle
|
||||
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
|
||||
float pCmd = _phi2P.update(phiCmd - _att.roll);
|
||||
|
||||
// throttle channel -> velocity
|
||||
// negative sign because nose over to increase speed
|
||||
float vCmd = _vLimit.update(_manual.throttle *
|
||||
(_vLimit.getMax() - _vLimit.getMin()) +
|
||||
_vLimit.getMin());
|
||||
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
|
||||
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
|
||||
|
||||
// yaw rate cmd
|
||||
float rCmd = 0;
|
||||
|
||||
// stabilization
|
||||
_stabilization.update(pCmd, qCmd, rCmd,
|
||||
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
||||
|
||||
// output
|
||||
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
|
||||
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
|
||||
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
|
||||
|
||||
// currently using manual throttle
|
||||
// XXX if you enable this watch out, vz might be very noisy
|
||||
//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
|
||||
_actuators.control[CH_THR] = _manual.throttle;
|
||||
|
||||
// XXX limit throttle to manual setting (safety) for now.
|
||||
// If it turns out to be confusing, it can be removed later once
|
||||
// a first binary release can be targeted.
|
||||
// This is not a hack, but a design choice.
|
||||
|
||||
/* do not limit in HIL */
|
||||
#warning fix this
|
||||
// if (!_status.flag_hil_enabled) {
|
||||
// /* limit to value of manual throttle */
|
||||
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
|
||||
// _actuators.control[CH_THR] : _manual.throttle;
|
||||
// }
|
||||
#warning fix this
|
||||
// }
|
||||
|
||||
// body rates controller, disabled for now
|
||||
else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
|
||||
|
||||
_stabilization.update(_manual.roll, _manual.pitch, _manual.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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,280 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <controllib/blocks.hpp>
|
||||
#include <controllib/uorb/blocks.hpp>
|
||||
|
||||
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, float outputScale = 1.0);
|
||||
|
||||
/**
|
||||
* 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 outputScale = 1.0);
|
||||
float getAileron() { return _aileron; }
|
||||
float getElevator() { return _elevator; }
|
||||
float getRudder() { return _yawDamper.getRudder(); }
|
||||
};
|
||||
|
||||
/**
|
||||
* 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.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Multi-mode Autopilot
|
||||
*/
|
||||
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
|
||||
{
|
||||
private:
|
||||
// stabilization
|
||||
BlockStabilization _stabilization;
|
||||
|
||||
// heading hold
|
||||
BlockP _psi2Phi;
|
||||
BlockP _phi2P;
|
||||
BlockLimitSym _phiLimit;
|
||||
|
||||
// velocity hold
|
||||
BlockPID _v2Theta;
|
||||
BlockPID _theta2Q;
|
||||
BlockLimit _theLimit;
|
||||
BlockLimit _vLimit;
|
||||
|
||||
// altitude/ climb rate hold
|
||||
BlockPID _h2Thr;
|
||||
BlockPID _cr2Thr;
|
||||
|
||||
// guidance
|
||||
BlockWaypointGuidance _guide;
|
||||
|
||||
// block params
|
||||
BlockParam<float> _trimAil;
|
||||
BlockParam<float> _trimElv;
|
||||
BlockParam<float> _trimRdr;
|
||||
BlockParam<float> _trimThr;
|
||||
BlockParam<float> _trimV;
|
||||
BlockParam<float> _vCmd;
|
||||
BlockParam<float> _crMax;
|
||||
|
||||
struct pollfd _attPoll;
|
||||
vehicle_global_position_set_triplet_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
|
||||
|
||||
@@ -45,12 +45,13 @@
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <controllib/fixedwing.hpp>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "fixedwing.hpp"
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
@@ -38,4 +38,5 @@
|
||||
MODULE_COMMAND = fixedwing_backside
|
||||
|
||||
SRCS = fixedwing_backside_main.cpp \
|
||||
fixedwing.cpp \
|
||||
params.c
|
||||
|
||||
Reference in New Issue
Block a user