mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 04:53:33 +08:00
PositionControl: make it self contained library
This commit is contained in:
Matthias Grob
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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(PositionControl
|
||||
PositionControl.cpp
|
||||
ControlMath.cpp
|
||||
)
|
||||
target_include_directories(PositionControl
|
||||
PUBLIC
|
||||
${CMAKE_CURRENT_SOURCE_DIR}
|
||||
)
|
||||
@@ -0,0 +1,222 @@
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ControlMath.cpp
|
||||
*/
|
||||
|
||||
#include "ControlMath.hpp"
|
||||
#include <platforms/px4_defines.h>
|
||||
#include <float.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace matrix;
|
||||
namespace ControlMath
|
||||
{
|
||||
vehicle_attitude_setpoint_s thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp)
|
||||
{
|
||||
vehicle_attitude_setpoint_s att_sp = {};
|
||||
att_sp.yaw_body = yaw_sp;
|
||||
|
||||
// desired body_z axis = -normalize(thrust_vector)
|
||||
Vector3f body_x, body_y, body_z;
|
||||
|
||||
if (thr_sp.length() > 0.00001f) {
|
||||
body_z = -thr_sp.normalized();
|
||||
|
||||
} else {
|
||||
// no thrust, set Z axis to safe value
|
||||
body_z = Vector3f(0.f, 0.f, 1.f);
|
||||
}
|
||||
|
||||
// vector of desired yaw direction in XY plane, rotated by PI/2
|
||||
Vector3f y_C(-sinf(att_sp.yaw_body), cosf(att_sp.yaw_body), 0.0f);
|
||||
|
||||
if (fabsf(body_z(2)) > 0.000001f) {
|
||||
// desired body_x axis, orthogonal to body_z
|
||||
body_x = y_C % body_z;
|
||||
|
||||
// keep nose to front while inverted upside down
|
||||
if (body_z(2) < 0.0f) {
|
||||
body_x = -body_x;
|
||||
}
|
||||
|
||||
body_x.normalize();
|
||||
|
||||
} else {
|
||||
// desired thrust is in XY plane, set X downside to construct correct matrix,
|
||||
// but yaw component will not be used actually
|
||||
body_x.zero();
|
||||
body_x(2) = 1.0f;
|
||||
}
|
||||
|
||||
// desired body_y axis
|
||||
body_y = body_z % body_x;
|
||||
|
||||
Dcmf R_sp;
|
||||
|
||||
// fill rotation matrix
|
||||
for (int i = 0; i < 3; i++) {
|
||||
R_sp(i, 0) = body_x(i);
|
||||
R_sp(i, 1) = body_y(i);
|
||||
R_sp(i, 2) = body_z(i);
|
||||
}
|
||||
|
||||
//copy quaternion setpoint to attitude setpoint topic
|
||||
Quatf q_sp = R_sp;
|
||||
q_sp.copyTo(att_sp.q_d);
|
||||
att_sp.q_d_valid = true;
|
||||
|
||||
// calculate euler angles, for logging only, must not be used for control
|
||||
Eulerf euler = R_sp;
|
||||
att_sp.roll_body = euler(0);
|
||||
att_sp.pitch_body = euler(1);
|
||||
att_sp.thrust_body[2] = -thr_sp.length();
|
||||
|
||||
return att_sp;
|
||||
}
|
||||
|
||||
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
|
||||
{
|
||||
if (Vector2f(v0 + v1).norm() <= max) {
|
||||
// vector does not exceed maximum magnitude
|
||||
return v0 + v1;
|
||||
|
||||
} else if (v0.length() >= max) {
|
||||
// the magnitude along v0, which has priority, already exceeds maximum.
|
||||
return v0.normalized() * max;
|
||||
|
||||
} else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) {
|
||||
// the two vectors are equal
|
||||
return v0.normalized() * max;
|
||||
|
||||
} else if (v0.length() < 0.001f) {
|
||||
// the first vector is 0.
|
||||
return v1.normalized() * max;
|
||||
|
||||
} else {
|
||||
// vf = final vector with ||vf|| <= max
|
||||
// s = scaling factor
|
||||
// u1 = unit of v1
|
||||
// vf = v0 + v1 = v0 + s * u1
|
||||
// constraint: ||vf|| <= max
|
||||
//
|
||||
// solve for s: ||vf|| = ||v0 + s * u1|| <= max
|
||||
//
|
||||
// Derivation:
|
||||
// For simplicity, replace v0 -> v, u1 -> u
|
||||
// v0(0/1/2) -> v0/1/2
|
||||
// u1(0/1/2) -> u0/1/2
|
||||
//
|
||||
// ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2
|
||||
// v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2
|
||||
// s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0
|
||||
//
|
||||
// quadratic equation:
|
||||
// -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a)
|
||||
//
|
||||
// b = 2 * u.dot(v)
|
||||
// a = 1 (because u is normalized)
|
||||
// c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2
|
||||
//
|
||||
// sqrt(b^2 - 4*a*c) =
|
||||
// sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2))
|
||||
//
|
||||
// s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2
|
||||
// = -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2))
|
||||
// m = u.dot(v)
|
||||
// s = -m + sqrt(m^2 - c)
|
||||
//
|
||||
//
|
||||
//
|
||||
// notes:
|
||||
// - s (=scaling factor) needs to be positive
|
||||
// - (max - ||v||) always larger than zero, otherwise it never entered this if-statement
|
||||
|
||||
Vector2f u1 = v1.normalized();
|
||||
float m = u1.dot(v0);
|
||||
float c = v0.dot(v0) - max * max;
|
||||
float s = -m + sqrtf(m * m - c);
|
||||
return v0 + u1 * s;
|
||||
}
|
||||
}
|
||||
|
||||
bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
|
||||
const Vector3f &line_a, const Vector3f &line_b, Vector3f &res)
|
||||
{
|
||||
// project center of sphere on line normalized AB
|
||||
Vector3f ab_norm = line_b - line_a;
|
||||
|
||||
if (ab_norm.length() < 0.01f) {
|
||||
return true;
|
||||
}
|
||||
|
||||
ab_norm.normalize();
|
||||
Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
|
||||
float cd_len = (sphere_c - d).length();
|
||||
|
||||
if (sphere_r > cd_len) {
|
||||
// we have triangle CDX with known CD and CX = R, find DX
|
||||
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
|
||||
|
||||
if ((sphere_c - line_b) * ab_norm > 0.0f) {
|
||||
// target waypoint is already behind us
|
||||
res = line_b;
|
||||
|
||||
} else {
|
||||
// target is in front of us
|
||||
res = d + ab_norm * dx_len; // vector A->B on line
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
|
||||
// have no roots, return D
|
||||
res = d; // go directly to line
|
||||
|
||||
// previous waypoint is still in front of us
|
||||
if ((sphere_c - line_a) * ab_norm < 0.0f) {
|
||||
res = line_a;
|
||||
}
|
||||
|
||||
// target waypoint is already behind us
|
||||
if ((sphere_c - line_b) * ab_norm > 0.0f) {
|
||||
res = line_b;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 ControlMath.hpp
|
||||
*
|
||||
* Simple functions for vector manipulation that do not fit into matrix lib.
|
||||
* These functions are specific for controls.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
|
||||
namespace ControlMath
|
||||
{
|
||||
/**
|
||||
* Converts thrust vector and yaw set-point to a desired attitude.
|
||||
* @param thr_sp a 3D vector
|
||||
* @param yaw_sp the desired yaw
|
||||
* @return vehicle_attitude_setpoints_s structure
|
||||
*/
|
||||
vehicle_attitude_setpoint_s thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp);
|
||||
|
||||
/**
|
||||
* Outputs the sum of two vectors but respecting the limits and priority.
|
||||
* The sum of two vectors are constraint such that v0 has priority over v1.
|
||||
* This means that if the length of (v0+v1) exceeds max, then it is constraint such
|
||||
* that v0 has priority.
|
||||
*
|
||||
* @param v0 a 2D vector that has priority given the maximum available magnitude.
|
||||
* @param v1 a 2D vector that less priority given the maximum available magnitude.
|
||||
* @return 2D vector
|
||||
*/
|
||||
matrix::Vector2f constrainXY(const matrix::Vector2f &v0, const matrix::Vector2f &v1, const float &max);
|
||||
|
||||
/**
|
||||
* This method was used for smoothing the corners along two lines.
|
||||
*
|
||||
* @param sphere_c
|
||||
* @param sphere_r
|
||||
* @param line_a
|
||||
* @param line_b
|
||||
* @param res
|
||||
* return boolean
|
||||
*
|
||||
* Note: this method is not used anywhere and first requires review before usage.
|
||||
*/
|
||||
bool cross_sphere_line(const matrix::Vector3f &sphere_c, const float sphere_r, const matrix::Vector3f &line_a,
|
||||
const matrix::Vector3f &line_b, matrix::Vector3f &res);
|
||||
}
|
||||
@@ -0,0 +1,357 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 PositionControl.cpp
|
||||
*/
|
||||
|
||||
#include "PositionControl.hpp"
|
||||
#include <float.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <ControlMath.hpp>
|
||||
#include <px4_defines.h>
|
||||
|
||||
using namespace matrix;
|
||||
|
||||
PositionControl::PositionControl(ModuleParams *parent) :
|
||||
ModuleParams(parent)
|
||||
{}
|
||||
|
||||
void PositionControl::updateState(const PositionControlStates &states)
|
||||
{
|
||||
_pos = states.position;
|
||||
_vel = states.velocity;
|
||||
_yaw = states.yaw;
|
||||
_vel_dot = states.acceleration;
|
||||
}
|
||||
|
||||
void PositionControl::_setCtrlFlag(bool value)
|
||||
{
|
||||
for (int i = 0; i <= 2; i++) {
|
||||
_ctrl_pos[i] = _ctrl_vel[i] = value;
|
||||
}
|
||||
}
|
||||
|
||||
bool PositionControl::updateSetpoint(const vehicle_local_position_setpoint_s &setpoint)
|
||||
{
|
||||
// by default we use the entire position-velocity control-loop pipeline (flag only for logging purpose)
|
||||
_setCtrlFlag(true);
|
||||
|
||||
_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
|
||||
_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
|
||||
_acc_sp = Vector3f(setpoint.acc_x, setpoint.acc_y, setpoint.acc_z);
|
||||
_thr_sp = Vector3f(setpoint.thrust);
|
||||
_yaw_sp = setpoint.yaw;
|
||||
_yawspeed_sp = setpoint.yawspeed;
|
||||
bool mapping_succeeded = _interfaceMapping();
|
||||
|
||||
// If full manual is required (thrust already generated), don't run position/velocity
|
||||
// controller and just return thrust.
|
||||
_skip_controller = PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))
|
||||
&& PX4_ISFINITE(_thr_sp(2));
|
||||
|
||||
return mapping_succeeded;
|
||||
}
|
||||
|
||||
void PositionControl::generateThrustYawSetpoint(const float dt)
|
||||
{
|
||||
if (_skip_controller) {
|
||||
|
||||
// Already received a valid thrust set-point.
|
||||
// Limit the thrust vector.
|
||||
float thr_mag = _thr_sp.length();
|
||||
|
||||
if (thr_mag > _param_mpc_thr_max.get()) {
|
||||
_thr_sp = _thr_sp.normalized() * _param_mpc_thr_max.get();
|
||||
|
||||
} else if (thr_mag < _param_mpc_manthr_min.get() && thr_mag > FLT_EPSILON) {
|
||||
_thr_sp = _thr_sp.normalized() * _param_mpc_manthr_min.get();
|
||||
}
|
||||
|
||||
// Just set the set-points equal to the current vehicle state.
|
||||
_pos_sp = _pos;
|
||||
_vel_sp = _vel;
|
||||
_acc_sp = _acc;
|
||||
|
||||
} else {
|
||||
_positionController();
|
||||
_velocityController(dt);
|
||||
}
|
||||
}
|
||||
|
||||
bool PositionControl::_interfaceMapping()
|
||||
{
|
||||
// if nothing is valid, then apply failsafe landing
|
||||
bool failsafe = false;
|
||||
|
||||
// Respects FlightTask interface, where NAN-set-points are of no interest
|
||||
// and do not require control. A valid position and velocity setpoint will
|
||||
// be mapped to a desired position setpoint with a feed-forward term.
|
||||
// States and setpoints which are integrals of the reference setpoint are set to 0.
|
||||
// For instance: reference is velocity-setpoint -> position and position-setpoint = 0
|
||||
// reference is thrust-setpoint -> position, velocity, position-/velocity-setpoint = 0
|
||||
for (int i = 0; i <= 2; i++) {
|
||||
|
||||
if (PX4_ISFINITE(_pos_sp(i))) {
|
||||
// Position control is required
|
||||
|
||||
if (!PX4_ISFINITE(_vel_sp(i))) {
|
||||
// Velocity is not used as feedforward term.
|
||||
_vel_sp(i) = 0.0f;
|
||||
}
|
||||
|
||||
// thrust setpoint is not supported in position control
|
||||
_thr_sp(i) = NAN;
|
||||
|
||||
// to run position control, we require valid position and velocity
|
||||
if (!PX4_ISFINITE(_pos(i)) || !PX4_ISFINITE(_vel(i))) {
|
||||
failsafe = true;
|
||||
}
|
||||
|
||||
} else if (PX4_ISFINITE(_vel_sp(i))) {
|
||||
|
||||
// Velocity controller is active without position control.
|
||||
// Set integral states and setpoints to 0
|
||||
_pos_sp(i) = _pos(i) = 0.0f;
|
||||
_ctrl_pos[i] = false; // position control-loop is not used
|
||||
|
||||
// thrust setpoint is not supported in velocity control
|
||||
_thr_sp(i) = NAN;
|
||||
|
||||
// to run velocity control, we require valid velocity
|
||||
if (!PX4_ISFINITE(_vel(i))) {
|
||||
failsafe = true;
|
||||
}
|
||||
|
||||
} else if (PX4_ISFINITE(_thr_sp(i))) {
|
||||
|
||||
// Thrust setpoint was generated from sticks directly.
|
||||
// Set all integral states and setpoints to 0
|
||||
_pos_sp(i) = _pos(i) = 0.0f;
|
||||
_vel_sp(i) = _vel(i) = 0.0f;
|
||||
_ctrl_pos[i] = _ctrl_vel[i] = false; // position/velocity control loop is not used
|
||||
|
||||
// Reset the Integral term.
|
||||
_thr_int(i) = 0.0f;
|
||||
// Don't require velocity derivative.
|
||||
_vel_dot(i) = 0.0f;
|
||||
|
||||
} else {
|
||||
// nothing is valid. do failsafe
|
||||
failsafe = true;
|
||||
}
|
||||
}
|
||||
|
||||
// ensure that vel_dot is finite, otherwise set to 0
|
||||
if (!PX4_ISFINITE(_vel_dot(0)) || !PX4_ISFINITE(_vel_dot(1))) {
|
||||
_vel_dot(0) = _vel_dot(1) = 0.0f;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_vel_dot(2))) {
|
||||
_vel_dot(2) = 0.0f;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_yawspeed_sp)) {
|
||||
// Set the yawspeed to 0 since not used.
|
||||
_yawspeed_sp = 0.0f;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_yaw_sp)) {
|
||||
// Set the yaw-sp equal the current yaw.
|
||||
// That is the best we can do and it also
|
||||
// agrees with FlightTask-interface definition.
|
||||
if (PX4_ISFINITE(_yaw)) {
|
||||
_yaw_sp = _yaw;
|
||||
|
||||
} else {
|
||||
failsafe = true;
|
||||
}
|
||||
}
|
||||
|
||||
// check failsafe
|
||||
if (failsafe) {
|
||||
// point the thrust upwards
|
||||
_thr_sp(0) = _thr_sp(1) = 0.0f;
|
||||
// throttle down such that vehicle goes down with
|
||||
// 70% of throttle range between min and hover
|
||||
_thr_sp(2) = -(_param_mpc_thr_min.get() + (_param_mpc_thr_hover.get() - _param_mpc_thr_min.get()) * 0.7f);
|
||||
// position and velocity control-loop is currently unused (flag only for logging purpose)
|
||||
_setCtrlFlag(false);
|
||||
}
|
||||
|
||||
return !(failsafe);
|
||||
}
|
||||
|
||||
void PositionControl::_positionController()
|
||||
{
|
||||
// P-position controller
|
||||
const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
|
||||
_param_mpc_z_p.get()));
|
||||
_vel_sp = vel_sp_position + _vel_sp;
|
||||
|
||||
// Constrain horizontal velocity by prioritizing the velocity component along the
|
||||
// the desired position setpoint over the feed-forward term.
|
||||
const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
|
||||
Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
|
||||
_vel_sp(0) = vel_sp_xy(0);
|
||||
_vel_sp(1) = vel_sp_xy(1);
|
||||
// Constrain velocity in z-direction.
|
||||
_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
|
||||
}
|
||||
|
||||
void PositionControl::_velocityController(const float &dt)
|
||||
{
|
||||
// Generate desired thrust setpoint.
|
||||
// PID
|
||||
// u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
|
||||
// Umin <= u_des <= Umax
|
||||
//
|
||||
// Anti-Windup:
|
||||
// u_des = _thr_sp; r = _vel_sp; y = _vel
|
||||
// u_des >= Umax and r - y >= 0 => Saturation = true
|
||||
// u_des >= Umax and r - y <= 0 => Saturation = false
|
||||
// u_des <= Umin and r - y <= 0 => Saturation = true
|
||||
// u_des <= Umin and r - y >= 0 => Saturation = false
|
||||
//
|
||||
// Notes:
|
||||
// - PID implementation is in NED-frame
|
||||
// - control output in D-direction has priority over NE-direction
|
||||
// - the equilibrium point for the PID is at hover-thrust
|
||||
// - the maximum tilt cannot exceed 90 degrees. This means that it is
|
||||
// not possible to have a desired thrust direction pointing in the positive
|
||||
// D-direction (= downward)
|
||||
// - the desired thrust in D-direction is limited by the thrust limits
|
||||
// - the desired thrust in NE-direction is limited by the thrust excess after
|
||||
// consideration of the desired thrust in D-direction. In addition, the thrust in
|
||||
// NE-direction is also limited by the maximum tilt.
|
||||
|
||||
const Vector3f vel_err = _vel_sp - _vel;
|
||||
|
||||
// Consider thrust in D-direction.
|
||||
float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) + _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
|
||||
2) - _param_mpc_thr_hover.get();
|
||||
|
||||
// The Thrust limits are negated and swapped due to NED-frame.
|
||||
float uMax = -_param_mpc_thr_min.get();
|
||||
float uMin = -_param_mpc_thr_max.get();
|
||||
|
||||
// make sure there's always enough thrust vector length to infer the attitude
|
||||
uMax = math::min(uMax, -10e-4f);
|
||||
|
||||
// Apply Anti-Windup in D-direction.
|
||||
bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
|
||||
(thrust_desired_D <= uMin && vel_err(2) <= 0.0f);
|
||||
|
||||
if (!stop_integral_D) {
|
||||
_thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;
|
||||
|
||||
// limit thrust integral
|
||||
_thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
|
||||
}
|
||||
|
||||
// Saturate thrust setpoint in D-direction.
|
||||
_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);
|
||||
|
||||
if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
|
||||
// Thrust set-point in NE-direction is already provided. Only
|
||||
// scaling by the maximum tilt is required.
|
||||
float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
|
||||
_thr_sp(0) *= thr_xy_max;
|
||||
_thr_sp(1) *= thr_xy_max;
|
||||
|
||||
} else {
|
||||
// PID-velocity controller for NE-direction.
|
||||
Vector2f thrust_desired_NE;
|
||||
thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
|
||||
thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);
|
||||
|
||||
// Get maximum allowed thrust in NE based on tilt and excess thrust.
|
||||
float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
|
||||
float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
|
||||
thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
|
||||
|
||||
// Saturate thrust in NE-direction.
|
||||
_thr_sp(0) = thrust_desired_NE(0);
|
||||
_thr_sp(1) = thrust_desired_NE(1);
|
||||
|
||||
if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
|
||||
float mag = thrust_desired_NE.length();
|
||||
_thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
|
||||
_thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
|
||||
}
|
||||
|
||||
// Use tracking Anti-Windup for NE-direction: during saturation, the integrator is used to unsaturate the output
|
||||
// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
|
||||
float arw_gain = 2.f / _param_mpc_xy_vel_p.get();
|
||||
|
||||
Vector2f vel_err_lim;
|
||||
vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
|
||||
vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;
|
||||
|
||||
// Update integral
|
||||
_thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
|
||||
_thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
|
||||
}
|
||||
}
|
||||
|
||||
void PositionControl::updateConstraints(const vehicle_constraints_s &constraints)
|
||||
{
|
||||
_constraints = constraints;
|
||||
|
||||
// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
|
||||
// constraints, then just use global constraints for the limits.
|
||||
|
||||
const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()));
|
||||
|
||||
if (!PX4_ISFINITE(constraints.tilt)
|
||||
|| !(constraints.tilt < tilt_max_radians)) {
|
||||
_constraints.tilt = tilt_max_radians;
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) {
|
||||
_constraints.speed_up = _param_mpc_z_vel_max_up.get();
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(constraints.speed_down) || !(constraints.speed_down < _param_mpc_z_vel_max_dn.get())) {
|
||||
_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(constraints.speed_xy) || !(constraints.speed_xy < _param_mpc_xy_vel_max.get())) {
|
||||
_constraints.speed_xy = _param_mpc_xy_vel_max.get();
|
||||
}
|
||||
}
|
||||
|
||||
void PositionControl::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
}
|
||||
@@ -0,0 +1,247 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 PositionControl.hpp
|
||||
*
|
||||
* A cascaded position controller for position/velocity control only.
|
||||
*/
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_constraints.h>
|
||||
#include <px4_module_params.h>
|
||||
#pragma once
|
||||
|
||||
struct PositionControlStates {
|
||||
matrix::Vector3f position;
|
||||
matrix::Vector3f velocity;
|
||||
matrix::Vector3f acceleration;
|
||||
float yaw;
|
||||
};
|
||||
|
||||
/**
|
||||
* Core Position-Control for MC.
|
||||
* This class contains P-controller for position and
|
||||
* PID-controller for velocity.
|
||||
* Inputs:
|
||||
* vehicle position/velocity/yaw
|
||||
* desired set-point position/velocity/thrust/yaw/yaw-speed
|
||||
* constraints that are stricter than global limits
|
||||
* Output
|
||||
* thrust vector and a yaw-setpoint
|
||||
*
|
||||
* If there is a position and a velocity set-point present, then
|
||||
* the velocity set-point is used as feed-forward. If feed-forward is
|
||||
* active, then the velocity component of the P-controller output has
|
||||
* priority over the feed-forward component.
|
||||
*
|
||||
* A setpoint that is NAN is considered as not set.
|
||||
* If there is a position/velocity- and thrust-setpoint present, then
|
||||
* the thrust-setpoint is ommitted and recomputed from position-velocity-PID-loop.
|
||||
*/
|
||||
class PositionControl : public ModuleParams
|
||||
{
|
||||
public:
|
||||
|
||||
PositionControl(ModuleParams *parent);
|
||||
~PositionControl() = default;
|
||||
|
||||
/**
|
||||
* Overwrites certain parameters.
|
||||
* Overwrites are required for unit-conversion.
|
||||
* This method should only be called if parameters
|
||||
* have been updated.
|
||||
*/
|
||||
void overwriteParams();
|
||||
|
||||
/**
|
||||
* Update the current vehicle state.
|
||||
* @param PositionControlStates structure
|
||||
*/
|
||||
void updateState(const PositionControlStates &states);
|
||||
|
||||
/**
|
||||
* Update the desired setpoints.
|
||||
* @param setpoint a vehicle_local_position_setpoint_s structure
|
||||
* @return true if setpoint has updated correctly
|
||||
*/
|
||||
bool updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
|
||||
|
||||
/**
|
||||
* Set constraints that are stricter than the global limits.
|
||||
* @param constraints a PositionControl structure with supported constraints
|
||||
*/
|
||||
void updateConstraints(const vehicle_constraints_s &constraints);
|
||||
|
||||
/**
|
||||
* Apply P-position and PID-velocity controller that updates the member
|
||||
* thrust, yaw- and yawspeed-setpoints.
|
||||
* @see _thr_sp
|
||||
* @see _yaw_sp
|
||||
* @see _yawspeed_sp
|
||||
* @param dt the delta-time
|
||||
*/
|
||||
void generateThrustYawSetpoint(const float dt);
|
||||
|
||||
/**
|
||||
* Set the integral term in xy to 0.
|
||||
* @see _thr_int
|
||||
*/
|
||||
void resetIntegralXY() { _thr_int(0) = _thr_int(1) = 0.0f; }
|
||||
|
||||
/**
|
||||
* Set the integral term in z to 0.
|
||||
* @see _thr_int
|
||||
*/
|
||||
void resetIntegralZ() { _thr_int(2) = 0.0f; }
|
||||
|
||||
/**
|
||||
* Get the
|
||||
* @see _thr_sp
|
||||
* @return The thrust set-point member.
|
||||
*/
|
||||
const matrix::Vector3f &getThrustSetpoint() { return _thr_sp; }
|
||||
|
||||
/**
|
||||
* Get the
|
||||
* @see _yaw_sp
|
||||
* @return The yaw set-point member.
|
||||
*/
|
||||
const float &getYawSetpoint() { return _yaw_sp; }
|
||||
|
||||
/**
|
||||
* Get the
|
||||
* @see _yawspeed_sp
|
||||
* @return The yawspeed set-point member.
|
||||
*/
|
||||
const float &getYawspeedSetpoint() { return _yawspeed_sp; }
|
||||
|
||||
/**
|
||||
* Get the
|
||||
* @see _vel_sp
|
||||
* @return The velocity set-point that was executed in the control-loop. Nan if velocity control-loop was skipped.
|
||||
*/
|
||||
const matrix::Vector3f getVelSp()
|
||||
{
|
||||
matrix::Vector3f vel_sp{};
|
||||
|
||||
for (int i = 0; i <= 2; i++) {
|
||||
if (_ctrl_vel[i]) {
|
||||
vel_sp(i) = _vel_sp(i);
|
||||
|
||||
} else {
|
||||
vel_sp(i) = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
return vel_sp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the
|
||||
* @see _pos_sp
|
||||
* @return The position set-point that was executed in the control-loop. Nan if the position control-loop was skipped.
|
||||
*/
|
||||
const matrix::Vector3f getPosSp()
|
||||
{
|
||||
matrix::Vector3f pos_sp{};
|
||||
|
||||
for (int i = 0; i <= 2; i++) {
|
||||
if (_ctrl_pos[i]) {
|
||||
pos_sp(i) = _pos_sp(i);
|
||||
|
||||
} else {
|
||||
pos_sp(i) = NAN;
|
||||
}
|
||||
}
|
||||
|
||||
return pos_sp;
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* Maps setpoints to internal-setpoints.
|
||||
* @return true if mapping succeeded.
|
||||
*/
|
||||
bool _interfaceMapping();
|
||||
|
||||
void _positionController(); /** applies the P-position-controller */
|
||||
void _velocityController(const float &dt); /** applies the PID-velocity-controller */
|
||||
void _setCtrlFlag(bool value); /**< set control-loop flags (only required for logging) */
|
||||
|
||||
matrix::Vector3f _pos{}; /**< MC position */
|
||||
matrix::Vector3f _vel{}; /**< MC velocity */
|
||||
matrix::Vector3f _vel_dot{}; /**< MC velocity derivative */
|
||||
matrix::Vector3f _acc{}; /**< MC acceleration */
|
||||
float _yaw{0.0f}; /**< MC yaw */
|
||||
matrix::Vector3f _pos_sp{}; /**< desired position */
|
||||
matrix::Vector3f _vel_sp{}; /**< desired velocity */
|
||||
matrix::Vector3f _acc_sp{}; /**< desired acceleration: not supported yet */
|
||||
matrix::Vector3f _thr_sp{}; /**< desired thrust */
|
||||
float _yaw_sp{}; /**< desired yaw */
|
||||
float _yawspeed_sp{}; /** desired yaw-speed */
|
||||
matrix::Vector3f _thr_int{}; /**< thrust integral term */
|
||||
vehicle_constraints_s _constraints{}; /**< variable constraints */
|
||||
bool _skip_controller{false}; /**< skips position/velocity controller. true for stabilized mode */
|
||||
bool _ctrl_pos[3] = {true, true, true}; /**< True if the control-loop for position was used */
|
||||
bool _ctrl_vel[3] = {true, true, true}; /**< True if the control-loop for velocity was used */
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
|
||||
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
|
||||
(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
|
||||
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min,
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
|
||||
(ParamFloat<px4::params::MPC_TILTMAX_AIR>)
|
||||
_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees
|
||||
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
|
||||
_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees
|
||||
(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_D>) _param_mpc_z_vel_d,
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_P>) _param_mpc_xy_vel_p,
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_I>) _param_mpc_xy_vel_i,
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_D>) _param_mpc_xy_vel_d
|
||||
)
|
||||
};
|
||||
Reference in New Issue
Block a user