diff options
Diffstat (limited to 'src/modules/controllib')
-rw-r--r-- | src/modules/controllib/block/Block.cpp | 4 | ||||
-rw-r--r-- | src/modules/controllib/blocks.hpp | 1 | ||||
-rw-r--r-- | src/modules/controllib/fixedwing.cpp | 379 | ||||
-rw-r--r-- | src/modules/controllib/fixedwing.hpp | 344 | ||||
-rw-r--r-- | src/modules/controllib/module.mk | 8 | ||||
-rw-r--r-- | src/modules/controllib/uorb/UOrbPublication.cpp (renamed from src/modules/controllib/block/UOrbPublication.cpp) | 0 | ||||
-rw-r--r-- | src/modules/controllib/uorb/UOrbPublication.hpp (renamed from src/modules/controllib/block/UOrbPublication.hpp) | 4 | ||||
-rw-r--r-- | src/modules/controllib/uorb/UOrbSubscription.cpp (renamed from src/modules/controllib/block/UOrbSubscription.cpp) | 0 | ||||
-rw-r--r-- | src/modules/controllib/uorb/UOrbSubscription.hpp (renamed from src/modules/controllib/block/UOrbSubscription.hpp) | 4 | ||||
-rw-r--r-- | src/modules/controllib/uorb/blocks.cpp | 64 | ||||
-rw-r--r-- | src/modules/controllib/uorb/blocks.hpp | 90 |
11 files changed, 165 insertions, 733 deletions
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 5994d2315..b964d40a3 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include "../uorb/UOrbSubscription.hpp" +#include "../uorb/UOrbPublication.hpp" namespace control { diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 7a785d12e..fefe99702 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -42,6 +42,7 @@ #include <assert.h> #include <time.h> #include <stdlib.h> +#include <math.h> #include <mathlib/math/test/test.hpp> #include "block/Block.hpp" diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp deleted file mode 100644 index 77b2ac806..000000000 --- a/src/modules/controllib/fixedwing.cpp +++ /dev/null @@ -1,379 +0,0 @@ -/**************************************************************************** - * - * 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, 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); -} - -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_set_triplet), 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 - - // 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; - - // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { - // 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 - - // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { - - // 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 */ - 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.state_machine == SYSTEM_STATE_MANUAL) { - - 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 */ - 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; - } - } - - // 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 - diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp deleted file mode 100644 index e4028c40d..000000000 --- a/src/modules/controllib/fixedwing.hpp +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * 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_set_triplet.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, 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. - */ - -/** - * 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_set_triplet_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: - // 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 - diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index 13d1069c7..d815a8feb 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,7 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - block/UOrbPublication.cpp \ - block/UOrbSubscription.cpp \ - blocks.cpp \ - fixedwing.cpp + uorb/UOrbPublication.cpp \ + uorb/UOrbSubscription.cpp \ + uorb/blocks.cpp \ + blocks.cpp diff --git a/src/modules/controllib/block/UOrbPublication.cpp b/src/modules/controllib/uorb/UOrbPublication.cpp index f69b39d90..f69b39d90 100644 --- a/src/modules/controllib/block/UOrbPublication.cpp +++ b/src/modules/controllib/uorb/UOrbPublication.cpp diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/uorb/UOrbPublication.hpp index 0a8ae2ff7..6f1f3fc1c 100644 --- a/src/modules/controllib/block/UOrbPublication.hpp +++ b/src/modules/controllib/uorb/UOrbPublication.hpp @@ -39,8 +39,8 @@ #pragma once #include <uORB/uORB.h> -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/block/UOrbSubscription.cpp b/src/modules/controllib/uorb/UOrbSubscription.cpp index 022cadd24..022cadd24 100644 --- a/src/modules/controllib/block/UOrbSubscription.cpp +++ b/src/modules/controllib/uorb/UOrbSubscription.cpp diff --git a/src/modules/controllib/block/UOrbSubscription.hpp b/src/modules/controllib/uorb/UOrbSubscription.hpp index 22cc2e114..d337d89a8 100644 --- a/src/modules/controllib/block/UOrbSubscription.hpp +++ b/src/modules/controllib/uorb/UOrbSubscription.hpp @@ -39,8 +39,8 @@ #pragma once #include <uORB/uORB.h> -#include "Block.hpp" -#include "List.hpp" +#include "../block/Block.hpp" +#include "../block/List.hpp" namespace control diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp new file mode 100644 index 000000000..6e5ade519 --- /dev/null +++ b/src/modules/controllib/uorb/blocks.cpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * 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 uorb_blocks.cpp + * + * uorb block library code + */ + +#include "blocks.hpp" + +namespace control +{ + +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_set_triplet), 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() {}; + +} // namespace control + diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp new file mode 100644 index 000000000..54f31735c --- /dev/null +++ b/src/modules/controllib/uorb/blocks.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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 uorb_blocks.h + * + * uorb block 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_set_triplet.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 "UOrbSubscription.hpp" +#include "UOrbPublication.hpp" + +namespace control +{ + +/** + * 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_set_triplet_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(); +}; + +} // namespace control + |