diff options
Diffstat (limited to 'apps/systemlib/control/fixedwing.cpp')
-rw-r--r-- | apps/systemlib/control/fixedwing.cpp | 351 |
1 files changed, 0 insertions, 351 deletions
diff --git a/apps/systemlib/control/fixedwing.cpp b/apps/systemlib/control/fixedwing.cpp deleted file mode 100644 index 695653714..000000000 --- a/apps/systemlib/control/fixedwing.cpp +++ /dev/null @@ -1,351 +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) -{ - _rudder = _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) -{ - _aileron = _p2Ail.update( - pCmd - _pLowPass.update(p)); - _elevator = _q2Elv.update( - qCmd - _qLowPass.update(q)); - _yawDamper.update(rCmd, r); -} - -BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _psi2Phi(this, "PSI2PHI"), - _phi2P(this, "PHI2P"), - _phiLimit(this, "PHI_LIM") -{ -} - -BlockHeadingHold::~BlockHeadingHold() {}; - -float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p) -{ - float psiError = _wrap_pi(psiCmd - psi); - float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); - return _phi2P.update(phiCmd - phi); -} - -BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _v2Theta(this, "V2THE"), - _theta2Q(this, "THE2Q"), - _theLimit(this, "THE"), - _vLimit(this, "V") -{ -} - -BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {}; - -float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q) -{ - // negative sign because nose over to increase speed - float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v)); - return _theta2Q.update(thetaCmd - theta); -} - -BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _v2Thr(this, "V2THR") -{ -} - -BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {}; - -float BlockVelocityHoldFrontside::update(float vCmd, float v) -{ - return _v2Thr.update(vCmd - v); -} - -BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _h2Thr(this, "H2THR"), - _throttle(0) -{ -} - -BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {}; - -void BlockAltitudeHoldBackside::update(float hCmd, float h) -{ - _throttle = _h2Thr.update(hCmd - h); -} - -BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - _h2Theta(this, "H2THE"), - _theta2Q(this, "THE2Q") -{ -} - -BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {}; - -float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q) -{ - float thetaCmd = _h2Theta.update(hCmd - h); - return _theta2Q.update(thetaCmd - theta); -} - -BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent, - const char *name, - BlockStabilization *stabilization) : - SuperBlock(parent, name), - _stabilization(stabilization), - _headingHold(this, ""), - _velocityHold(this, ""), - _altitudeHold(this, ""), - _trimAil(this, "TRIM_AIL"), - _trimElv(this, "TRIM_ELV"), - _trimRdr(this, "TRIM_RDR"), - _trimThr(this, "TRIM_THR") -{ -} - -BlockBacksideAutopilot::~BlockBacksideAutopilot() {}; - -void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd, - float h, float v, - float phi, float theta, float psi, - float p, float q, float r) -{ - _altitudeHold.update(hCmd, h); - _stabilization->update( - _headingHold.update(psiCmd, phi, psi, p), - _velocityHold.update(vCmd, v, theta, q), - rCmd, - p, q, r); -} - -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_setpoint), 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 - _backsideAutopilot(this, "", &_stabilization), - _guide(this, ""), - _vCmd(this, "V_CMD"), - _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; - - // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_STABILIZED) { - _stabilization.update( - _ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.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; - - } else if (_status.state_machine == SYSTEM_STATE_AUTO) { - // update guidance - _guide.update(_pos, _att, _posCmd, _lastPosCmd); - - // calculate velocity, XXX should be airspeed, but using ground speed for now - float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz); - - // commands - float rCmd = 0; - - _backsideAutopilot.update( - _posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(), - _pos.alt, v, - _att.roll, _att.pitch, _att.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed - ); - _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); - _actuators.control[CH_ELV] = _backsideAutopilot.getElevator(); - _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); - _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); - - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _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 - |