mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 15:43:34 +08:00
Merge branch 'master' into beta
This commit is contained in:
Lorenz Meier
@@ -9,33 +9,22 @@ This repository contains the [PX4 Flight Core](http://px4.io), with the main app
|
|||||||
* Official Website: http://px4.io
|
* Official Website: http://px4.io
|
||||||
* License: BSD 3-clause (see [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md))
|
* License: BSD 3-clause (see [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md))
|
||||||
* Supported airframes (more experimental are supported):
|
* Supported airframes (more experimental are supported):
|
||||||
* [Multicopters](http://px4.io/platforms/multicopters/start)
|
* [Multicopters](http://px4.io/portfolio_category/multicopter/)
|
||||||
* [Fixed wing](http://px4.io/platforms/planes/start)
|
* [Fixed wing](http://px4.io/portfolio_category/vtol/)
|
||||||
* [VTOL](http://px4.io/platforms/vtol/start)
|
* [VTOL](http://px4.io/portfolio_category/plane/)
|
||||||
* Binaries (always up-to-date from master):
|
|
||||||
* [Downloads](http://px4.io/firmware/downloads)
|
|
||||||
* Releases
|
* Releases
|
||||||
* [Downloads](https://github.com/PX4/Firmware/releases)
|
* [Downloads](https://github.com/PX4/Firmware/releases)
|
||||||
* Forum / Mailing list: [Google Groups](http://groups.google.com/group/px4users)
|
|
||||||
|
|
||||||
### Users ###
|
### Users ###
|
||||||
|
|
||||||
Please refer to the [user documentation](https://pixhawk.org/users/start) for flying drones with the PX4 flight stack.
|
Please refer to the [user documentation](http://px4.io) and [user forum](http://discuss.px4.io) for flying drones with the PX4 flight stack.
|
||||||
|
|
||||||
### Developers ###
|
### Developers ###
|
||||||
|
|
||||||
Contributing guide:
|
* [Developer Forum / Mailing list](http://groups.google.com/group/px4users)
|
||||||
* [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md)
|
* [Guide for Contributions](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md)
|
||||||
* [PX4 Contribution Guide](http://px4.io/dev/contributing)
|
* [Developer guide](http://dev.px4.io)
|
||||||
|
|
||||||
Software in the Loop guide:
|
|
||||||
Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl).
|
|
||||||
|
|
||||||
Developer guide:
|
|
||||||
http://dev.px4.io
|
|
||||||
|
|
||||||
Testing guide:
|
|
||||||
http://px4.io/dev/unit_tests
|
|
||||||
|
|
||||||
This repository contains code supporting these boards:
|
This repository contains code supporting these boards:
|
||||||
* [Snapdragon Flight](http://dev.px4.io/hardware-snapdragon.html)
|
* [Snapdragon Flight](http://dev.px4.io/hardware-snapdragon.html)
|
||||||
@@ -44,8 +33,3 @@ This repository contains code supporting these boards:
|
|||||||
* FMUv4.x (Pixhawk X and [Pixracer](http://dev.px4.io/hardware-pixracer.html))
|
* FMUv4.x (Pixhawk X and [Pixracer](http://dev.px4.io/hardware-pixracer.html))
|
||||||
* AeroCore (v1 and v2)
|
* AeroCore (v1 and v2)
|
||||||
* STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
|
* STM32F4Discovery (basic support) [Tutorial](https://pixhawk.org/modules/stm32f4discovery)
|
||||||
|
|
||||||
## NuttShell (NSH) ##
|
|
||||||
|
|
||||||
NSH usage documentation:
|
|
||||||
http://px4.io/users/serial_connection
|
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
#!nsh
|
#!nsh
|
||||||
#
|
#
|
||||||
# @name Generic Quadrotor X config
|
# @name 3DR Solo
|
||||||
#
|
#
|
||||||
# @type Quadrotor x
|
# @type Quadrotor x
|
||||||
#
|
#
|
||||||
|
|||||||
@@ -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 :
|
|
||||||
@@ -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
|
|
||||||
|
|
||||||
@@ -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 <controllib/blocks.hpp>
|
|
||||||
#include <controllib/uorb/blocks.hpp>
|
|
||||||
|
|
||||||
namespace control
|
|
||||||
{
|
|
||||||
|
|
||||||
namespace fixedwing
|
|
||||||
{
|
|
||||||
|
|
||||||
/**
|
|
||||||
* BlockYawDamper
|
|
||||||
*
|
|
||||||
* This block has more explations to help new developers
|
|
||||||
* add their own blocks. It includes a limited explanation
|
|
||||||
* of some C++ basics.
|
|
||||||
*
|
|
||||||
* Block: The generic class describing a typical block as you
|
|
||||||
* would expect in Simulink or ScicosLab. A block can have
|
|
||||||
* parameters. It cannot have other blocks.
|
|
||||||
*
|
|
||||||
* SuperBlock: A superblock is a block that can have other
|
|
||||||
* blocks. It has methods that manage the blocks beneath it.
|
|
||||||
*
|
|
||||||
* BlockYawDamper inherits from SuperBlock publically, this
|
|
||||||
* means that any public function in SuperBlock are public within
|
|
||||||
* BlockYawDamper and may be called from outside the
|
|
||||||
* class methods. Any protected function within block
|
|
||||||
* are private to the class and may not be called from
|
|
||||||
* outside this class. Protected should be preferred
|
|
||||||
* where possible to public as it is important to
|
|
||||||
* limit access to the bare minimum to prevent
|
|
||||||
* accidental errors.
|
|
||||||
*/
|
|
||||||
class __EXPORT BlockYawDamper : public SuperBlock
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
/**
|
|
||||||
* Declaring other blocks used by this block
|
|
||||||
*
|
|
||||||
* In this section we declare all child blocks that
|
|
||||||
* this block is composed of. They are private
|
|
||||||
* members so only this block has direct access to
|
|
||||||
* them.
|
|
||||||
*/
|
|
||||||
BlockLowPass _rLowPass;
|
|
||||||
BlockHighPass _rWashout;
|
|
||||||
BlockP _r2Rdr;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Declaring output values and accessors
|
|
||||||
*
|
|
||||||
* If we have any output values for the block we
|
|
||||||
* declare them here. Output can be directly returned
|
|
||||||
* through the update function, but outputs should be
|
|
||||||
* declared here if the information will likely be requested
|
|
||||||
* again, or if there are multiple outputs.
|
|
||||||
*
|
|
||||||
* You should only be able to set outputs from this block,
|
|
||||||
* so the outputs are declared in the private section.
|
|
||||||
* A get accessor is provided
|
|
||||||
* in the public section for other blocks to get the
|
|
||||||
* value of the output.
|
|
||||||
*/
|
|
||||||
float _rudder;
|
|
||||||
public:
|
|
||||||
/**
|
|
||||||
* BlockYawDamper Constructor
|
|
||||||
*
|
|
||||||
* The job of the constructor is to initialize all
|
|
||||||
* parameter in this block and initialize all child
|
|
||||||
* blocks. Note also, that the output values must be
|
|
||||||
* initialized here. The order of the members in the
|
|
||||||
* member initialization list should follow the
|
|
||||||
* order in which they are declared within the class.
|
|
||||||
* See the private member declarations above.
|
|
||||||
*
|
|
||||||
* Block Construction
|
|
||||||
*
|
|
||||||
* All blocks are constructed with their parent block
|
|
||||||
* and their name. This allows parameters within the
|
|
||||||
* block to construct a fully qualified name from
|
|
||||||
* concatenating the two. If the name provided to the
|
|
||||||
* block is "", then the block will use the parent
|
|
||||||
* name as it's name. This is useful in cases where
|
|
||||||
* you have a block that has parameters "MIN", "MAX",
|
|
||||||
* such as BlockLimit and you do not want an extra name
|
|
||||||
* to qualify them since the parent block has no "MIN",
|
|
||||||
* "MAX" parameters.
|
|
||||||
*
|
|
||||||
* Block Parameter Construction
|
|
||||||
*
|
|
||||||
* Block parameters are named constants, they are
|
|
||||||
* constructed using:
|
|
||||||
* BlockParam::BlockParam(Block * parent, const char * name)
|
|
||||||
* This funciton takes both a parent block and a name.
|
|
||||||
* The constructore then uses the parent name and the name of
|
|
||||||
* the paramter to ask the px4 param library if it has any
|
|
||||||
* parameters with this name. If it does, a handle to the
|
|
||||||
* parameter is retrieved.
|
|
||||||
*
|
|
||||||
* Block/ BlockParam Naming
|
|
||||||
*
|
|
||||||
* When desigining new blocks, the naming of the parameters and
|
|
||||||
* blocks determines the fully qualified name of the parameters
|
|
||||||
* within the ground station, so it is important to choose
|
|
||||||
* short, easily understandable names. Again, when a name of
|
|
||||||
* "" is passed, the parent block name is used as the value to
|
|
||||||
* prepend to paramters names.
|
|
||||||
*/
|
|
||||||
BlockYawDamper(SuperBlock *parent, const char *name);
|
|
||||||
/**
|
|
||||||
* Block deconstructor
|
|
||||||
*
|
|
||||||
* It is always a good idea to declare a virtual
|
|
||||||
* deconstructor so that upon calling delete from
|
|
||||||
* a class derived from this, all of the
|
|
||||||
* deconstructors all called, the derived class first, and
|
|
||||||
* then the base class
|
|
||||||
*/
|
|
||||||
virtual ~BlockYawDamper();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Block update function
|
|
||||||
*
|
|
||||||
* The job of the update function is to compute the output
|
|
||||||
* values for the block. In a simple block with one output,
|
|
||||||
* the output may be returned directly. If the output is
|
|
||||||
* required frequenly by other processses, it might be a
|
|
||||||
* good idea to declare a member to store the temporary
|
|
||||||
* variable.
|
|
||||||
*/
|
|
||||||
void update(float rCmd, float r, float outputScale = 1.0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Rudder output value accessor
|
|
||||||
*
|
|
||||||
* This is a public accessor function, which means that the
|
|
||||||
* private value _rudder is returned to anyone calling
|
|
||||||
* BlockYawDamper::getRudder(). Note thate a setRudder() is
|
|
||||||
* not provided, this is because the updateParams() call
|
|
||||||
* for a block provides the mechanism for updating the
|
|
||||||
* paramter.
|
|
||||||
*/
|
|
||||||
float getRudder() { return _rudder; }
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Stability augmentation system.
|
|
||||||
* Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
|
|
||||||
*/
|
|
||||||
class __EXPORT BlockStabilization : public SuperBlock
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
BlockYawDamper _yawDamper;
|
|
||||||
BlockLowPass _pLowPass;
|
|
||||||
BlockLowPass _qLowPass;
|
|
||||||
BlockP _p2Ail;
|
|
||||||
BlockP _q2Elv;
|
|
||||||
float _aileron;
|
|
||||||
float _elevator;
|
|
||||||
public:
|
|
||||||
BlockStabilization(SuperBlock *parent, const char *name);
|
|
||||||
virtual ~BlockStabilization();
|
|
||||||
void update(float pCmd, float qCmd, float rCmd,
|
|
||||||
float p, float q, float r,
|
|
||||||
float outputScale = 1.0);
|
|
||||||
float getAileron() { return _aileron; }
|
|
||||||
float getElevator() { return _elevator; }
|
|
||||||
float getRudder() { return _yawDamper.getRudder(); }
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Frontside/ Backside Control Systems
|
|
||||||
*
|
|
||||||
* Frontside :
|
|
||||||
* velocity error -> throttle
|
|
||||||
* altitude error -> elevator
|
|
||||||
*
|
|
||||||
* Backside :
|
|
||||||
* velocity error -> elevator
|
|
||||||
* altitude error -> throttle
|
|
||||||
*
|
|
||||||
* Backside control systems are more resilient at
|
|
||||||
* slow speeds on the back-side of the power
|
|
||||||
* required curve/ landing etc. Less performance
|
|
||||||
* than frontside at high speeds.
|
|
||||||
*/
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Multi-mode Autopilot
|
|
||||||
*/
|
|
||||||
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
|
|
||||||
{
|
|
||||||
private:
|
|
||||||
// stabilization
|
|
||||||
BlockStabilization _stabilization;
|
|
||||||
|
|
||||||
// heading hold
|
|
||||||
BlockP _psi2Phi;
|
|
||||||
BlockP _phi2P;
|
|
||||||
BlockLimitSym _phiLimit;
|
|
||||||
|
|
||||||
// velocity hold
|
|
||||||
BlockPID _v2Theta;
|
|
||||||
BlockPID _theta2Q;
|
|
||||||
BlockLimit _theLimit;
|
|
||||||
BlockLimit _vLimit;
|
|
||||||
|
|
||||||
// altitude/ climb rate hold
|
|
||||||
BlockPID _h2Thr;
|
|
||||||
BlockPID _cr2Thr;
|
|
||||||
|
|
||||||
// guidance
|
|
||||||
BlockWaypointGuidance _guide;
|
|
||||||
|
|
||||||
// block params
|
|
||||||
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
|
|
||||||
|
|
||||||
@@ -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 <px4_config.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <systemlib/systemlib.h>
|
|
||||||
#include <systemlib/param/param.h>
|
|
||||||
#include <systemlib/err.h>
|
|
||||||
#include <drivers/drv_hrt.h>
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
#include "fixedwing.hpp"
|
|
||||||
|
|
||||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
|
||||||
static bool thread_running = false; /**< Deamon status flag */
|
|
||||||
static int deamon_task; /**< Handle of deamon task / thread */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* 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 <additional params>]\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();
|
|
||||||
}
|
|
||||||
@@ -1,72 +0,0 @@
|
|||||||
#include <systemlib/param/param.h>
|
|
||||||
|
|
||||||
// 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
|
|
||||||
@@ -1754,6 +1754,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
configure_stream("EXTENDED_SYS_STATE", 1.0f);
|
configure_stream("EXTENDED_SYS_STATE", 1.0f);
|
||||||
configure_stream("ALTITUDE", 1.0f);
|
configure_stream("ALTITUDE", 1.0f);
|
||||||
configure_stream("VISION_POSITION_NED", 10.0f);
|
configure_stream("VISION_POSITION_NED", 10.0f);
|
||||||
|
configure_stream("NAMED_VALUE_FLOAT", 1.0f);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MODE_ONBOARD:
|
case MAVLINK_MODE_ONBOARD:
|
||||||
@@ -1782,6 +1783,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
configure_stream("EXTENDED_SYS_STATE", 2.0f);
|
configure_stream("EXTENDED_SYS_STATE", 2.0f);
|
||||||
configure_stream("ALTITUDE", 10.0f);
|
configure_stream("ALTITUDE", 10.0f);
|
||||||
configure_stream("VISION_POSITION_NED", 10.0f);
|
configure_stream("VISION_POSITION_NED", 10.0f);
|
||||||
|
configure_stream("NAMED_VALUE_FLOAT", 10.0f);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MODE_OSD:
|
case MAVLINK_MODE_OSD:
|
||||||
@@ -1810,7 +1812,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
configure_stream("HOME_POSITION", 0.5f);
|
configure_stream("HOME_POSITION", 0.5f);
|
||||||
configure_stream("GLOBAL_POSITION_INT", 10.0f);
|
configure_stream("GLOBAL_POSITION_INT", 10.0f);
|
||||||
configure_stream("ATTITUDE_TARGET", 8.0f);
|
configure_stream("ATTITUDE_TARGET", 8.0f);
|
||||||
configure_stream("PARAM_VALUE", 300.0f);
|
//configure_stream("PARAM_VALUE", 300.0f);
|
||||||
configure_stream("MISSION_ITEM", 50.0f);
|
configure_stream("MISSION_ITEM", 50.0f);
|
||||||
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
|
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
|
||||||
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
configure_stream("OPTICAL_FLOW_RAD", 10.0f);
|
||||||
@@ -1830,7 +1832,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
configure_stream("EXTENDED_SYS_STATE", 2.0f);
|
configure_stream("EXTENDED_SYS_STATE", 2.0f);
|
||||||
configure_stream("ALTITUDE", 10.0f);
|
configure_stream("ALTITUDE", 10.0f);
|
||||||
configure_stream("VISION_POSITION_NED", 10.0f);
|
configure_stream("VISION_POSITION_NED", 10.0f);
|
||||||
|
configure_stream("NAMED_VALUE_FLOAT", 50.0f);
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -33,7 +33,7 @@
|
|||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE systemcmds__pwm
|
MODULE systemcmds__pwm
|
||||||
MAIN pwm
|
MAIN pwm
|
||||||
STACK 1800
|
STACK 2400
|
||||||
COMPILE_FLAGS
|
COMPILE_FLAGS
|
||||||
-Os
|
-Os
|
||||||
SRCS
|
SRCS
|
||||||
|
|||||||
Reference in New Issue
Block a user