From 1980d9dd63e29390f7c3ba9b31be576c07706f73 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 01:35:43 -0400 Subject: Working on segway controller, restructure of fixedwing. --- src/modules/fixedwing_backside/fixedwing.cpp | 361 +++++++++++++++++++++ src/modules/fixedwing_backside/fixedwing.hpp | 303 +++++++++++++++++ .../fixedwing_backside/fixedwing_backside_main.cpp | 3 +- src/modules/fixedwing_backside/module.mk | 1 + 4 files changed, 667 insertions(+), 1 deletion(-) create mode 100644 src/modules/fixedwing_backside/fixedwing.cpp create mode 100644 src/modules/fixedwing_backside/fixedwing.hpp (limited to 'src/modules/fixedwing_backside') diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp new file mode 100644 index 000000000..d5dc746e0 --- /dev/null +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -0,0 +1,361 @@ +/**************************************************************************** + * + * 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))); +} + +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/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp new file mode 100644 index 000000000..eb5d38381 --- /dev/null +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -0,0 +1,303 @@ +/**************************************************************************** + * + * 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 + +extern "C" { +#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. + */ + +/** + * 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; } +}; + +/** + * 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 _trimAil; + BlockParam _trimElv; + BlockParam _trimRdr; + BlockParam _trimThr; + BlockParam _trimV; + BlockParam _vCmd; + BlockParam _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/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526e..f4ea05088 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -45,12 +45,13 @@ #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 */ diff --git a/src/modules/fixedwing_backside/module.mk b/src/modules/fixedwing_backside/module.mk index ec958d7cb..133728781 100644 --- a/src/modules/fixedwing_backside/module.mk +++ b/src/modules/fixedwing_backside/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = fixedwing_backside SRCS = fixedwing_backside_main.cpp \ + fixedwing.cpp \ params.c -- cgit v1.2.3 From dc542b2a4818bfc1c3c1cd24c5ee513d5b5ea0c4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 28 Jul 2013 22:27:05 -0400 Subject: Segway stabilized. --- src/drivers/md25/md25.cpp | 17 +++++-- src/drivers/md25/md25.hpp | 13 +++++ src/drivers/md25/md25_main.cpp | 27 ++++++++++- src/modules/controllib/uorb/blocks.cpp | 37 ++++++++++++++ src/modules/controllib/uorb/blocks.hpp | 23 +++++++++ src/modules/fixedwing_backside/fixedwing.cpp | 37 -------------- src/modules/fixedwing_backside/fixedwing.hpp | 23 --------- src/modules/segway/BlockSegwayController.cpp | 19 ++++---- src/modules/segway/BlockSegwayController.hpp | 13 ++++- src/modules/segway/params.c | 72 ++-------------------------- src/modules/segway/segway_main.cpp | 22 ++------- 11 files changed, 138 insertions(+), 165 deletions(-) (limited to 'src/modules/fixedwing_backside') diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d6dd64a09..d43e3aef9 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -116,7 +116,8 @@ MD25::MD25(const char *deviceName, int bus, setMotor2Speed(0); resetEncoders(); _setMode(MD25::MODE_UNSIGNED_SPEED); - setSpeedRegulation(true); + setSpeedRegulation(false); + setMotorAccel(10); setTimeout(true); } @@ -308,6 +309,12 @@ int MD25::setDeviceAddress(uint8_t address) return OK; } +int MD25::setMotorAccel(uint8_t accel) +{ + return _writeUint8(REG_ACCEL_RATE_RW, + accel); +} + int MD25::setMotor1Speed(float value) { return _writeUint8(REG_SPEED1_RW, @@ -461,12 +468,12 @@ int md25Test(const char *deviceName, uint8_t bus, uint8_t address) MD25 md25("/dev/md25", bus, address); // print status - char buf[200]; + char buf[400]; md25.status(buf, sizeof(buf)); printf("%s\n", buf); // setup for test - md25.setSpeedRegulation(true); + md25.setSpeedRegulation(false); md25.setTimeout(true); float dt = 0.1; float speed = 0.2; @@ -568,12 +575,12 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu MD25 md25("/dev/md25", bus, address); // print status - char buf[200]; + char buf[400]; md25.status(buf, sizeof(buf)); printf("%s\n", buf); // setup for test - md25.setSpeedRegulation(true); + md25.setSpeedRegulation(false); md25.setTimeout(true); float dt = 0.01; float t_final = 60.0; diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 780978514..1661f67f9 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -212,6 +212,19 @@ public: */ int setDeviceAddress(uint8_t address); + /** + * set motor acceleration + * @param accel + * controls motor speed change (1-10) + * accel rate | time for full fwd. to full rev. + * 1 | 6.375 s + * 2 | 1.6 s + * 3 | 0.675 s + * 5(default) | 1.275 s + * 10 | 0.65 s + */ + int setMotorAccel(uint8_t accel); + /** * set motor 1 speed * @param normSpeed normalize speed between -1 and 1 diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp index 3260705c1..7e5904d05 100644 --- a/src/drivers/md25/md25_main.cpp +++ b/src/drivers/md25/md25_main.cpp @@ -82,7 +82,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n"); + fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n"); exit(1); } @@ -184,6 +184,29 @@ int md25_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "read")) { + if (argc < 4) { + printf("usage: md25 read bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + // print status + char buf[400]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + exit(0); + } + + if (!strcmp(argv[1], "search")) { if (argc < 3) { printf("usage: md25 search bus\n"); @@ -268,7 +291,7 @@ int md25_thread_main(int argc, char *argv[]) uint8_t address = strtoul(argv[4], nullptr, 0); // start - MD25 md25("/dev/md25", bus, address); + MD25 md25(deviceName, bus, address); thread_running = true; diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 6e5ade519..448a42a99 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -42,6 +42,43 @@ namespace control { +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 diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 54f31735c..9c0720aa5 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -57,6 +57,10 @@ #include #include +extern "C" { +#include +} + #include "../blocks.hpp" #include "UOrbSubscription.hpp" #include "UOrbPublication.hpp" @@ -64,6 +68,25 @@ namespace control { +/** + * 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 */ diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d5dc746e0..f655a13bd 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -86,43 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd, _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))); -} - BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) : BlockUorbEnabledAutopilot(parent, name), _stabilization(this, ""), // no name needed, already unique diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index eb5d38381..3876e4630 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -42,10 +42,6 @@ #include #include -extern "C" { -#include -} - namespace control { @@ -231,25 +227,6 @@ public: * 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; } -}; - /** * Multi-mode Autopilot */ diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b7a0bbbcc..682758a14 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -30,23 +30,24 @@ void BlockSegwayController::update() { // update guidance } - // 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 + // compute speed command + float spdCmd = -theta2spd.update(_att.pitch) - q2spd.update(_att.pitchspeed); // handle autopilot modes if (_status.state_machine == SYSTEM_STATE_AUTO || _status.state_machine == SYSTEM_STATE_STABILIZED) { - - float spdCmd = phi2spd.update(_att.phi); - - // output _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[0] = _manual.roll; - _actuators.control[1] = _manual.roll; + if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + _actuators.control[CH_LEFT] = _manual.throttle; + _actuators.control[CH_RIGHT] = _manual.pitch; + + } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + _actuators.control[0] = spdCmd; + _actuators.control[1] = spdCmd; + } } // update all publications diff --git a/src/modules/segway/BlockSegwayController.hpp b/src/modules/segway/BlockSegwayController.hpp index b16d38338..e2faa4916 100644 --- a/src/modules/segway/BlockSegwayController.hpp +++ b/src/modules/segway/BlockSegwayController.hpp @@ -8,11 +8,20 @@ class BlockSegwayController : public control::BlockUorbEnabledAutopilot { public: BlockSegwayController() : BlockUorbEnabledAutopilot(NULL,"SEG"), - phi2spd(this, "PHI2SPD") + theta2spd(this, "THETA2SPD"), + q2spd(this, "Q2SPD"), + _attPoll(), + _timeStamp(0) { + _attPoll.fd = _att.getHandle(); + _attPoll.events = POLLIN; } void update(); private: - BlockP phi2spd; + enum {CH_LEFT, CH_RIGHT}; + BlockPI theta2spd; + BlockP q2spd; + struct pollfd _attPoll; + uint64_t _timeStamp; }; diff --git a/src/modules/segway/params.c b/src/modules/segway/params.c index db30af416..1669785d3 100644 --- a/src/modules/segway/params.c +++ b/src/modules/segway/params.c @@ -1,72 +1,8 @@ #include -// currently tuned for easystar from arkhangar in HIL -//https://github.com/arktools/arkhangar - // 16 is max name length +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_P, 10.0f); // pitch to speed +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I, 0.0f); // pitch integral to speed +PARAM_DEFINE_FLOAT(SEG_THETA2SPD_I_MAX, 0.0f); // integral limiter +PARAM_DEFINE_FLOAT(SEG_Q2SPD, 1.0f); // pitch rate to speed -// 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 diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 8be1cc7aa..061fbf9b9 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -64,12 +64,7 @@ extern "C" __EXPORT int segway_main(int argc, char *argv[]); /** * Mainloop of deamon. */ -int control_demo_thread_main(int argc, char *argv[]); - -/** - * Test function - */ -void test(); +int segway_thread_main(int argc, char *argv[]); /** * Print the correct usage. @@ -114,16 +109,11 @@ int segway_main(int argc, char *argv[]) SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, - control_demo_thread_main, + segway_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } - if (!strcmp(argv[1], "test")) { - test(); - exit(0); - } - if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -144,7 +134,7 @@ int segway_main(int argc, char *argv[]) exit(1); } -int control_demo_thread_main(int argc, char *argv[]) +int segway_thread_main(int argc, char *argv[]) { warnx("starting"); @@ -165,9 +155,3 @@ int control_demo_thread_main(int argc, char *argv[]) return 0; } - -void test() -{ - warnx("beginning control lib test"); - control::basicBlocksTest(); -} -- cgit v1.2.3