GotoControl: add go-to control interface to mc position controller
goto control class handles smoothing of goto setpoints, outputs trajectory setpoint for mc pos control some minor encapsulation done in mc pos control for readability new param MPC_YAWAAUTO_MAX limiting heading accelerations in heading smoother
This commit is contained in:
Thomas Stastny
Matthias Grob
parent
e47aba8bc9
commit
4b920a6628
|
|
@ -355,7 +355,7 @@ class SourceParser(object):
|
|||
'%', 'Hz', '1/s', 'mAh',
|
||||
'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m', 'rad s/m',
|
||||
'bit/s', 'B/s',
|
||||
'deg', 'deg*1e7', 'deg/s',
|
||||
'deg', 'deg*1e7', 'deg/s', 'deg/s^2',
|
||||
'celcius', 'gauss', 'gauss/s', 'gauss^2',
|
||||
'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
|
||||
'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(GotoControl)
|
||||
add_subdirectory(PositionControl)
|
||||
add_subdirectory(Takeoff)
|
||||
|
||||
|
|
@ -42,6 +43,7 @@ px4_add_module(
|
|||
MulticopterPositionControl.cpp
|
||||
MulticopterPositionControl.hpp
|
||||
DEPENDS
|
||||
GotoControl
|
||||
PositionControl
|
||||
Takeoff
|
||||
controllib
|
||||
|
|
|
|||
|
|
@ -0,0 +1,39 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023 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(GotoControl
|
||||
GotoControl.cpp
|
||||
GotoControl.hpp
|
||||
)
|
||||
target_include_directories(GotoControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(GotoControl PUBLIC PositionControl)
|
||||
|
|
@ -0,0 +1,210 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file GotoControl.cpp
|
||||
*/
|
||||
|
||||
#include "GotoControl.hpp"
|
||||
#include "PositionControl.hpp"
|
||||
|
||||
#include <float.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading,
|
||||
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint)
|
||||
{
|
||||
trajectory_setpoint = PositionControl::empty_trajectory_setpoint;
|
||||
|
||||
const Vector3f position_setpoint(goto_setpoint.position);
|
||||
|
||||
if (!position_setpoint.isAllFinite()) {
|
||||
// TODO: error messaging
|
||||
_need_smoother_reset = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (!position.isAllFinite()) {
|
||||
// TODO: error messaging
|
||||
_need_smoother_reset = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (_need_smoother_reset) {
|
||||
resetPositionSmoother(position);
|
||||
}
|
||||
|
||||
setPositionSmootherLimits(goto_setpoint);
|
||||
|
||||
const Vector3f feedforward_velocity{};
|
||||
const bool force_zero_velocity_setpoint = false;
|
||||
PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
|
||||
_position_smoother.generateSetpoints(position, position_setpoint, feedforward_velocity, dt,
|
||||
force_zero_velocity_setpoint, out_setpoints);
|
||||
|
||||
_position_smoother.getCurrentPosition().copyTo(trajectory_setpoint.position);
|
||||
_position_smoother.getCurrentVelocity().copyTo(trajectory_setpoint.velocity);
|
||||
_position_smoother.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration);
|
||||
out_setpoints.jerk.copyTo(trajectory_setpoint.jerk);
|
||||
|
||||
if (goto_setpoint.flag_control_heading && PX4_ISFINITE(goto_setpoint.heading) && PX4_ISFINITE(heading)) {
|
||||
if (!_controlling_heading || _need_smoother_reset) {
|
||||
resetHeadingSmoother(heading);
|
||||
}
|
||||
|
||||
setHeadingSmootherLimits(goto_setpoint);
|
||||
_heading_smoother.update(goto_setpoint.heading, dt);
|
||||
|
||||
trajectory_setpoint.yaw = _heading_smoother.getSmoothedHeading();
|
||||
trajectory_setpoint.yawspeed = _heading_smoother.getSmoothedHeadingRate();
|
||||
|
||||
_controlling_heading = true;
|
||||
|
||||
} else {
|
||||
// TODO: error messaging for non-finite headings
|
||||
|
||||
trajectory_setpoint.yaw = NAN;
|
||||
trajectory_setpoint.yawspeed = NAN;
|
||||
|
||||
_controlling_heading = false;
|
||||
}
|
||||
|
||||
trajectory_setpoint.timestamp = goto_setpoint.timestamp;
|
||||
|
||||
_need_smoother_reset = false;
|
||||
}
|
||||
|
||||
void GotoControl::resetPositionSmoother(const matrix::Vector3f &position)
|
||||
{
|
||||
if (!position.isAllFinite()) {
|
||||
// TODO: error messaging
|
||||
_need_smoother_reset = true;
|
||||
return;
|
||||
}
|
||||
|
||||
const Vector3f initial_acceleration{};
|
||||
const Vector3f initial_velocity{};
|
||||
_position_smoother.reset(initial_acceleration, initial_velocity, position);
|
||||
|
||||
_need_smoother_reset = false;
|
||||
}
|
||||
|
||||
void GotoControl::resetHeadingSmoother(const float heading)
|
||||
{
|
||||
if (!PX4_ISFINITE(heading)) {
|
||||
// TODO: error messaging
|
||||
_controlling_heading = false;
|
||||
return;
|
||||
}
|
||||
|
||||
const float initial_heading_rate{0.f};
|
||||
_heading_smoother.reset(heading, initial_heading_rate);
|
||||
}
|
||||
|
||||
void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint)
|
||||
{
|
||||
// constrain horizontal velocity
|
||||
float max_horizontal_speed = _vehicle_constraints.max_horizontal_speed;
|
||||
float max_horizontal_accel = _vehicle_constraints.max_horizontal_accel;
|
||||
|
||||
if (goto_setpoint.flag_set_max_horizontal_speed
|
||||
&& PX4_ISFINITE(goto_setpoint.max_horizontal_speed)) {
|
||||
max_horizontal_speed = math::constrain(goto_setpoint.max_horizontal_speed, 0.f,
|
||||
_vehicle_constraints.max_horizontal_speed);
|
||||
|
||||
// linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic
|
||||
// only limit acceleration once within velocity constraints
|
||||
if (_position_smoother.getCurrentVelocityXY().norm() <= max_horizontal_speed) {
|
||||
const float speed_scale = max_horizontal_speed / _vehicle_constraints.max_horizontal_speed;
|
||||
max_horizontal_accel = math::constrain(_vehicle_constraints.max_horizontal_accel * speed_scale,
|
||||
VelocitySmoothing::kMinAccel,
|
||||
_vehicle_constraints.max_horizontal_accel);
|
||||
}
|
||||
}
|
||||
|
||||
_position_smoother.setCruiseSpeed(max_horizontal_speed);
|
||||
_position_smoother.setMaxVelocityXY(max_horizontal_speed);
|
||||
_position_smoother.setMaxAccelerationXY(max_horizontal_accel);
|
||||
|
||||
// constrain vertical velocity
|
||||
const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoother.getCurrentPositionZ() >
|
||||
0.f;
|
||||
const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_speed :
|
||||
_vehicle_constraints.max_up_speed;
|
||||
const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_accel :
|
||||
_vehicle_constraints.max_up_accel;
|
||||
|
||||
float max_vertical_speed = vehicle_max_vertical_speed;
|
||||
float max_vertical_accel = vehicle_max_vertical_accel;
|
||||
|
||||
if (goto_setpoint.flag_set_max_vertical_speed && PX4_ISFINITE(goto_setpoint.max_vertical_speed)) {
|
||||
|
||||
max_vertical_speed = math::constrain(goto_setpoint.max_vertical_speed, 0.f, vehicle_max_vertical_speed);
|
||||
|
||||
// linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic
|
||||
// only limit acceleration once within velocity constraints
|
||||
if (fabsf(_position_smoother.getCurrentVelocityZ()) <= max_vertical_speed) {
|
||||
const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed;
|
||||
max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, VelocitySmoothing::kMinAccel,
|
||||
vehicle_max_vertical_accel);
|
||||
}
|
||||
}
|
||||
|
||||
_position_smoother.setMaxVelocityZ(max_vertical_speed);
|
||||
_position_smoother.setMaxAccelerationZ(max_vertical_accel);
|
||||
|
||||
_position_smoother.setMaxJerkXY(_vehicle_constraints.max_jerk);
|
||||
_position_smoother.setMaxJerkZ(_vehicle_constraints.max_jerk);
|
||||
}
|
||||
|
||||
void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint)
|
||||
{
|
||||
float max_heading_rate = _vehicle_constraints.max_heading_rate;
|
||||
float max_heading_accel = _vehicle_constraints.max_heading_accel;
|
||||
|
||||
if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) {
|
||||
max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, HeadingSmoother::kMinHeadingRate,
|
||||
_vehicle_constraints.max_heading_rate);
|
||||
|
||||
// linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic
|
||||
// only limit acceleration once within velocity constraints
|
||||
if (fabsf(_heading_smoother.getSmoothedHeadingRate()) <= max_heading_rate) {
|
||||
const float rate_scale = max_heading_rate / _vehicle_constraints.max_heading_rate;
|
||||
max_heading_accel = math::constrain(_vehicle_constraints.max_heading_accel * rate_scale,
|
||||
HeadingSmoother::kMinHeadingAccel, _vehicle_constraints.max_heading_accel);
|
||||
}
|
||||
}
|
||||
|
||||
_heading_smoother.setMaxHeadingRate(max_heading_rate);
|
||||
_heading_smoother.setMaxHeadingAccel(max_heading_accel);
|
||||
}
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 GotoControl.hpp
|
||||
*
|
||||
* A class which smooths position and heading references from "go-to" setpoints
|
||||
* for planar multicopters.
|
||||
*
|
||||
* Be sure to set constraints with setVehicleConstraints() before calling the update() method for the first time, otherwise
|
||||
* the default motion will be VERY slow.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
#include <lib/motion_planning/PositionSmoothing.hpp>
|
||||
#include <lib/motion_planning/HeadingSmoother.hpp>
|
||||
#include <uORB/topics/goto_setpoint.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
|
||||
class GotoControl
|
||||
{
|
||||
public:
|
||||
GotoControl() = default;
|
||||
~GotoControl() = default;
|
||||
|
||||
/**
|
||||
* @brief struct containing maximum vehicle translational and rotational constraints
|
||||
*
|
||||
*/
|
||||
struct VehicleConstraints {
|
||||
float max_horizontal_speed = kMinSpeed; // [m/s]
|
||||
float max_down_speed = kMinSpeed; // [m/s]
|
||||
float max_up_speed = kMinSpeed; // [m/s]
|
||||
float max_horizontal_accel = VelocitySmoothing::kMinAccel; // [m/s^2]
|
||||
float max_down_accel = VelocitySmoothing::kMinAccel; // [m/s^2]
|
||||
float max_up_accel = VelocitySmoothing::kMinAccel; // [m/s^2]
|
||||
float max_jerk = VelocitySmoothing::kMinJerk; // [m/s^3]
|
||||
float max_heading_rate = HeadingSmoother::kMinHeadingRate; // [rad/s]
|
||||
float max_heading_accel = HeadingSmoother::kMinHeadingAccel; // [rad/s^2]
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level
|
||||
* loops to track.
|
||||
*
|
||||
* @param[in] dt [s] time since last control update
|
||||
* @param[in] position [m] (NED) local vehicle position
|
||||
* @param[in] heading [rad] (from North) vehicle heading
|
||||
* @param[in] goto_setpoint struct containing current go-to setpoints
|
||||
* @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints
|
||||
*/
|
||||
void update(const float dt, const matrix::Vector3f &position, const float heading,
|
||||
const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint);
|
||||
|
||||
/**
|
||||
* @brief resets the position smoother at the current position with zero velocity and acceleration.
|
||||
*
|
||||
* @param position [m] (NED) local vehicle position
|
||||
*/
|
||||
void resetPositionSmoother(const matrix::Vector3f &position);
|
||||
|
||||
/**
|
||||
* @brief resets the heading smoother at the current heading with zero heading rate and acceleration.
|
||||
*
|
||||
* @param heading [rad] (from North) vehicle heading
|
||||
*/
|
||||
void resetHeadingSmoother(const float heading);
|
||||
|
||||
/**
|
||||
* @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively
|
||||
* overriden (e.g. slowed down) via the speed scalers in the go-to setpoint.
|
||||
*
|
||||
* @param vehicle_constraints Struct containing desired vehicle constraints
|
||||
*/
|
||||
void setVehicleConstraints(const VehicleConstraints &vehicle_constraints)
|
||||
{
|
||||
_vehicle_constraints.max_horizontal_speed = math::max(kMinSpeed, vehicle_constraints.max_horizontal_speed);
|
||||
_vehicle_constraints.max_down_speed = math::max(kMinSpeed, vehicle_constraints.max_down_speed);
|
||||
_vehicle_constraints.max_up_speed = math::max(kMinSpeed, vehicle_constraints.max_up_speed);
|
||||
_vehicle_constraints.max_horizontal_accel = math::max(VelocitySmoothing::kMinAccel,
|
||||
vehicle_constraints.max_horizontal_accel);
|
||||
_vehicle_constraints.max_down_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_down_accel);
|
||||
_vehicle_constraints.max_up_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_up_accel);
|
||||
_vehicle_constraints.max_jerk = math::max(VelocitySmoothing::kMinJerk, vehicle_constraints.max_jerk);
|
||||
_vehicle_constraints.max_heading_rate = math::max(HeadingSmoother::kMinHeadingRate,
|
||||
vehicle_constraints.max_heading_rate);
|
||||
_vehicle_constraints.max_heading_accel = math::max(HeadingSmoother::kMinHeadingAccel,
|
||||
vehicle_constraints.max_heading_accel);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set the position smoother's maximum allowed horizontal position error at which trajectory integration halts
|
||||
*
|
||||
* @param error [m] horizontal position error
|
||||
*/
|
||||
void setMaxAllowedHorizontalPositionError(const float error)
|
||||
{
|
||||
_position_smoother.setMaxAllowedHorizontalError(error);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
// [m/s] minimum value of the maximum translational speeds
|
||||
static constexpr float kMinSpeed{0.1f};
|
||||
|
||||
/**
|
||||
* @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration
|
||||
*
|
||||
* @param goto_setpoint struct containing current go-to setpoints
|
||||
*/
|
||||
void setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint);
|
||||
|
||||
/**
|
||||
* @brief optionally sets a dynamic heading rate limit with corresponding scale on heading acceleration
|
||||
*
|
||||
* @param goto_setpoint struct containing current go-to setpoints
|
||||
*/
|
||||
void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint);
|
||||
|
||||
VehicleConstraints _vehicle_constraints{};
|
||||
PositionSmoothing _position_smoother;
|
||||
HeadingSmoother _heading_smoother;
|
||||
|
||||
// flags that the next update() requires a valid current vehicle position to reset the smoothers
|
||||
bool _need_smoother_reset{true};
|
||||
|
||||
// flags if the last update() was controlling heading
|
||||
bool _controlling_heading{false};
|
||||
};
|
||||
|
|
@ -368,52 +368,57 @@ void MulticopterPositionControl::Run()
|
|||
}
|
||||
}
|
||||
|
||||
_trajectory_setpoint_sub.update(&_setpoint);
|
||||
|
||||
// adjust existing (or older) setpoint with any EKF reset deltas
|
||||
if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
|
||||
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
|
||||
_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
|
||||
_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
|
||||
_setpoint.velocity[2] += vehicle_local_position.delta_vz;
|
||||
}
|
||||
|
||||
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
|
||||
_setpoint.position[0] += vehicle_local_position.delta_xy[0];
|
||||
_setpoint.position[1] += vehicle_local_position.delta_xy[1];
|
||||
}
|
||||
|
||||
if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
|
||||
_setpoint.position[2] += vehicle_local_position.delta_z;
|
||||
}
|
||||
|
||||
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
|
||||
_setpoint.yaw = wrap_pi(_setpoint.yaw + vehicle_local_position.delta_heading);
|
||||
}
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
|
||||
_vel_x_deriv.reset();
|
||||
_vel_y_deriv.reset();
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
|
||||
_vel_z_deriv.reset();
|
||||
}
|
||||
|
||||
// save latest reset counters
|
||||
_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
|
||||
_vz_reset_counter = vehicle_local_position.vz_reset_counter;
|
||||
_xy_reset_counter = vehicle_local_position.xy_reset_counter;
|
||||
_z_reset_counter = vehicle_local_position.z_reset_counter;
|
||||
_heading_reset_counter = vehicle_local_position.heading_reset_counter;
|
||||
|
||||
|
||||
PositionControlStates states{set_vehicle_states(vehicle_local_position)};
|
||||
|
||||
_trajectory_setpoint_sub.update(&_setpoint);
|
||||
|
||||
const hrt_abstime last_goto_timestamp = _goto_setpoint.timestamp;
|
||||
_goto_setpoint_sub.update(&_goto_setpoint);
|
||||
|
||||
if ((_goto_setpoint.timestamp != 0)
|
||||
&& (_goto_setpoint.timestamp >= _time_position_control_enabled)
|
||||
&& (hrt_elapsed_time(&last_goto_timestamp) < 200_ms)
|
||||
&& _vehicle_control_mode.flag_multicopter_position_control_enabled) {
|
||||
// take position heading setpoint as priority over trajectory setpoint
|
||||
|
||||
if (_last_active_setpoint_interface == SetpointInterface::kTrajectory) {
|
||||
_goto_control.resetPositionSmoother(states.position);
|
||||
_goto_control.resetHeadingSmoother(states.yaw);
|
||||
}
|
||||
|
||||
const GotoControl::VehicleConstraints vehicle_constraints = {
|
||||
_param_mpc_xy_cruise.get(),
|
||||
_param_mpc_z_v_auto_dn.get(),
|
||||
_param_mpc_z_v_auto_up.get(),
|
||||
_param_mpc_acc_hor.get(),
|
||||
_param_mpc_acc_down_max.get(),
|
||||
_param_mpc_acc_up_max.get(),
|
||||
_param_mpc_jerk_auto.get(),
|
||||
_param_mpc_yawrauto_max.get(),
|
||||
_param_mpc_yawaauto_max.get()
|
||||
};
|
||||
|
||||
_goto_control.setVehicleConstraints(vehicle_constraints);
|
||||
_goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get());
|
||||
|
||||
_goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);
|
||||
|
||||
// for logging
|
||||
_trajectory_setpoint_pub.publish(_setpoint);
|
||||
|
||||
_vehicle_constraints.timestamp = hrt_absolute_time();
|
||||
_vehicle_constraints.want_takeoff = false;
|
||||
_vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get();
|
||||
_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
|
||||
|
||||
_last_active_setpoint_interface = SetpointInterface::kGoto;
|
||||
|
||||
} else {
|
||||
_last_active_setpoint_interface = SetpointInterface::kTrajectory;
|
||||
_vehicle_constraints_sub.update(&_vehicle_constraints);
|
||||
}
|
||||
|
||||
adjustSetpointForEKFResets(vehicle_local_position, _setpoint);
|
||||
|
||||
if (_vehicle_control_mode.flag_multicopter_position_control_enabled) {
|
||||
// set failsafe setpoint if there hasn't been a new
|
||||
|
|
@ -428,9 +433,6 @@ void MulticopterPositionControl::Run()
|
|||
if (_vehicle_control_mode.flag_multicopter_position_control_enabled
|
||||
&& (_setpoint.timestamp >= _time_position_control_enabled)) {
|
||||
|
||||
// update vehicle constraints and handle smooth takeoff
|
||||
_vehicle_constraints_sub.update(&_vehicle_constraints);
|
||||
|
||||
// fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN
|
||||
// TODO: this should get obsolete once the takeoff limiting moves into the flight tasks
|
||||
if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) {
|
||||
|
|
@ -633,6 +635,50 @@ trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const
|
|||
return failsafe_setpoint;
|
||||
}
|
||||
|
||||
void MulticopterPositionControl::adjustSetpointForEKFResets(const vehicle_local_position_s &vehicle_local_position,
|
||||
trajectory_setpoint_s &setpoint)
|
||||
{
|
||||
if ((setpoint.timestamp != 0) && (setpoint.timestamp < vehicle_local_position.timestamp)) {
|
||||
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
|
||||
setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
|
||||
setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
|
||||
setpoint.velocity[2] += vehicle_local_position.delta_vz;
|
||||
}
|
||||
|
||||
if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
|
||||
setpoint.position[0] += vehicle_local_position.delta_xy[0];
|
||||
setpoint.position[1] += vehicle_local_position.delta_xy[1];
|
||||
}
|
||||
|
||||
if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
|
||||
setpoint.position[2] += vehicle_local_position.delta_z;
|
||||
}
|
||||
|
||||
if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
|
||||
setpoint.yaw = wrap_pi(setpoint.yaw + vehicle_local_position.delta_heading);
|
||||
}
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
|
||||
_vel_x_deriv.reset();
|
||||
_vel_y_deriv.reset();
|
||||
}
|
||||
|
||||
if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
|
||||
_vel_z_deriv.reset();
|
||||
}
|
||||
|
||||
// save latest reset counters
|
||||
_vxy_reset_counter = vehicle_local_position.vxy_reset_counter;
|
||||
_vz_reset_counter = vehicle_local_position.vz_reset_counter;
|
||||
_xy_reset_counter = vehicle_local_position.xy_reset_counter;
|
||||
_z_reset_counter = vehicle_local_position.z_reset_counter;
|
||||
_heading_reset_counter = vehicle_local_position.heading_reset_counter;
|
||||
}
|
||||
|
||||
int MulticopterPositionControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
bool vtol = false;
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@
|
|||
|
||||
#include "PositionControl/PositionControl.hpp"
|
||||
#include "Takeoff/Takeoff.hpp"
|
||||
#include "GotoControl/GotoControl.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/controllib/blocks.hpp>
|
||||
|
|
@ -57,6 +58,7 @@
|
|||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/hover_thrust_estimate.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/goto_setpoint.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_constraints.h>
|
||||
|
|
@ -95,12 +97,14 @@ private:
|
|||
uORB::PublicationData<takeoff_status_s> _takeoff_status_pub{ORB_ID(takeoff_status)};
|
||||
uORB::Publication<vehicle_attitude_setpoint_s> _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
|
||||
uORB::Publication<vehicle_local_position_setpoint_s> _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */
|
||||
uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
|
||||
uORB::Subscription _goto_setpoint_sub{ORB_ID(goto_setpoint)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
|
|
@ -109,6 +113,7 @@ private:
|
|||
hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */
|
||||
hrt_abstime _time_position_control_enabled{0};
|
||||
|
||||
goto_setpoint_s _goto_setpoint{};
|
||||
trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint};
|
||||
vehicle_control_mode_s _vehicle_control_mode{};
|
||||
|
||||
|
|
@ -127,6 +132,13 @@ private:
|
|||
.landed = true,
|
||||
};
|
||||
|
||||
enum SetpointInterface {
|
||||
kTrajectory = 0,
|
||||
kGoto
|
||||
} _last_active_setpoint_interface{kTrajectory};
|
||||
|
||||
GotoControl _goto_control;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
// Position Control
|
||||
(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
|
||||
|
|
@ -176,7 +188,11 @@ private:
|
|||
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
|
||||
|
||||
(ParamFloat<px4::params::MPC_XY_VEL_ALL>) _param_mpc_xy_vel_all,
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all
|
||||
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all,
|
||||
|
||||
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
|
||||
(ParamFloat<px4::params::MPC_YAWAAUTO_MAX>) _param_mpc_yawaauto_max,
|
||||
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max
|
||||
);
|
||||
|
||||
control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
|
||||
|
|
@ -225,4 +241,13 @@ private:
|
|||
* This should only happen briefly when transitioning and never during mode operation or by design.
|
||||
*/
|
||||
trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn);
|
||||
|
||||
/**
|
||||
* @brief adjust existing (or older) setpoint with any EKF reset deltas and update the local counters
|
||||
*
|
||||
* @param[in] vehicle_local_position struct containing EKF reset deltas and counters
|
||||
* @param[out] setpoint trajectory setpoint struct to be adjusted
|
||||
*/
|
||||
void adjustSetpointForEKFResets(const vehicle_local_position_s &vehicle_local_position,
|
||||
trajectory_setpoint_s &setpoint);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -139,7 +139,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f);
|
|||
* control output and mixer saturation.
|
||||
*
|
||||
* @unit deg/s
|
||||
* @min 0
|
||||
* @min 5
|
||||
* @max 360
|
||||
* @decimal 0
|
||||
* @increment 5
|
||||
|
|
@ -147,6 +147,21 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f);
|
|||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f);
|
||||
|
||||
/**
|
||||
* Max yaw acceleration in autonomous modes
|
||||
*
|
||||
* Limits the acceleration of the yaw setpoint to avoid large
|
||||
* control output and mixer saturation.
|
||||
*
|
||||
* @unit deg/s^2
|
||||
* @min 5
|
||||
* @max 360
|
||||
* @decimal 0
|
||||
* @increment 5
|
||||
* @group Multicopter Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MPC_YAWAAUTO_MAX, 60.f);
|
||||
|
||||
/**
|
||||
* Heading behavior in autonomous modes
|
||||
*
|
||||
|
|
|
|||
|
|
@ -145,14 +145,14 @@ parameters:
|
|||
'%', 'Hz', 'mAh',
|
||||
'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m','rad s/m',
|
||||
'bit/s', 'B/s',
|
||||
'deg', 'deg*1e7', 'deg/s',
|
||||
'deg', 'deg*1e7', 'deg/s', 'deg/s^2',
|
||||
'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2',
|
||||
'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
|
||||
'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
|
||||
'Ohm', 'V', 'A',
|
||||
'us', 'ms', 's',
|
||||
'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad',
|
||||
'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
|
||||
'm/s^3/sqrt(Hz)', 'm/s/sqrt(Hz)', 's/(1000*PWM)', '%m/s', 'min', 'us/C',
|
||||
'N/(m/s)', 'Nm/(rad/s)', 'Nm', 'N',
|
||||
'rpm',
|
||||
'normalized_thrust/s', 'normalized_thrust', 'norm', 'SD']
|
||||
|
|
|
|||
Loading…
Reference in New Issue