fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 15:24:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

MPC: LAND: create Stop motion primitive to also be used elsewhere

Browse Source
This commit is contained in:
Claudio Chies 2024-10-18 09:29:41 +02:00
parent b04b64c260
commit 4d003fd782
11 changed files with 259 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -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

2
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -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

126
src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.cpp
View File

@ -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,109 +168,21 @@ FlightTaskLand::_PerformLanding()
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void
FlightTaskLand::_SmoothBrakingPath()
FlightTaskLand::_PerformBraking()
{
PositionSmoothing::PositionSmoothingSetpoints out_setpoints;
_stop.getConstraints(_constraints);
_stop.update(_acceleration_setpoint, _velocity_setpoint, _position, _deltatime);
_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;
_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
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()
{
// 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());
}
}
void
FlightTaskLand::updateParams()
{

26
src/modules/flight_mode_manager/tasks/Land/FlightTaskLand.hpp
View File

@ -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 @@ public:
protected:
void updateParams() override;
PositionSmoothing _position_smoothing;
Sticks _sticks{this};
StickAccelerationXY _stick_acceleration_xy{this};
StickYaw _stick_yaw{this};
Stop _stop{this};
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_triplet_setpoint{ORB_ID(position_setpoint_triplet)};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
(ParamFloat<px4::params::MPC_LAND_ALT3>) _param_mpc_land_alt3,
(ParamFloat<px4::params::MPC_LAND_CRWL>) _param_mpc_land_crawl_speed,
(ParamFloat<px4::params::MPC_LAND_RADIUS>) _param_mpc_land_radius,
(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _param_mpc_z_v_auto_up,
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad
(ParamFloat<px4::params::MPC_LAND_SPEED>) _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};
};

1
src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt
View File

@ -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)

125
src/modules/flight_mode_manager/tasks/Utility/Stop.cpp Normal file
View File

@ -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);
}

108
src/modules/flight_mode_manager/tasks/Utility/Stop.hpp Normal file
View File

@ -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 <claudio@chies.com>
*/
#include <px4_platform_common/module_params.h>
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <uORB/topics/vehicle_constraints.h>
#include <matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <px4_platform_common/log.h>
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<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
(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::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad
);
};

1
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
View File

@ -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);

1
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -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());

1
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -119,6 +119,7 @@ private:
.timestamp = 0,
.speed_up = NAN,
.speed_down = NAN,
.speed_xy = NAN,
.want_takeoff = false,
};

2
src/modules/navigator/land.cpp
View File

@ -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;