PX4 Firmware
PX4 Autopilot Software http://px4.io
CollisionPrevention.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 CollisionPrevention.cpp
36  * CollisionPrevention controller.
37  *
38  */
39 
40 #include "CollisionPrevention.hpp"
41 
42 using namespace matrix;
43 using namespace time_literals;
44 
45 namespace
46 {
47 static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly
48 static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
49 
50 static float wrap_360(float f)
51 {
52  return wrap(f, 0.f, 360.f);
53 }
54 
55 static int wrap_bin(int i)
56 {
57  i = i % INTERNAL_MAP_USED_BINS;
58 
59  while (i < 0) {
60  i += INTERNAL_MAP_USED_BINS;
61  }
62 
63  return i;
64 }
65 
66 } // namespace
67 
69  ModuleParams(parent)
70 {
71  static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5");
72  static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly");
73 
74  // initialize internal obstacle map
76  _obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG;
80  uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]);
81  uint64_t current_time = getTime();
82 
83  for (uint32_t i = 0 ; i < internal_bins; i++) {
84  _data_timestamps[i] = current_time;
85  _data_maxranges[i] = 0;
86  _obstacle_map_body_frame.distances[i] = UINT16_MAX;
87  }
88 }
89 
91 {
92  //unadvertise publishers
93  if (_mavlink_log_pub != nullptr) {
95  }
96 }
97 
99 {
100  return hrt_absolute_time();
101 }
102 
104 {
105  return hrt_absolute_time() - *ptr;
106 }
107 
109 {
110  bool activated = _param_cp_dist.get() > 0;
111 
112  if (activated && !_was_active) {
114  }
115 
116  _was_active = activated;
117  return activated;
118 }
119 
120 void
122 {
123  int msg_index = 0;
124  float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi());
125  float increment_factor = 1.f / obstacle.increment;
126 
127  if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) {
128  // Obstacle message arrives in local_origin frame (north aligned)
129  // corresponding data index (convert to world frame and shift by msg offset)
130  for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
131  float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset;
132  msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor);
133 
134  //add all data points inside to FOV
135  if (obstacle.distances[msg_index] != UINT16_MAX) {
136  if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
137  _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
139  _data_maxranges[i] = obstacle.max_distance;
140  }
141  }
142  }
143 
144  } else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) {
145  // Obstacle message arrives in body frame (front aligned)
146  // corresponding data index (shift by msg offset)
147  for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
148  float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG +
150  msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor);
151 
152  //add all data points inside to FOV
153  if (obstacle.distances[msg_index] != UINT16_MAX) {
154 
155  if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) {
156  _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index];
158  _data_maxranges[i] = obstacle.max_distance;
159  }
160  }
161  }
162 
163  } else {
164  mavlink_log_critical(&_mavlink_log_pub, "Obstacle message received in unsupported frame %.0f\n",
165  (double)obstacle.frame);
166  }
167 }
168 
169 bool
170 CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_reading)
171 {
172  //use data from this sensor if:
173  //1. this sensor data is in range, the bin contains already valid data and this data is coming from the same or less range sensor
174  //2. this sensor data is in range, and the last reading was out of range
175  //3. this sensor data is out of range, the last reading was as well and this is the sensor with longest range
176  //4. this sensor data is out of range, the last reading was valid and coming from the same sensor
177 
178  uint16_t sensor_range_cm = (int)(100 * sensor_range); //convert to cm
179 
180  if (sensor_reading < sensor_range) {
181  if ((_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index]
182  && sensor_range_cm <= _data_maxranges[map_index])
183  || _obstacle_map_body_frame.distances[map_index] >= _data_maxranges[map_index]) {
184 
185  return true;
186  }
187 
188  } else {
189  if ((_obstacle_map_body_frame.distances[map_index] >= _data_maxranges[map_index]
190  && sensor_range_cm >= _data_maxranges[map_index])
191  || (_obstacle_map_body_frame.distances[map_index] < _data_maxranges[map_index]
192  && sensor_range_cm == _data_maxranges[map_index])) {
193 
194  return true;
195  }
196  }
197 
198  return false;
199 }
200 
201 void
203 {
205 
206  // add distance sensor data
207  for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
208 
209  // if a new distance sensor message has arrived
210  if (_sub_distance_sensor[i].updated()) {
211  distance_sensor_s distance_sensor {};
212  _sub_distance_sensor[i].copy(&distance_sensor);
213 
214  // consider only instances with valid data and orientations useful for collision prevention
215  if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
216  (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
217  (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
218 
219  // update message description
222  (uint16_t)(distance_sensor.max_distance * 100.0f));
224  (uint16_t)(distance_sensor.min_distance * 100.0f));
225 
227  }
228  }
229  }
230 
231  // add obstacle distance data
233  const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();
234 
235  // Update map with obstacle data if the data is not stale
236  if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) {
237  //update message description
240  obstacle_distance.max_distance);
242  obstacle_distance.min_distance);
243  _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q));
244  }
245  }
246 
247  // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
249 }
250 
251 void
253 {
254  // clamp at maximum sensor range
255  float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance);
256 
257  // discard values below min range
258  if ((distance_reading > distance_sensor.min_distance)) {
259 
260  float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
261  float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
262 
263  // calculate the field of view boundary bin indices
264  int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) /
265  INTERNAL_MAP_INCREMENT_DEG);
266  int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) /
267  INTERNAL_MAP_INCREMENT_DEG);
268 
269  // floor values above zero, ceil values below zero
270  if (lower_bound < 0) { lower_bound++; }
271 
272  if (upper_bound < 0) { upper_bound++; }
273 
274  // rotate vehicle attitude into the sensor body frame
275  matrix::Quatf attitude_sensor_frame = vehicle_attitude;
276  attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
277  float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta());
278 
279  if (distance_reading < distance_sensor.max_distance) {
280  distance_reading = distance_reading * sensor_dist_scale;
281  }
282 
283  uint16_t sensor_range = (int)(100 * distance_sensor.max_distance); // convert to cm
284 
285  for (int bin = lower_bound; bin <= upper_bound; ++bin) {
286  int wrapped_bin = wrap_bin(bin);
287 
288  if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
289  _obstacle_map_body_frame.distances[wrapped_bin] = (int)(100 * distance_reading);
291  _data_maxranges[wrapped_bin] = sensor_range;
292  }
293  }
294  }
295 }
296 
297 void
298 CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
299 {
300  const float col_prev_d = _param_cp_dist.get();
301  const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
302  const int sp_index_original = setpoint_index;
303  float best_cost = 9999.f;
304  int new_sp_index = setpoint_index;
305 
306  for (int i = sp_index_original - guidance_bins; i <= sp_index_original + guidance_bins; i++) {
307 
308  // apply moving average filter to the distance array to be able to center in larger gaps
309  const int filter_size = 1;
310  float mean_dist = 0;
311 
312  for (int j = i - filter_size; j <= i + filter_size; j++) {
313  int bin = wrap_bin(j);
314 
315  if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
316  mean_dist += col_prev_d * 100.f;
317 
318  } else {
319  mean_dist += _obstacle_map_body_frame.distances[bin];
320  }
321  }
322 
323  const int bin = wrap_bin(i);
324  mean_dist = mean_dist / (2.f * filter_size + 1.f);
325  const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original);
326  const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
327 
328  if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) {
329  best_cost = bin_cost;
330  new_sp_index = bin;
331  }
332  }
333 
334  //only change setpoint direction if it was moved to a different bin
335  if (new_sp_index != setpoint_index) {
336  float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
337  angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
338  setpoint_dir = {cosf(angle), sinf(angle)};
339  setpoint_index = new_sp_index;
340  }
341 }
342 
343 float
344 CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
345 {
346  float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f;
347 
348  switch (distance_sensor.orientation) {
349  case distance_sensor_s::ROTATION_YAW_0:
350  offset = 0.0f;
351  break;
352 
354  offset = M_PI_F / 4.0f;
355  break;
356 
358  offset = M_PI_F / 2.0f;
359  break;
360 
362  offset = 3.0f * M_PI_F / 4.0f;
363  break;
364 
366  offset = M_PI_F;
367  break;
368 
370  offset = -3.0f * M_PI_F / 4.0f;
371  break;
372 
374  offset = -M_PI_F / 2.0f;
375  break;
376 
378  offset = -M_PI_F / 4.0f;
379  break;
380 
381  case distance_sensor_s::ROTATION_CUSTOM:
382  offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi();
383  break;
384  }
385 
386  return offset;
387 }
388 
389 void
391  const Vector2f &curr_vel)
392 {
394 
395  // read parameters
396  const float col_prev_d = _param_cp_dist.get();
397  const float col_prev_dly = _param_cp_delay.get();
398  const bool move_no_data = _param_cp_go_nodata.get() > 0;
399  const float xy_p = _param_mpc_xy_p.get();
400  const float max_jerk = _param_mpc_jerk_max.get();
401  const float max_accel = _param_mpc_acc_hor.get();
403  const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();
404 
405  const float setpoint_length = setpoint.norm();
406 
407  const hrt_abstime constrain_time = getTime();
408 
410  if (setpoint_length > 0.001f) {
411 
412  Vector2f setpoint_dir = setpoint / setpoint_length;
413  float vel_max = setpoint_length;
414  const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);
415 
416  const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
417  const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
419  int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
420 
421  // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
422  _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad);
423 
424  // limit speed for safe flight
425  for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message
426 
427  // delete stale values
428  const hrt_abstime data_age = constrain_time - _data_timestamps[i];
429 
430  if (data_age > RANGE_STREAM_TIMEOUT_US) {
431  _obstacle_map_body_frame.distances[i] = UINT16_MAX;
432  }
433 
434  const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters
435  const float max_range = _data_maxranges[i] * 0.01f; // convert to meters
436  float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);
437 
438  // convert from body to local frame in the range [0, 2*pi]
439  angle = wrap_2pi(vehicle_yaw_angle_rad + angle);
440 
441  // get direction of current bin
442  const Vector2f bin_direction = {cosf(angle), sinf(angle)};
443 
445  && _obstacle_map_body_frame.distances[i] < UINT16_MAX) {
446 
447  if (setpoint_dir.dot(bin_direction) > 0) {
448  // calculate max allowed velocity with a P-controller (same gain as in the position controller)
449  const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
450  float delay_distance = curr_vel_parallel * col_prev_dly;
451 
452  if (distance < max_range) {
453  delay_distance += curr_vel_parallel * (data_age * 1e-6f);
454  }
455 
456  const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance);
457  const float vel_max_posctrl = xy_p * stop_distance;
458 
459  const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f);
460  const float projection = bin_direction.dot(setpoint_dir);
461  float vel_max_bin = vel_max;
462 
463  if (projection > 0.01f) {
464  vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection;
465  }
466 
467  // constrain the velocity
468  if (vel_max_bin >= 0) {
469  vel_max = math::min(vel_max, vel_max_bin);
470  }
471  }
472 
473  } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index && (!move_no_data)) {
474  vel_max = 0.f;
475  }
476  }
477 
478  setpoint = setpoint_dir * vel_max;
479  }
480 
481  } else {
482  //allow no movement
483  float vel_max = 0.f;
484  setpoint = setpoint * vel_max;
485 
486  // if distance data is stale, switch to Loiter
488  mavlink_log_critical(&_mavlink_log_pub, "No range data received, no movement allowed.");
489 
490  if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
493  mavlink_log_critical(&_mavlink_log_pub, "No range data received, switch to HOLD.");
494  }
495 
497  }
498 
499 
500  }
501 }
502 
503 void
504 CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos,
505  const Vector2f &curr_vel)
506 {
507  //calculate movement constraints based on range data
508  Vector2f new_setpoint = original_setpoint;
509  _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);
510 
511  //warn user if collision prevention starts to interfere
512  bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
513  || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
514  || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
515  || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
516 
517  if (currently_interfering && !_interfering) {
519  mavlink_log_info(&_mavlink_log_pub, "Collision Prevention limiting velocity");
521  }
522  }
523 
524  _interfering = currently_interfering;
525 
526  // publish constraints
527  collision_constraints_s constraints{};
528  constraints.timestamp = getTime();
529  original_setpoint.copyTo(constraints.original_setpoint);
530  new_setpoint.copyTo(constraints.adapted_setpoint);
531  _constraints_pub.publish(constraints);
532 
533  original_setpoint = new_setpoint;
534 }
535 
537 {
539  command.timestamp = getTime();
540  command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
541  command.param1 = (float)1; // base mode
542  command.param3 = (float)0; // sub mode
543  command.target_system = 1;
544  command.target_component = 1;
545  command.source_system = 1;
546  command.source_component = 1;
547  command.confirmation = false;
548  command.from_external = false;
549  command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
551 
552  // publish the vehicle command
554 }
void _publishVehicleCmdDoLoiter()
Publishes vehicle command.
void _updateObstacleMap()
Aggregates the sensor data into a internal obstacle map in body frame.
bool _enterData(int map_index, float sensor_range, float sensor_reading)
Determines whether a new sensor measurement is used.
Type wrap(Type x, Type low, Type high)
Wrap value to stay in range [low, high)
bool _was_active
states if the collision prevention interferes with the user input
bool is_active()
Returs true if Collision Prevention is running.
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US
bool _interfering
states if the collision prevention interferes with the user input
bool publish(const T &data)
Publish the struct.
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
Definition: Dual.hpp:203
constexpr T degrees(T radians)
Definition: Limits.hpp:91
void copyTo(Type dst[M *N]) const
Definition: Matrix.hpp:115
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
Computes collision free setpoints.
Quaternion class.
Definition: Dcm.hpp:24
obstacle_distance_s _obstacle_map_body_frame
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
Computes an adaption to the setpoint direction to guide towards free space.
virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr)
void rotate(const AxisAngle< Type > &vec)
Rotate quaternion from rotation vector.
Definition: Quaternion.hpp:383
bool publish(const T &data)
Publish the struct.
Definition: Publication.hpp:68
CollisionPrevention(ModuleParams *parent)
uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]
uORB::PublicationQueued< vehicle_command_s > _vehicle_command_pub
vehicle command do publication
Type dot(const MatrixM1 &b) const
Definition: Vector.hpp:55
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
Euler< float > Eulerf
Definition: Euler.hpp:156
virtual hrt_abstime getTime()
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
const T & get() const
uORB::SubscriptionData< obstacle_distance_s > _sub_obstacle_distance
obstacle distances received form a range sensor
float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance, const float final_speed)
Definition: TrajMath.hpp:61
Vector3< float > Vector3f
Definition: Vector3.hpp:136
static constexpr uint64_t TIMEOUT_HOLD_US
uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES]
distance data received from onboard rangefinders
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
orb_advert_t _mavlink_log_pub
Mavlink log uORB handle.
#define ORB_MULTI_MAX_INSTANCES
Maximum number of multi topic instances.
Definition: uORB.h:62
uORB::Publication< collision_constraints_s > _constraints_pub
constraints publication
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
uORB::Publication< obstacle_distance_s > _obstacle_distance_pub
obstacle_distance publication
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
Computes collision free setpoints.
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
Updates obstacle distance message with measurement from offboard.
#define M_PI_F
Definition: ashtech.cpp:44
Type norm() const
Definition: Vector.hpp:73
uORB::SubscriptionData< vehicle_attitude_s > _sub_vehicle_attitude
uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]
in cm
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Definition: Dual.hpp:196
bool copy(void *dst)
Copy the struct.
Dual< Scalar, N > floor(const Dual< Scalar, N > &a)
Definition: Dual.hpp:210
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).