diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index 93aca16358..0e2c0b0ef9 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -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', diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 49f61ac967..0dea1c6147 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -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 diff --git a/src/modules/mc_pos_control/GotoControl/CMakeLists.txt b/src/modules/mc_pos_control/GotoControl/CMakeLists.txt new file mode 100644 index 0000000000..3990ea905c --- /dev/null +++ b/src/modules/mc_pos_control/GotoControl/CMakeLists.txt @@ -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) diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp new file mode 100644 index 0000000000..dd05e635a3 --- /dev/null +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -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 +#include + +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); +} diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp new file mode 100644 index 0000000000..ee9d4d2a9c --- /dev/null +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -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 +#include +#include +#include +#include + +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}; +}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index a14a659549..25b4338b4e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -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; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 9c815c60ed..6f7e73e5e9 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -39,6 +39,7 @@ #include "PositionControl/PositionControl.hpp" #include "Takeoff/Takeoff.hpp" +#include "GotoControl/GotoControl.hpp" #include #include @@ -57,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -95,12 +97,14 @@ private: uORB::PublicationData _takeoff_status_pub{ORB_ID(takeoff_status)}; uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _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) _param_mpc_xy_p, @@ -176,7 +188,11 @@ private: (ParamFloat) _param_mpc_man_y_tau, (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all + (ParamFloat) _param_mpc_z_vel_all, + + (ParamFloat) _param_mpc_yawrauto_max, + (ParamFloat) _param_mpc_yawaauto_max, + (ParamFloat) _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); }; diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c index 8596cedd8b..bffaa5b513 100644 --- a/src/modules/mc_pos_control/multicopter_autonomous_params.c +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -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 * diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 273024fc18..3d16eedf31 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -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']