From 4d003fd7826d071a9e50197f51a12eb900ce9581 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 18 Oct 2024 09:29:41 +0200 Subject: [PATCH] MPC: LAND: create Stop motion primitive to also be used elsewhere --- .../flight_mode_manager/FlightModeManager.cpp | 4 +- .../tasks/Auto/FlightTaskAuto.cpp | 2 +- .../tasks/Land/FlightTaskLand.cpp | 128 ++---------------- .../tasks/Land/FlightTaskLand.hpp | 26 +--- .../tasks/Utility/CMakeLists.txt | 1 + .../tasks/Utility/Stop.cpp | 125 +++++++++++++++++ .../tasks/Utility/Stop.hpp | 108 +++++++++++++++ .../GotoControl/GotoControl.cpp | 1 + .../MulticopterPositionControl.cpp | 1 + .../MulticopterPositionControl.hpp | 1 + src/modules/navigator/land.cpp | 2 +- 11 files changed, 260 insertions(+), 139 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/Utility/Stop.cpp create mode 100644 src/modules/flight_mode_manager/tasks/Utility/Stop.hpp diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 4e652efedf43..4dac6027cfdc 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -153,7 +153,7 @@ void FlightModeManager::start_flight_task() bool matching_task_running = true; bool task_failure = false; const bool nav_state_descend = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND); - const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_LAND); + const bool nav_state_land = (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND); // Follow me if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) { @@ -400,7 +400,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index) if (!isAnyTaskActive()) { // no task running - return FlightTaskError::NoE rror; + return FlightTaskError::NoError; } // activation failed diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 4ab6e4a61a1a..fe34719c98c1 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -816,7 +816,7 @@ void FlightTaskAuto::_updateTrajConstraints() // correction required by the altitude/vertical position controller _constraints.speed_down = 1.2f * _param_mpc_z_v_auto_dn.get(); _constraints.speed_up = 1.2f * _param_mpc_xy_vel_max.get(); - _constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get();; + _constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get(); if (_is_emergency_braking_active) { // When initializing with large velocity, allow 1g of diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp index 6b4fd557d5e0..5efb4f2ba84b 100644 --- a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp +++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp @@ -59,19 +59,14 @@ FlightTaskLand::activate(const trajectory_setpoint_s &last_setpoint) } _yaw_setpoint = _land_heading = _yaw; // set the yaw setpoint to the current yaw - _position_smoothing.reset(pos_prev, vel_prev, accel_prev); - _acceleration_setpoint = accel_prev; _velocity_setpoint = vel_prev; _position_setpoint = pos_prev; + _stop.initialize(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime); - - // Initialize the Landing locations and parameters - // calculate where to land based on the current velocity and acceleration constraints - _CalculateBrakingLocation(); - _initial_land_position = _land_position; + _initial_land_position = _land_position = _stop.getStopPosition(); return ret; } @@ -80,8 +75,6 @@ void FlightTaskLand::reActivate() { FlightTask::reActivate(); - // On ground, reset acceleration and velocity to zero - _position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position); } bool @@ -89,12 +82,7 @@ FlightTaskLand::update() { bool ret = FlightTask::update(); - if (!_is_initialized) { - _position_smoothing.reset(_acceleration_setpoint, _velocity, _position); - _is_initialized = true; - } - - if (_velocity.norm() < 0.1f * _param_mpc_xy_vel_max.get() && !_landing) { + if (!_stop.isActive() && !_landing) { _landing = true; } @@ -102,7 +90,7 @@ FlightTaskLand::update() _PerformLanding(); } else { - _SmoothBrakingPath(); + _PerformBraking(); } return ret; @@ -180,107 +168,19 @@ FlightTaskLand::_PerformLanding() _gear.landing_gear = landing_gear_s::GEAR_DOWN; } -void -FlightTaskLand::_SmoothBrakingPath() -{ - PositionSmoothing::PositionSmoothingSetpoints out_setpoints; - - _HandleHighVelocities(); - - _position_smoothing.generateSetpoints( - _position, - _land_position, - Vector3f{0.f, 0.f, 0.f}, - _deltatime, - false, - out_setpoints - ); - - _jerk_setpoint = out_setpoints.jerk; - _acceleration_setpoint = out_setpoints.acceleration; - _velocity_setpoint = out_setpoints.velocity; - _position_setpoint = out_setpoints.position; - _yaw_setpoint = _land_heading; -} - -void -FlightTaskLand::_CalculateBrakingLocation() -{ - // Calculate the 3D point where we until where we can slow down smoothly and then land based on the current velocities and system constraints on jerk and acceleration. - _UpdateTrajConstraints(); - float delay_scale = 0.4f; // delay scale factor - const float velocity_hor_abs = sqrtf(_velocity(0) * _velocity(0) + _velocity(1) * _velocity(1)); - const float braking_dist_xy = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs, - _param_mpc_jerk_auto.get(), _param_mpc_acc_hor.get(), delay_scale * _param_mpc_jerk_auto.get()); - float braking_dist_z = 0.0f; - - if (_velocity(2) < -0.1f) { - braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2), - _param_mpc_jerk_max.get(), _param_mpc_acc_down_max.get(), 0.f); - - } else if (_velocity(2) > 0.1f) { - braking_dist_z = math::trajectory::computeBrakingDistanceFromVelocity(_velocity(2), - _param_mpc_jerk_max.get(), _param_mpc_acc_up_max.get(), 0.f); - } - - const Vector3f braking_dir = _velocity.unit_or_zero(); - const Vector3f braking_dist = {braking_dist_xy, braking_dist_xy, braking_dist_z}; - _land_position = _position + braking_dir.emult(braking_dist); -} - - -void -FlightTaskLand::_HandleHighVelocities() -{ - // This logic here is to fix the problem that the trajectory generator will generate a smoot trajectory from the current velocity to zero velocity, - // but if the velocity is too high, the Position Controller will be able to comand higher accelerations than set by the parameter which means the vehicle will break faster than expected predicted by the trajectory generator. - // But if we then do a reset the deceleration will be smooth again. - const bool _exceeded_vel_z = fabsf(_velocity(2)) > math::max(_param_mpc_z_v_auto_dn.get(), - _param_mpc_z_vel_max_up.get()); - const bool _exceeded_vel_xy = _velocity.xy().norm() > _param_mpc_xy_vel_max.get(); - - if ((_exceeded_vel_xy || _exceeded_vel_z) && !_exceeded_max_vel) { - _exceeded_max_vel = true; - - } else if ((!_exceeded_vel_xy && !_exceeded_vel_z) && _exceeded_max_vel) { - // This Reset will be called when the velocity is again in the normal range and will be called only once. - _exceeded_max_vel = false; - _position_smoothing.reset(_acceleration_setpoint, _velocity, _position); - _CalculateBrakingLocation(); - _initial_land_position = _land_position; - } -} void -FlightTaskLand::_UpdateTrajConstraints() +FlightTaskLand::_PerformBraking() { - // update params of the position smoothing - _position_smoothing.setCruiseSpeed(_param_mpc_xy_vel_max.get()); - _position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get()); - _position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get()); - _position_smoothing.setTargetAcceptanceRadius(_param_nav_mc_alt_rad.get()); - _position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get()); - - // Update the constraints of the trajectories - _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); - _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); - _position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get()); - _position_smoothing.setMaxJerkZ(_param_mpc_jerk_max.get()); - - // set the constraints for the vertical direction - // if moving up, acceleration constraint is always in deceleration direction, eg opposite to the velocity - if (_velocity(2) < 0.0f && !_landing) { - _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); - _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get()); - - } else if (!_landing) { - _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get()); - _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); - - } else { - _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); - _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); - } + _stop.getConstraints(_constraints); + _stop.update(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime); + + _initial_land_position = _land_position = _stop.getStopPosition(); + _jerk_setpoint = _stop.getJerkSetpoint(); + _acceleration_setpoint = _stop.getAccelerationSetpoint(); + _velocity_setpoint = _stop.getVelocitySetpoint(); + _position_setpoint = _stop.getPositionSetpoint(); + _yaw_setpoint = _land_heading; } void diff --git a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp index 0e930f9a07cd..6d0f87a20cbb 100644 --- a/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp +++ b/src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp @@ -49,6 +49,7 @@ #include "Sticks.hpp" #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" +#include "Stop.hpp" using matrix::Vector3f; using matrix::Vector2f; @@ -66,49 +67,32 @@ class FlightTaskLand : public FlightTask protected: void updateParams() override; - PositionSmoothing _position_smoothing; Sticks _sticks{this}; StickAccelerationXY _stick_acceleration_xy{this}; StickYaw _stick_yaw{this}; + Stop _stop{this}; uORB::SubscriptionData _sub_home_position{ORB_ID(home_position)}; uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; uORB::SubscriptionData _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, - (ParamFloat) _param_mpc_acc_down_max, - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_land_alt1, (ParamFloat) _param_mpc_land_alt2, (ParamFloat) _param_mpc_land_alt3, (ParamFloat) _param_mpc_land_crawl_speed, (ParamFloat) _param_mpc_land_radius, (ParamInt) _param_mpc_land_rc_help, - (ParamFloat) _param_mpc_land_speed, - (ParamFloat) _param_mpc_xy_err_max, - (ParamFloat) _param_mpc_xy_traj_p, - (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_z_v_auto_dn, - (ParamFloat) _param_mpc_z_v_auto_up, - (ParamFloat) _param_nav_mc_alt_rad + (ParamFloat) _param_mpc_land_speed ); private: - void _CalculateBrakingLocation(); - void _HandleHighVelocities(); void _PerformLanding(); - void _SmoothBrakingPath(); - void _UpdateTrajConstraints(); + void _PerformBraking(); bool _landing{false}; - bool _is_initialized{false}; - bool _exceeded_max_vel{false}; Vector3f _initial_land_position; Vector3f _land_position; float _land_heading{0.0f}; - - }; diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 911da11d2682..9b4af7d40af3 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp + Stop.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Stop.cpp b/src/modules/flight_mode_manager/tasks/Utility/Stop.cpp new file mode 100644 index 000000000000..df2e19422a9a --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Stop.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ +#include "Stop.hpp" + +Stop::Stop(ModuleParams *parent) : + ModuleParams(parent) +{ + // update params of the position smoothing + _position_smoothing.setCruiseSpeed(_param_mpc_xy_vel_max.get()); + _position_smoothing.setHorizontalTrajectoryGain(_param_mpc_xy_traj_p.get()); + _position_smoothing.setMaxAllowedHorizontalError(_param_mpc_xy_err_max.get()); + _position_smoothing.setTargetAcceptanceRadius(_param_nav_mc_alt_rad.get()); + _position_smoothing.setVerticalAcceptanceRadius(_param_nav_mc_alt_rad.get()); + + // Update the constraints of the trajectories + _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); + _position_smoothing.setMaxVelocityZ(math::max(_param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get())); + _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); + _position_smoothing.setMaxAccelerationZ(math::max(_param_mpc_acc_down_max.get(), _param_mpc_acc_up_max.get())); + _position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get()); + _position_smoothing.setMaxJerkZ(_param_mpc_jerk_max.get()); +} + +void Stop::getConstraints(vehicle_constraints_s &constraints) +{ + constraints.speed_down = 1.2f * _param_mpc_z_vel_max_dn.get(); + constraints.speed_up = 1.2f * _param_mpc_z_vel_max_up.get(); + constraints.speed_xy = 1.2f * _param_mpc_xy_vel_max.get(); + _position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get()); + _position_smoothing.setMaxJerkZ(_param_mpc_jerk_max.get()); + + if (_exceeded_max_vel) { + constraints.speed_down = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), constraints.speed_down); + constraints.speed_up = math::max(fabsf(_position_smoothing.getCurrentVelocityZ()), constraints.speed_up); + constraints.speed_xy = math::max(_position_smoothing.getCurrentVelocityXY().norm(), constraints.speed_xy); + + _position_smoothing.setMaxAccelerationXY(CONSTANTS_ONE_G); + _position_smoothing.setMaxAccelerationZ(2 * CONSTANTS_ONE_G); + _position_smoothing.setMaxJerk(CONSTANTS_ONE_G); + _position_smoothing.setMaxJerkZ(10.f * CONSTANTS_ONE_G); // Jerk in Z direction is only limited by motor inertia. + } +} + +void +Stop::initialize(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position, + const float &deltatime) +{ + if (velocity.isAllNan() || position.isAllNan() || acceleration.isAllNan()) { + PX4_ERR("Initialize stop with valid values"); + } + + _isActive = true; + _position_smoothing.reset(acceleration, velocity, position); + update(acceleration, velocity, position, deltatime); +} + +void +Stop::update(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position, const float &deltatime) +{ + if (checkMaxVelocityLimit(velocity) && !_exceeded_max_vel) { + _exceeded_max_vel = true; + + } else if (_position_smoothing.getCurrentVelocityZ() < 0.01f + && _position_smoothing.getCurrentVelocityZ() > -0.01f + && !_position_smoothing.getCurrentVelocityXY().longerThan(0.01f)) { + _isActive = false; + _stop_position = position; + } + + PositionSmoothing::PositionSmoothingSetpoints out_setpoints; + // Generate the setpoints + _position_smoothing.generateSetpoints( + position, + _stop_position, + Vector3f{0.f, 0.f, 0.f}, + deltatime, + true, + out_setpoints + ); + _jerk_setpoint = out_setpoints.jerk; + _acceleration_setpoint = out_setpoints.acceleration; + _velocity_setpoint = out_setpoints.velocity; + _position_setpoint = out_setpoints.position; + _unsmoothed_velocity = out_setpoints.unsmoothed_velocity; +} + +bool +Stop::checkMaxVelocityLimit(const Vector3f &velocity, const float &factor) +{ + const bool exceeded_vel_z = fabsf(velocity(2)) > math::max(_param_mpc_z_vel_max_dn.get(), + _param_mpc_z_vel_max_up.get()); + const bool exceeded_vel_xy = velocity.xy().norm() > _param_mpc_xy_vel_max.get(); + + return (exceeded_vel_xy || exceeded_vel_z); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp b/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp new file mode 100644 index 000000000000..3bcb95ac1d01 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Stop.hpp @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 StickYaw.hpp + * @brief Generate setpoints to Stop the vehicle smoothly from any given initial velocity + * @author Claudio Chies + */ +#include +#include +#include +#include +#include +#include +#include + +using matrix::Vector3f; +using matrix::Vector2f; +#pragma once +/** + * This utility class generates setpoints which slow down the vehicle smoothly from any given initial velocity + * to use it, first call initialize(), and then update() at every iteration. + * it is important to use the getConstraints() to adjust the PositionController constraints if we exceed the maximum velocity in the + * current flighttask. + */ +class Stop : public ModuleParams +{ +public: + Stop(ModuleParams *parent); + ~Stop() = default; + + void initialize(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position, + const float &deltatime); + void update(const Vector3f &acceleration, const Vector3f &velocity, const Vector3f &position, const float &deltatime); + + /** @brief the the current constraints based on the current state of the vehicle, and update the constraints in the FlightTask by passing the reference + */ + void getConstraints(vehicle_constraints_s &constraints); + + bool checkMaxVelocityLimit(const Vector3f &velocity, const float &factor = 1.0f); + + // Getters + Vector3f getPositionSetpoint() const { return _position_setpoint; } + Vector3f getVelocitySetpoint() const { return _velocity_setpoint; } + Vector3f getAccelerationSetpoint() const { return _acceleration_setpoint; } + Vector3f getJerkSetpoint() const { return _jerk_setpoint; } + Vector3f getUnsmoothedVelocity() const { return _unsmoothed_velocity; } + Vector3f getStopPosition() const { return _stop_position; } + + bool isActive() const {return _isActive;}; +private: + + PositionSmoothing _position_smoothing; + + Vector3f _stop_position; + bool _exceeded_max_vel{false}; // true if we exceed the maximum velcoity of the auto flight task. + bool _isActive{false}; // true if the condition for braking is still valid + + Vector3f _jerk_setpoint; + Vector3f _acceleration_setpoint; + Vector3f _velocity_setpoint; + Vector3f _position_setpoint; + Vector3f _unsmoothed_velocity; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mpc_acc_down_max, + (ParamFloat) _param_mpc_acc_hor, + (ParamFloat) _param_mpc_acc_up_max, + (ParamFloat) _param_mpc_jerk_auto, + (ParamFloat) _param_mpc_jerk_max, + (ParamFloat) _param_mpc_xy_err_max, + (ParamFloat) _param_mpc_xy_traj_p, + (ParamFloat) _param_mpc_xy_vel_max, + (ParamFloat) _param_mpc_z_vel_max_dn, + (ParamFloat) _param_mpc_z_vel_max_up, + (ParamFloat) _param_nav_mc_alt_rad + ); +}; diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index 2453997dcb15..a3fc4f44bf44 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -128,6 +128,7 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const .timestamp = goto_setpoint.timestamp, .speed_up = NAN, .speed_down = NAN, + .speed_xy = NAN, .want_takeoff = false }; _vehicle_constraints_pub.publish(vehicle_constraints); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 5588c4560353..f52a40c2f9c8 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -525,6 +525,7 @@ void MulticopterPositionControl::Run() PX4_ISFINITE(_vehicle_constraints.speed_up) ? _vehicle_constraints.speed_up : _param_mpc_z_vel_max_up.get()); const float speed_down = PX4_ISFINITE(_vehicle_constraints.speed_down) ? _vehicle_constraints.speed_down : _param_mpc_z_vel_max_dn.get(); + // Allow ramping from zero thrust on takeoff const float minimum_thrust = flying ? _param_mpc_thr_min.get() : 0.f; _control.setThrustLimits(minimum_thrust, _param_mpc_thr_max.get()); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 6d58b85cd7ad..d591185cbda0 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -119,6 +119,7 @@ class MulticopterPositionControl : public ModuleBase .timestamp = 0, .speed_up = NAN, .speed_down = NAN, + .speed_xy = NAN, .want_takeoff = false, }; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 4198e916aed7..a58e39368322 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -54,10 +54,10 @@ Land::on_activation() _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); + /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false;