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

FlightTaskManualPosition: remove update method and old member variable setpoints

Browse Source
This commit is contained in:
Dennis Mannhart
2018-04-05 07:30:12 +02:00
committed by Beat Küng
parent a7f4859698
commit d6fe2159ae
2 changed files with 21 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+18 -30
View File
@@ -1,6 +1,6 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2017 PX4 Development Team. All rights reserved. * Copyright (c) 2018 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -50,15 +50,14 @@ FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent,
bool FlightTaskManualPosition::activate() bool FlightTaskManualPosition::activate()
{ {
_pos_sp_xy = matrix::Vector2f(NAN, NAN); bool ret = FlightTaskManualAltitude::activate();
_vel_sp_xy = matrix::Vector2f(0.0f, 0.0f); _vel_sp(0) = _vel_sp(1) = 0.0f;
return FlightTaskManualAltitude::activate(); return ret;
} }
void FlightTaskManualPosition::_scaleSticks() void FlightTaskManualPosition::_scaleSticks()
{ {
/* Use same scaling as for FlightTaskManualAltitude to /* Use same scaling as for FlightTaskManualAltitude */
* get yaw and z */
FlightTaskManualAltitude::_scaleSticks(); FlightTaskManualAltitude::_scaleSticks();
/* Constrain length of stick inputs to 1 for xy*/ /* Constrain length of stick inputs to 1 for xy*/
@@ -71,47 +70,36 @@ void FlightTaskManualPosition::_scaleSticks()
} }
/* Scale to velocity.*/ /* Scale to velocity.*/
_vel_sp_xy = stick_xy * _vel_xy_manual_max.get(); matrix::Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
/* Rotate setpoint into local frame. */ /* Rotate setpoint into local frame. */
matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw); matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), 0.0f)); matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
_vel_sp_xy = matrix::Vector2f(vel_world(0), vel_world(1)); _vel_sp(0) = vel_world(0);
_vel_sp(1) = vel_world(1);
} }
void FlightTaskManualPosition::_updateXYlock() void FlightTaskManualPosition::_updateXYlock()
{ {
/* If position lock is not active, position setpoint is set to NAN.*/ /* If position lock is not active, position setpoint is set to NAN.*/
const float vel_xy_norm = Vector2f(&_velocity(0)).length(); const float vel_xy_norm = Vector2f(&_velocity(0)).length();
const bool apply_brake = _vel_sp_xy.length() < FLT_EPSILON; const bool apply_brake = Vector2f(&_vel_sp(0)).length() < FLT_EPSILON;
const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get()); const bool stopped = (_vel_hold_thr_xy.get() < FLT_EPSILON || vel_xy_norm < _vel_hold_thr_xy.get());
if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp_xy(0))) { if (apply_brake && stopped && !PX4_ISFINITE(_pos_sp(0))) {
_pos_sp_xy = matrix::Vector2f(&_position(0)); _pos_sp(0) = _position(0);
_pos_sp(1) = _position(1);
} else if (!apply_brake) { } else if (!apply_brake) {
/* Don't use position setpoint */ /* don't lock*/
_pos_sp_xy = _pos_sp_xy * NAN; _pos_sp(0) = NAN;
_pos_sp(1) = NAN;
} }
} }
void FlightTaskManualPosition::_updateSetpoints() void FlightTaskManualPosition::_updateSetpoints()
{ {
FlightTaskManualAltitude::_updateSetpoints(); // get yaw and z setpoints FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
_thr_sp *= NAN; // don't require any thrust setpoints
_updateXYlock(); // check for position lock _updateXYlock(); // check for position lock
} }
bool FlightTaskManualPosition::update()
{
_scaleSticks();
_updateSetpoints();
_setPositionSetpoint(Vector3f(_pos_sp_xy(0), _pos_sp_xy(1), _pos_sp_z));
_setVelocitySetpoint(Vector3f(_vel_sp_xy(0), _vel_sp_xy(1), _vel_sp_z));
_setYawSetpoint(_yaw_sp);
_setYawspeedSetpoint(_yaw_rate_sp);
_setThrustSetpoint(Vector3f(NAN, NAN, NAN));
return true;
}
src/lib/FlightTasks/tasks/FlightTaskManualPosition.hpp
+3 -8
View File
@@ -51,17 +51,12 @@ public:
bool activate() override; bool activate() override;
bool update() override;
protected: protected:
matrix::Vector2f _vel_sp_xy{}; /**< Scaled velocity setpoint from stick. NAN during position lock */ control::BlockParamFloat _vel_xy_manual_max; /**< maximum speed allowed horizontally */
matrix::Vector2f _pos_sp_xy{}; /**< Position setpoint during lock. Otherwise NAN */ control::BlockParamFloat _acc_xy_max;/**< maximum acceleration horizontally. Only used to compute lock time */
control::BlockParamFloat _vel_xy_manual_max; /**< Maximum speed allowed horizontally */
control::BlockParamFloat _acc_xy_max;/**< Maximum acceleration horizontally. Only used to compute lock time */
control::BlockParamFloat _vel_hold_thr_xy; /**< velocity threshold to switch back into horizontal position hold */ control::BlockParamFloat _vel_hold_thr_xy; /**< velocity threshold to switch back into horizontal position hold */
void _updateXYlock(); /**< Applies positon lock based on stick and velocity */ void _updateXYlock(); /**< applies positon lock based on stick and velocity */
void _updateSetpoints() override; void _updateSetpoints() override;
void _scaleSticks() override; void _scaleSticks() override;
}; };