From 83339a2de0b6ee86854828e2025229ef36cc5f2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Jan 2016 13:56:39 +0100 Subject: [PATCH] Remove unmaintained fixed wing controller --- src/modules/fixedwing_backside/CMakeLists.txt | 43 --- src/modules/fixedwing_backside/fixedwing.cpp | 332 ------------------ src/modules/fixedwing_backside/fixedwing.hpp | 280 --------------- .../fixedwing_backside_main.cpp | 175 --------- src/modules/fixedwing_backside/params.c | 72 ---- 5 files changed, 902 deletions(-) delete mode 100644 src/modules/fixedwing_backside/CMakeLists.txt delete mode 100644 src/modules/fixedwing_backside/fixedwing.cpp delete mode 100644 src/modules/fixedwing_backside/fixedwing.hpp delete mode 100644 src/modules/fixedwing_backside/fixedwing_backside_main.cpp delete mode 100644 src/modules/fixedwing_backside/params.c diff --git a/src/modules/fixedwing_backside/CMakeLists.txt b/src/modules/fixedwing_backside/CMakeLists.txt deleted file mode 100644 index c2c8ec649f..0000000000 --- a/src/modules/fixedwing_backside/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE modules__fixedwing_backside - MAIN fixedwing_backside - SRCS - fixedwing_backside_main.cpp - fixedwing.cpp - params.c - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp deleted file mode 100644 index 39b624f24a..0000000000 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ /dev/null @@ -1,332 +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); -} - -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(), - _lastMissionCmd(), - _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 (_missionCmd.updated()) { - _lastMissionCmd = _missionCmd.get(); - } - - // 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 = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { - _actuators.get().control[i] = 0.0f; - } - - // only update guidance in auto mode - if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { - // TODO use vehicle_control_mode here? - // update guidance - _guide.update(_pos.get(), _att.get(), - _missionCmd.get().current, - _lastMissionCmd.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.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) { - - // 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.get().vel_n * _pos.get().vel_n + - _pos.get().vel_e * _pos.get().vel_e + - _pos.get().vel_d * _pos.get().vel_d)); - - // limit velocity command between min/max velocity - float vCmd = _vLimit.update(_vCmd.get()); - - // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt); - - // heading hold - float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw); - float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); - float pCmd = _phi2P.update(phiCmd - _att.get().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.get().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.get().rollspeed, - _att.get().pitchspeed, - _att.get().yawspeed, - outputScale); - - // output - _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - _actuators.get().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.get().hil_state != _vehicle_status::HIL_STATE_ON) { - /* limit to value of manual throttle */ - _actuators.get().control[CH_THR] = - (_actuators.get().control[CH_THR] < _manual.get().z) ? - _actuators.get().control[CH_THR] : _manual.get().z; - } - - } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) { - _actuators.get().control[CH_AIL] = _manual.get().y; - _actuators.get().control[CH_ELV] = _manual.get().x; - _actuators.get().control[CH_RDR] = _manual.get().r; - _actuators.get().control[CH_THR] = _manual.get().z; - - } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL || - _status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) { - - // 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.get().vel_n * _pos.get().vel_n + - _pos.get().vel_e * _pos.get().vel_e + - _pos.get().vel_d * _pos.get().vel_d)); - - // 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.get().y * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.get().roll); - - // throttle channel -> velocity - // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.get().z * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); - float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); - - // yaw rate cmd - float rCmd = 0; - - // stabilization - _stabilization.update(pCmd, qCmd, rCmd, - _att.get().rollspeed, - _att.get().pitchspeed, - _att.get().yawspeed); - - // output - _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.get().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.get().control[CH_THR] = _manual.get().z; - - // 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.get().hil_state != vehicle_status_s::HIL_STATE_ON) { - /* limit to value of manual throttle */ - _actuators.get().control[CH_THR] = - (_actuators.get().control[CH_THR] < _manual.get().z) ? - _actuators.get().control[CH_THR] : _manual.get().z; - } - - // body rates controller, disabled for now - // TODO - - } else if ( - 0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? - - _stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r, - _att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed); - - _actuators.get().control[CH_AIL] = _stabilization.getAileron(); - _actuators.get().control[CH_ELV] = _stabilization.getElevator(); - _actuators.get().control[CH_RDR] = _stabilization.getRudder(); - _actuators.get().control[CH_THR] = _manual.get().z; - } - - // update all publications - updatePublications(); -} - -BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() -{ - // send one last publication when destroyed, setting - // all output to zero - for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { - _actuators.get().control[i] = 0.0f; - } - - updatePublications(); -} - -} // namespace fixedwing - -} // namespace control - diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp deleted file mode 100644 index e1c85c2618..0000000000 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ /dev/null @@ -1,280 +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 -#include - -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 - BlockParamFloat _trimAil; - BlockParamFloat _trimElv; - BlockParamFloat _trimRdr; - BlockParamFloat _trimThr; - BlockParamFloat _trimV; - BlockParamFloat _vCmd; - BlockParamFloat _crMax; - - struct pollfd _attPoll; - position_setpoint_triplet_s _lastMissionCmd; - 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/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp deleted file mode 100644 index fe9a1066bd..0000000000 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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_backside_main.cpp - * @author James Goppert - * - * Fixedwing backside controller using control library - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int fixedwing_backside_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int control_demo_thread_main(int argc, char *argv[]); - -/** - * Test function - */ -void test(); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int fixedwing_backside_main(int argc, char *argv[]) -{ - - if (argc < 2) { - usage("missing command"); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - deamon_task = px4_task_spawn_cmd("fixedwing_backside", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 5120, - control_demo_thread_main, - (argv) ? (char *const *)&argv[2] : (char *const *)NULL); - exit(0); - } - - if (!strcmp(argv[1], "test")) { - test(); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("is running"); - - } else { - warnx("not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int control_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace control; - - fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); - - thread_running = true; - - while (!thread_should_exit) { - autopilot.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} - -void test() -{ - warnx("beginning control lib test"); - control::basicBlocksTest(); -} diff --git a/src/modules/fixedwing_backside/params.c b/src/modules/fixedwing_backside/params.c deleted file mode 100644 index aa82a4f596..0000000000 --- a/src/modules/fixedwing_backside/params.c +++ /dev/null @@ -1,72 +0,0 @@ -#include - -// currently tuned for easystar from arkhangar in HIL -//https://github.com/arktools/arkhangar - -// 16 is max name length - -// gyro low pass filter -PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq -PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq - -// yaw washout -PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass - -// stabilization mode -PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator -PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder - -// psi -> phi -> p -PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll -PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg - -// velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain -PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain -PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain -PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass -PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard -PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle -PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle - - -// theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID -PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); - -// h -> thr -PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID -PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); - -// crosstrack -PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain - -// speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity -PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity - -// rate of climb -// this is what rate of climb is commanded (in m/s) -// when the pitch stick is fully defelcted in simple mode -PARAM_DEFINE_FLOAT(FWB_CR_MAX, 1.0f); - -// climb rate -> thr -PARAM_DEFINE_FLOAT(FWB_CR2THR_P, 0.01f); // rate of climb to throttle PID -PARAM_DEFINE_FLOAT(FWB_CR2THR_I, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_D, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_D_LP, 0.0f); -PARAM_DEFINE_FLOAT(FWB_CR2THR_I_MAX, 0.0f); - -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s