PX4 Firmware
PX4 Autopilot Software http://px4.io
CollisionPrevention.hpp
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 CollisionPrevention.hpp
36  * @author Tanja Baumann <tanja@auterion.com>
37  *
38  * CollisionPrevention controller.
39  *
40  */
41 
42 #pragma once
43 
44 #include <float.h>
45 
47 #include <drivers/drv_hrt.h>
48 #include <mathlib/mathlib.h>
49 #include <matrix/matrix/math.hpp>
50 #include <px4_platform_common/module_params.h>
51 #include <systemlib/mavlink_log.h>
52 #include <uORB/Publication.hpp>
54 #include <uORB/Subscription.hpp>
61 
62 using namespace time_literals;
63 
64 class CollisionPrevention : public ModuleParams
65 {
66 public:
67  CollisionPrevention(ModuleParams *parent);
68 
69  virtual ~CollisionPrevention();
70 
71  /**
72  * Returs true if Collision Prevention is running
73  */
74  bool is_active();
75 
76  /**
77  * Computes collision free setpoints
78  * @param original_setpoint, setpoint before collision prevention intervention
79  * @param max_speed, maximum xy speed
80  * @param curr_pos, current vehicle position
81  * @param curr_vel, current vehicle velocity
82  */
83  void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed,
84  const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel);
85 
86 protected:
87 
88  obstacle_distance_s _obstacle_map_body_frame {};
89  uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])];
90  uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof(
91  _obstacle_map_body_frame.distances[0])]; /**< in cm */
92 
93  void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude);
94 
95  /**
96  * Updates obstacle distance message with measurement from offboard
97  * @param obstacle, obstacle_distance message to be updated
98  */
99  void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude);
100 
101  /**
102  * Computes an adaption to the setpoint direction to guide towards free space
103  * @param setpoint_dir, setpoint direction before collision prevention intervention
104  * @param setpoint_index, index of the setpoint in the internal obstacle map
105  * @param vehicle_yaw_angle_rad, vehicle orientation
106  */
107  void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad);
108 
109  /**
110  * Determines whether a new sensor measurement is used
111  * @param map_index, index of the bin in the internal map the measurement belongs in
112  * @param sensor_range, max range of the sensor in meters
113  * @param sensor_reading, distance measurement in meters
114  */
115  bool _enterData(int map_index, float sensor_range, float sensor_reading);
116 
117 
118  //Timing functions. Necessary to mock time in the tests
119  virtual hrt_abstime getTime();
120  virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr);
121 
122 
123 private:
124 
125  bool _interfering{false}; /**< states if the collision prevention interferes with the user input */
126  bool _was_active{false}; /**< states if the collision prevention interferes with the user input */
127 
128  orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
129 
130  uORB::Publication<collision_constraints_s> _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */
131  uORB::Publication<obstacle_distance_s> _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */
132  uORB::PublicationQueued<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */
133 
134  uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
135  uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
136  uORB::SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude{ORB_ID(vehicle_attitude)};
137 
138  static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms};
139  static constexpr uint64_t TIMEOUT_HOLD_US{5_s};
140 
141  hrt_abstime _last_collision_warning{0};
142  hrt_abstime _last_timeout_warning{0};
143  hrt_abstime _time_activated{0};
144 
145  DEFINE_PARAMETERS(
146  (ParamFloat<px4::params::CP_DIST>) _param_cp_dist, /**< collision prevention keep minimum distance */
147  (ParamFloat<px4::params::CP_DELAY>) _param_cp_delay, /**< delay of the range measurement data*/
148  (ParamFloat<px4::params::CP_GUIDE_ANG>) _param_cp_guide_ang, /**< collision prevention change setpoint angle */
149  (ParamFloat<px4::params::CP_GO_NO_DATA>) _param_cp_go_nodata, /**< movement allowed where no data*/
150  (ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p, /**< p gain from position controller*/
151  (ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max, /**< vehicle maximum jerk*/
152  (ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/
153  )
154 
155  /**
156  * Transforms the sensor orientation into a yaw in the local frame
157  * @param distance_sensor, distance sensor message
158  * @param angle_offset, sensor body frame offset
159  */
160  float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
161 
162  /**
163  * Computes collision free setpoints
164  * @param setpoint, setpoint before collision prevention intervention
165  * @param curr_pos, current vehicle position
166  * @param curr_vel, current vehicle velocity
167  */
168  void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos,
169  const matrix::Vector2f &curr_vel);
170 
171  /**
172  * Publishes collision_constraints message
173  * @param original_setpoint, setpoint before collision prevention intervention
174  * @param adapted_setpoint, collision prevention adaped setpoint
175  */
176  void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
177 
178  /**
179  * Publishes obstacle_distance message with fused data from offboard and from distance sensors
180  * @param obstacle, obstacle_distance message to be publsihed
181  */
182  void _publishObstacleDistance(obstacle_distance_s &obstacle);
183 
184  /**
185  * Aggregates the sensor data into a internal obstacle map in body frame
186  */
187  void _updateObstacleMap();
188 
189  /**
190  * Publishes vehicle command.
191  */
192  void _publishVehicleCmdDoLoiter();
193 
194 };
Quaternion class.
Definition: Dcm.hpp:24
High-resolution timer with callouts and timekeeping.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Vector2< float > Vector2f
Definition: Vector2.hpp:69
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
PX4 custom flight modes.
#define ORB_MULTI_MAX_INSTANCES
Maximum number of multi topic instances.
Definition: uORB.h:62