fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 10:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskManualAcceleration: horizontal velocity limit interface

Browse Source
This commit is contained in:
Matthias Grob
2023-12-08 21:17:59 +01:00
parent 77c06a9f9e
commit bb617f6c4d
2 changed files with 5 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp
+1 -8
View File
@@ -31,13 +31,6 @@
* *
****************************************************************************/ ****************************************************************************/
/**
* @file FlightTaskManualPosition.hpp
*
* Flight task for manual position controlled mode.
*
*/
#pragma once #pragma once
#include "FlightTaskManualAltitudeSmoothVel.hpp" #include "FlightTaskManualAltitudeSmoothVel.hpp"
@@ -53,7 +46,7 @@ public:
bool activate(const trajectory_setpoint_s &last_setpoint) override; bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool update() override; bool update() override;
private: protected:
void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+4 -2
View File
@@ -73,10 +73,12 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
const matrix::Vector2f &vel_sp_feedback, const float dt) const matrix::Vector2f &vel_sp_feedback, const float dt)
{ {
// maximum commanded acceleration and velocity // maximum commanded velocity can be constrained dynamically
Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint); const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
Vector2f velocity_scale(velocity_sc, velocity_sc); Vector2f velocity_scale(velocity_sc, velocity_sc);
// maximum commanded acceleration is scaled down with velocity
const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());
Vector2f acceleration_scale(acceleration_sc, acceleration_sc);
acceleration_scale *= 2.f; // because of drag the average acceleration is half acceleration_scale *= 2.f; // because of drag the average acceleration is half