mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 05:13:34 +08:00
mc stabilized mode: move from mc_pos_control to mc_att_control
- better in terms of dependencies: - the position controller code depended on position topics for yaw - mc_pos_control does not have to be run for Stabilized mode - the code path is much simpler, and thus less error prone. This is especially important since Stabilized is often used as a fail-safe flight mode.
This commit is contained in:
Beat Küng
@@ -51,7 +51,6 @@ endif()
|
||||
|
||||
# add core flight tasks to list
|
||||
list(APPEND flight_tasks_all
|
||||
ManualStabilized
|
||||
ManualAltitude
|
||||
ManualAltitudeSmooth
|
||||
ManualPosition
|
||||
|
||||
@@ -35,5 +35,5 @@ px4_add_library(FlightTaskManualAltitude
|
||||
FlightTaskManualAltitude.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManualStabilized)
|
||||
target_link_libraries(FlightTaskManualAltitude PUBLIC FlightTaskManual)
|
||||
target_include_directories(FlightTaskManualAltitude PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
@@ -43,19 +43,23 @@ using namespace matrix;
|
||||
|
||||
bool FlightTaskManualAltitude::updateInitialize()
|
||||
{
|
||||
bool ret = FlightTaskManualStabilized::updateInitialize();
|
||||
// in addition to stabilized require valid position and velocity in D-direction
|
||||
return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2));
|
||||
bool ret = FlightTaskManual::updateInitialize();
|
||||
// in addition to manual require valid position and velocity in D-direction and valid yaw
|
||||
return ret && PX4_ISFINITE(_position(2)) && PX4_ISFINITE(_velocity(2)) && PX4_ISFINITE(_yaw);
|
||||
}
|
||||
|
||||
bool FlightTaskManualAltitude::activate()
|
||||
{
|
||||
bool ret = FlightTaskManualStabilized::activate();
|
||||
_thrust_setpoint(2) = NAN; // altitude is controlled from position/velocity
|
||||
bool ret = FlightTaskManual::activate();
|
||||
_yaw_setpoint = _yaw;
|
||||
_yawspeed_setpoint = 0.0f;
|
||||
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
|
||||
_position_setpoint(2) = _position(2);
|
||||
_velocity_setpoint(2) = 0.0f;
|
||||
_setDefaultConstraints();
|
||||
|
||||
_constraints.tilt = math::radians(MPC_MAN_TILT_MAX.get());
|
||||
|
||||
if (PX4_ISFINITE(_sub_vehicle_local_position->get().hagl_min)) {
|
||||
_constraints.min_distance_to_ground = _sub_vehicle_local_position->get().hagl_min;
|
||||
|
||||
@@ -78,8 +82,9 @@ bool FlightTaskManualAltitude::activate()
|
||||
|
||||
void FlightTaskManualAltitude::_scaleSticks()
|
||||
{
|
||||
// reuse same scaling as for stabilized
|
||||
FlightTaskManualStabilized::_scaleSticks();
|
||||
/* Scale sticks to yaw and thrust using
|
||||
* linear scale for yaw and piecewise linear map for thrust. */
|
||||
_yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get());
|
||||
|
||||
// scale horizontal velocity with expo curve stick input
|
||||
const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
|
||||
@@ -249,9 +254,47 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskManualAltitude::_rotateIntoHeadingFrame(Vector2f &v)
|
||||
{
|
||||
float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
|
||||
Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
|
||||
v(0) = v_r(0);
|
||||
v(1) = v_r(1);
|
||||
}
|
||||
|
||||
void FlightTaskManualAltitude::_updateHeadingSetpoints()
|
||||
{
|
||||
/* Yaw-lock depends on stick input. If not locked,
|
||||
* yaw_sp is set to NAN.
|
||||
* TODO: add yawspeed to get threshold.*/
|
||||
if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
|
||||
// no fixed heading when rotating around yaw by stick
|
||||
_yaw_setpoint = NAN;
|
||||
|
||||
} else {
|
||||
// hold the current heading when no more rotation commanded
|
||||
if (!PX4_ISFINITE(_yaw_setpoint)) {
|
||||
_yaw_setpoint = _yaw;
|
||||
|
||||
} else {
|
||||
// check reset counter and update yaw setpoint if necessary
|
||||
if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) {
|
||||
_yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
|
||||
_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check if an external yaw handler is active and if yes, let it compute the yaw setpoints
|
||||
if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
|
||||
_yaw_setpoint = NAN;
|
||||
_yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate();
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskManualAltitude::_updateSetpoints()
|
||||
{
|
||||
FlightTaskManualStabilized::_updateHeadingSetpoints(); // get yaw setpoint
|
||||
_updateHeadingSetpoints(); // get yaw setpoint
|
||||
|
||||
// Thrust in xy are extracted directly from stick inputs. A magnitude of
|
||||
// 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no
|
||||
@@ -271,3 +314,11 @@ void FlightTaskManualAltitude::_updateSetpoints()
|
||||
|
||||
_updateAltitudeLock();
|
||||
}
|
||||
|
||||
bool FlightTaskManualAltitude::update()
|
||||
{
|
||||
_scaleSticks();
|
||||
_updateSetpoints();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -39,19 +39,31 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskManualStabilized.hpp"
|
||||
#include "FlightTaskManual.hpp"
|
||||
|
||||
class FlightTaskManualAltitude : public FlightTaskManualStabilized
|
||||
class FlightTaskManualAltitude : public FlightTaskManual
|
||||
{
|
||||
public:
|
||||
FlightTaskManualAltitude() = default;
|
||||
virtual ~FlightTaskManualAltitude() = default;
|
||||
bool activate() override;
|
||||
bool updateInitialize() override;
|
||||
bool update() override;
|
||||
|
||||
/**
|
||||
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
|
||||
*/
|
||||
void setYawHandler(WeatherVane *ext_yaw_handler) override { _ext_yaw_handler = ext_yaw_handler; }
|
||||
|
||||
protected:
|
||||
void _updateSetpoints() override; /**< updates all setpoints */
|
||||
void _scaleSticks() override; /**< scales sticks to velocity in z */
|
||||
void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
|
||||
virtual void _updateSetpoints(); /**< updates all setpoints */
|
||||
virtual void _scaleSticks(); /**< scales sticks to velocity in z */
|
||||
|
||||
/**
|
||||
* rotates vector into local frame
|
||||
*/
|
||||
void _rotateIntoHeadingFrame(matrix::Vector2f &vec);
|
||||
|
||||
/**
|
||||
* Check and sets for position lock.
|
||||
@@ -60,27 +72,15 @@ protected:
|
||||
*/
|
||||
void _updateAltitudeLock();
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualStabilized,
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
|
||||
(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
|
||||
(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
|
||||
(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
|
||||
(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P
|
||||
(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P,
|
||||
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
|
||||
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */
|
||||
)
|
||||
private:
|
||||
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
|
||||
float _max_speed_up = 10.0f;
|
||||
float _min_speed_down = 1.0f;
|
||||
bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
|
||||
bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
|
||||
|
||||
/**
|
||||
* Distance to ground during terrain following.
|
||||
* If user does not demand velocity change in D-direction and the vehcile
|
||||
* is in terrain-following mode, then height to ground will be locked to
|
||||
* _dist_to_ground_lock.
|
||||
*/
|
||||
float _dist_to_ground_lock = NAN;
|
||||
|
||||
/**
|
||||
* Terrain following.
|
||||
* During terrain following, the position setpoint is adjusted
|
||||
@@ -99,4 +99,22 @@ private:
|
||||
void _respectMinAltitude();
|
||||
|
||||
void _respectMaxAltitude();
|
||||
|
||||
|
||||
WeatherVane *_ext_yaw_handler =
|
||||
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
|
||||
|
||||
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
|
||||
float _max_speed_up = 10.0f;
|
||||
float _min_speed_down = 1.0f;
|
||||
bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */
|
||||
bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */
|
||||
|
||||
/**
|
||||
* Distance to ground during terrain following.
|
||||
* If user does not demand velocity change in D-direction and the vehcile
|
||||
* is in terrain-following mode, then height to ground will be locked to
|
||||
* _dist_to_ground_lock.
|
||||
*/
|
||||
float _dist_to_ground_lock = NAN;
|
||||
};
|
||||
|
||||
@@ -53,10 +53,11 @@ bool FlightTaskManualPosition::updateInitialize()
|
||||
|
||||
bool FlightTaskManualPosition::activate()
|
||||
{
|
||||
|
||||
// all requirements from altitude-mode still have to hold
|
||||
bool ret = FlightTaskManualAltitude::activate();
|
||||
|
||||
_constraints.tilt = math::radians(MPC_TILTMAX_AIR.get());
|
||||
|
||||
// set task specific constraint
|
||||
if (_constraints.speed_xy >= MPC_VEL_MANUAL.get()) {
|
||||
_constraints.speed_xy = MPC_VEL_MANUAL.get();
|
||||
|
||||
@@ -1,39 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2018 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_library(FlightTaskManualStabilized
|
||||
FlightTaskManualStabilized.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(FlightTaskManualStabilized PUBLIC FlightTaskManual)
|
||||
target_include_directories(FlightTaskManualStabilized PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
@@ -1,172 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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 FlightManualStabilized.cpp
|
||||
*/
|
||||
|
||||
#include "FlightTaskManualStabilized.hpp"
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <float.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
bool FlightTaskManualStabilized::activate()
|
||||
{
|
||||
bool ret = FlightTaskManual::activate();
|
||||
_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, -_throttle_hover.get());
|
||||
_yaw_setpoint = _yaw;
|
||||
_yawspeed_setpoint = 0.0f;
|
||||
_constraints.tilt = math::radians(_tilt_max_man.get());
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool FlightTaskManualStabilized::updateInitialize()
|
||||
{
|
||||
bool ret = FlightTaskManual::updateInitialize();
|
||||
// need a valid yaw-state
|
||||
return ret && PX4_ISFINITE(_yaw);
|
||||
}
|
||||
|
||||
void FlightTaskManualStabilized::_scaleSticks()
|
||||
{
|
||||
/* Scale sticks to yaw and thrust using
|
||||
* linear scale for yaw and piecewise linear map for thrust. */
|
||||
_yawspeed_setpoint = _sticks_expo(3) * math::radians(_yaw_rate_scaling.get());
|
||||
_throttle = _throttleCurve();
|
||||
}
|
||||
|
||||
void FlightTaskManualStabilized::_updateHeadingSetpoints()
|
||||
{
|
||||
/* Yaw-lock depends on stick input. If not locked,
|
||||
* yaw_sp is set to NAN.
|
||||
* TODO: add yawspeed to get threshold.*/
|
||||
if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
|
||||
// no fixed heading when rotating around yaw by stick
|
||||
_yaw_setpoint = NAN;
|
||||
|
||||
} else {
|
||||
// hold the current heading when no more rotation commanded
|
||||
if (!PX4_ISFINITE(_yaw_setpoint)) {
|
||||
_yaw_setpoint = _yaw;
|
||||
|
||||
} else {
|
||||
// check reset counter and update yaw setpoint if necessary
|
||||
if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) {
|
||||
_yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
|
||||
_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// check if an external yaw handler is active and if yes, let it compute the yaw setpoints
|
||||
if (_ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()) {
|
||||
_yaw_setpoint = NAN;
|
||||
_yawspeed_setpoint += _ext_yaw_handler->get_weathervane_yawrate();
|
||||
}
|
||||
}
|
||||
|
||||
void FlightTaskManualStabilized::_updateThrustSetpoints()
|
||||
{
|
||||
/* Rotate setpoint into local frame. */
|
||||
Vector2f sp(&_sticks(0));
|
||||
_rotateIntoHeadingFrame(sp);
|
||||
|
||||
/* Ensure that maximum tilt is in [0.001, Pi] */
|
||||
float tilt_max = math::constrain(_constraints.tilt, 0.001f, M_PI_F);
|
||||
|
||||
const float x = sp(0) * tilt_max;
|
||||
const float y = sp(1) * tilt_max;
|
||||
|
||||
/* The norm of the xy stick input provides the pointing
|
||||
* direction of the horizontal desired thrust setpoint. The magnitude of the
|
||||
* xy stick inputs represents the desired tilt. Both tilt and magnitude can
|
||||
* be captured through Axis-Angle.
|
||||
*/
|
||||
/* The Axis-Angle is the perpendicular vector to xy-stick input */
|
||||
Vector2f v = Vector2f(y, -x);
|
||||
float v_norm = v.norm(); // the norm of v defines the tilt angle
|
||||
|
||||
if (v_norm > tilt_max) { // limit to the configured maximum tilt angle
|
||||
v *= tilt_max / v_norm;
|
||||
}
|
||||
|
||||
/* The final thrust setpoint is found by rotating the scaled unit vector pointing
|
||||
* upward by the Axis-Angle.
|
||||
* Make sure that the attitude can be controlled even at 0 throttle.
|
||||
*/
|
||||
Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
|
||||
_thrust_setpoint = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * math::max(_throttle, 0.0001f);
|
||||
}
|
||||
|
||||
void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)
|
||||
{
|
||||
float yaw_rotate = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
|
||||
Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
|
||||
v(0) = v_r(0);
|
||||
v(1) = v_r(1);
|
||||
}
|
||||
|
||||
void FlightTaskManualStabilized::_updateSetpoints()
|
||||
{
|
||||
_updateHeadingSetpoints();
|
||||
_updateThrustSetpoints();
|
||||
}
|
||||
|
||||
float FlightTaskManualStabilized::_throttleCurve()
|
||||
{
|
||||
// Scale stick z from [-1,1] to [min thrust, max thrust]
|
||||
float throttle = -((_sticks(2) - 1.0f) * 0.5f);
|
||||
|
||||
switch (_throttle_curve.get()) {
|
||||
case 1: // no rescaling
|
||||
return throttle;
|
||||
|
||||
default: // 0 or other: rescale to hover throttle at 0.5 stick
|
||||
if (throttle < 0.5f) {
|
||||
return (_throttle_hover.get() - _throttle_min_stabilized.get()) / 0.5f * throttle + _throttle_min_stabilized.get();
|
||||
|
||||
} else {
|
||||
return (_throttle_max.get() - _throttle_hover.get()) / 0.5f * (throttle - 1.0f) + _throttle_max.get();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool FlightTaskManualStabilized::update()
|
||||
{
|
||||
_scaleSticks();
|
||||
_updateSetpoints();
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -1,83 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 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 FlightManualStabilized.hpp
|
||||
*
|
||||
* Flight task for manual controlled attitude.
|
||||
* It generates thrust and yaw setpoints.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "FlightTaskManual.hpp"
|
||||
|
||||
class FlightTaskManualStabilized : public FlightTaskManual
|
||||
{
|
||||
public:
|
||||
FlightTaskManualStabilized() = default;
|
||||
|
||||
virtual ~FlightTaskManualStabilized() = default;
|
||||
bool activate() override;
|
||||
bool updateInitialize() override;
|
||||
bool update() override;
|
||||
|
||||
/**
|
||||
* Sets an external yaw handler which can be used to implement a different yaw control strategy.
|
||||
*/
|
||||
void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
|
||||
|
||||
protected:
|
||||
virtual void _updateSetpoints(); /**< updates all setpoints */
|
||||
void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
|
||||
virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
|
||||
void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
|
||||
|
||||
private:
|
||||
void _updateThrustSetpoints(); /**< sets thrust setpoint */
|
||||
float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */
|
||||
|
||||
float _throttle{}; /** mapped from stick z */
|
||||
|
||||
WeatherVane *_ext_yaw_handler =
|
||||
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManual,
|
||||
(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _yaw_rate_scaling, /**< scaling factor from stick to yaw rate */
|
||||
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _tilt_max_man, /**< maximum tilt allowed for manual flight */
|
||||
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _throttle_min_stabilized, /**< minimum throttle for stabilized */
|
||||
(ParamFloat<px4::params::MPC_THR_MAX>) _throttle_max, /**< maximum throttle that always has to be satisfied in flight*/
|
||||
(ParamFloat<px4::params::MPC_THR_HOVER>) _throttle_hover, /**< throttle at which vehicle is at hover equilibrium */
|
||||
(ParamInt<px4::params::MPC_THR_CURVE>) _throttle_curve /**< throttle curve behavior */
|
||||
)
|
||||
};
|
||||
Reference in New Issue
Block a user