PX4 Firmware
PX4 Autopilot Software http://px4.io
FlightTaskManualPosition.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file FlightTaskManualPosition.cpp
36  */
37 
39 #include <mathlib/mathlib.h>
40 #include <float.h>
41 
42 using namespace matrix;
43 
45 {
46 
47 }
48 
50 {
52  // require valid position / velocity in xy
53  return ret && PX4_ISFINITE(_position(0))
54  && PX4_ISFINITE(_position(1))
55  && PX4_ISFINITE(_velocity(0))
56  && PX4_ISFINITE(_velocity(1));
57 }
58 
60 {
61  // all requirements from altitude-mode still have to hold
62  bool ret = FlightTaskManualAltitude::activate(last_setpoint);
63 
64  _constraints.tilt = math::radians(_param_mpc_tiltmax_air.get());
65 
66  // set task specific constraint
67  if (_constraints.speed_xy >= _param_mpc_vel_manual.get()) {
68  _constraints.speed_xy = _param_mpc_vel_manual.get();
69  }
70 
74  _velocity_scale = _constraints.speed_xy;
75 
76  // for position-controlled mode, we need a valid position and velocity state
77  // in NE-direction
78  return ret;
79 }
80 
82 {
83  /* Use same scaling as for FlightTaskManualAltitude */
85 
86  /* Constrain length of stick inputs to 1 for xy*/
87  Vector2f stick_xy(&_sticks_expo(0));
88 
89  float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
90 
91  if (mag > FLT_EPSILON) {
92  stick_xy = stick_xy.normalized() * mag;
93  }
94 
95  // scale the stick inputs
96  if (PX4_ISFINITE(_sub_vehicle_local_position.get().vxy_max)) {
97  // estimator provides vehicle specific max
98 
99  // use the minimum of the estimator and user specified limit
100  _velocity_scale = fminf(_constraints.speed_xy, _sub_vehicle_local_position.get().vxy_max);
101  // Allow for a minimum of 0.3 m/s for repositioning
102  _velocity_scale = fmaxf(_velocity_scale, 0.3f);
103 
104  } else if (stick_xy.length() > 0.5f) {
105  // raise the limit at a constant rate up to the user specified value
106 
107  if (_velocity_scale < _constraints.speed_xy) {
108  _velocity_scale += _deltatime * _param_mpc_acc_hor_estm.get();
109 
110  } else {
111  _velocity_scale = _constraints.speed_xy;
112 
113  }
114  }
115 
116  // scale velocity to its maximum limits
117  Vector2f vel_sp_xy = stick_xy * _velocity_scale;
118 
119  /* Rotate setpoint into local frame. */
120  _rotateIntoHeadingFrame(vel_sp_xy);
121 
122  // collision prevention
124  _collision_prevention.modifySetpoint(vel_sp_xy, _velocity_scale, Vector2f(_position),
126  }
127 
128  _velocity_setpoint(0) = vel_sp_xy(0);
129  _velocity_setpoint(1) = vel_sp_xy(1);
130 }
131 
133 {
134  /* If position lock is not active, position setpoint is set to NAN.*/
135  const float vel_xy_norm = Vector2f(_velocity).length();
136  const bool apply_brake = Vector2f(_velocity_setpoint).length() < FLT_EPSILON;
137  const bool stopped = (_param_mpc_hold_max_xy.get() < FLT_EPSILON || vel_xy_norm < _param_mpc_hold_max_xy.get());
138 
139  if (apply_brake && stopped && !PX4_ISFINITE(_position_setpoint(0))) {
142 
143  } else if (PX4_ISFINITE(_position_setpoint(0)) && apply_brake) {
144  // Position is locked but check if a reset event has happened.
145  // We will shift the setpoints.
150  }
151 
152  } else {
153  /* don't lock*/
154  _position_setpoint(0) = NAN;
155  _position_setpoint(1) = NAN;
156  }
157 }
158 
160 {
161  FlightTaskManualAltitude::_updateSetpoints(); // needed to get yaw and setpoints in z-direction
162 
163  // check if an external yaw handler is active and if yes, let it update the yaw setpoints
165  _yaw_setpoint = NAN;
167  }
168 
169  _thrust_setpoint.setAll(NAN); // don't require any thrust setpoints
170  _updateXYlock(); // check for position lock
171 }
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
matrix::Vector3f _velocity
current vehicle velocity
Definition: FlightTask.hpp:208
WeatherVane * _weathervane_yaw_handler
external weathervane library, used to implement a yaw control law that turns the vehicle nose into th...
bool is_active()
Definition: WeatherVane.hpp:59
float _yawspeed_setpoint
Definition: FlightTask.hpp:226
float _yaw_setpoint
Definition: FlightTask.hpp:225
float _deltatime
passed time in seconds since the task was last updated
Definition: FlightTask.hpp:201
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.
#define FLT_EPSILON
matrix::Vector3f _position_setpoint
Setpoints which the position controller has to execute.
Definition: FlightTask.hpp:220
Vector normalized() const
Definition: Vector.hpp:103
void _rotateIntoHeadingFrame(matrix::Vector2f &vec)
rotates vector into local frame
matrix::Vector3f _thrust_setpoint
Definition: FlightTask.hpp:224
float get_weathervane_yawrate()
Definition: WeatherVane.cpp:54
matrix::Vector< float, 4 > _sticks_expo
modified manual sticks using expo function
constexpr T radians(T degrees)
Definition: Limits.hpp:85
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector2< float > Vector2f
Definition: Vector2.hpp:69
Type length() const
Definition: Vector.hpp:83
matrix::Vector3f _position
current vehicle position
Definition: FlightTask.hpp:207
matrix::Vector3f _velocity_setpoint
Definition: FlightTask.hpp:221
uORB::SubscriptionData< vehicle_local_position_s > _sub_vehicle_local_position
Definition: FlightTask.hpp:171
vehicle_constraints_s _constraints
Vehicle constraints.
Definition: FlightTask.hpp:244
virtual void _updateSetpoints()
updates all setpoints
const T & get() const
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.
virtual void _scaleSticks()
scales sticks to velocity in z
CollisionPrevention _collision_prevention
collision avoidance setpoint amendment
void setAll(Type val)
Definition: Matrix.hpp:426