39 #include <mathlib/mathlib.h> 102 _velocity_scale = fmaxf(_velocity_scale, 0.3
f);
104 }
else if (stick_xy.
length() > 0.5f) {
108 _velocity_scale +=
_deltatime * _param_mpc_acc_hor_estm.get();
117 Vector2f vel_sp_xy = stick_xy * _velocity_scale;
137 const bool stopped = (_param_mpc_hold_max_xy.get() <
FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get());
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
matrix::Vector3f _velocity
current vehicle velocity
WeatherVane * _weathervane_yaw_handler
external weathervane library, used to implement a yaw control law that turns the vehicle nose into th...
float _deltatime
passed time in seconds since the task was last updated
bool is_active()
Returs true if Collision Prevention is running.
void _updateSetpoints() override
updates all setpoints
Flight task for manual position controlled mode.
bool updateInitialize() override
Call before activate() or update() to initialize time and input data.
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
matrix::Vector3f _position_setpoint
Setpoints which the position controller has to execute.
Vector normalized() const
void _rotateIntoHeadingFrame(matrix::Vector2f &vec)
rotates vector into local frame
matrix::Vector3f _thrust_setpoint
float get_weathervane_yawrate()
matrix::Vector< float, 4 > _sticks_expo
modified manual sticks using expo function
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector2< float > Vector2f
matrix::Vector3f _position
current vehicle position
matrix::Vector3f _velocity_setpoint
uORB::SubscriptionData< vehicle_local_position_s > _sub_vehicle_local_position
vehicle_constraints_s _constraints
Vehicle constraints.
virtual void _updateSetpoints()
updates all setpoints
bool activate(vehicle_local_position_setpoint_s last_setpoint) override
Call once on the event where you switch to the task.
bool updateInitialize() override
Call before activate() or update() to initialize time and input data.
void _scaleSticks() override
scales sticks to velocity in z
void _updateXYlock()
applies position lock based on stick and velocity
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,(ParamFloat< px4::params::MPC_VEL_MANUAL >) _param_mpc_vel_manual,(ParamFloat< px4::params::MPC_ACC_HOR_MAX >) _param_mpc_acc_hor_max,(ParamFloat< px4::params::MPC_HOLD_MAX_XY >) _param_mpc_hold_max_xy,(ParamFloat< px4::params::MPC_ACC_HOR_ESTM >) _param_mpc_acc_hor_estm) private uint8_t _reset_counter
counter for estimator resets in xy-direction
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
Computes collision free setpoints.
FlightTaskManualPosition()
virtual void _scaleSticks()
scales sticks to velocity in z
CollisionPrevention _collision_prevention
collision avoidance setpoint amendment