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

FlightTaskManualAcceleration: horizontal velocity limit interface

Browse Source
This commit is contained in:
Matthias Grob 2023-08-14 16:39:48 +02:00
parent 77c06a9f9e
commit bb617f6c4d
2 changed files with 5 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp
View File

@ -31,13 +31,6 @@
*
****************************************************************************/
/**
* @file FlightTaskManualPosition.hpp
*
* Flight task for manual position controlled mode.
*
*/
#pragma once
#include "FlightTaskManualAltitudeSmoothVel.hpp"
@ -53,7 +46,7 @@ public:
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool update() override;
private:
protected:
void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override;
void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override;

6
src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
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,
const matrix::Vector2f &vel_sp_feedback, const float dt)
{
// maximum commanded acceleration and velocity
Vector2f acceleration_scale(_param_mpc_acc_hor.get(), _param_mpc_acc_hor.get());
// maximum commanded velocity can be constrained dynamically
const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
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