From 09bfb00c88f0c5dd33348edd5ced5088ea62446c Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 5 Jun 2019 17:51:06 +0200 Subject: [PATCH] Obstacle_distance: use only one increment in float directly CollisionPrevention: rename a few variables to make the code more readable --- msg/obstacle_distance.msg | 3 +- .../CollisionPrevention.cpp | 45 ++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 10 ++++- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg index a42e03a183..d4ab2abaeb 100644 --- a/msg/obstacle_distance.msg +++ b/msg/obstacle_distance.msg @@ -9,10 +9,9 @@ uint8 MAV_DISTANCE_SENSOR_RADAR = 3 uint16[72] distances # Distance of obstacles around the UAV with index 0 corresponding to local North. A value of 0 means that the obstacle is right in front of the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. -uint8 increment # Angular width in degrees of each array element. +float32 increment # Angular width in degrees of each array element. uint16 min_distance # Minimum distance the sensor can measure in centimeters. uint16 max_distance # Maximum distance the sensor can measure in centimeters. -float32 increment_f # Angular width in degrees of each array element. If greater than 0, it's used instead of increment. float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right. diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 60fd7f85c7..51772353e9 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -104,7 +104,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) (distance_sensor.current_distance < distance_sensor.max_distance)) { - if (obstacle.increment_f > 0.f || obstacle.increment > 0) { + if (obstacle.increment > 0) { // data from companion obstacle.timestamp = math::min(obstacle.timestamp, distance_sensor.timestamp); obstacle.max_distance = math::max((int)obstacle.max_distance, @@ -113,7 +113,6 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) (int)distance_sensor.min_distance * 100); // since the data from the companion are already in the distances data structure, // keep the increment that is sent - obstacle.increment_f = math::max(obstacle.increment_f, (float)obstacle.increment); obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update) } else { @@ -121,53 +120,47 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm memset(&obstacle.distances[0], UINT16_MAX, sizeof(obstacle.distances)); - obstacle.increment_f = math::degrees(distance_sensor.h_fov); + obstacle.increment = math::degrees(distance_sensor.h_fov); obstacle.angle_offset = 0.f; } - // init offset for sensor orientation distance_sensor_s::ROTATION_FORWARD_FACING or with offset coming from the companion - float offset = obstacle.angle_offset > 0.f ? math::radians(obstacle.angle_offset) : 0.0f; + float sensor_yaw_body_rad = obstacle.angle_offset > 0.f ? math::radians(obstacle.angle_offset) : 0.0f; switch (distance_sensor.orientation) { case distance_sensor_s::ROTATION_RIGHT_FACING: - offset = M_PI_F / 2.0f; + sensor_yaw_body_rad = M_PI_F / 2.0f; break; case distance_sensor_s::ROTATION_LEFT_FACING: - offset = -M_PI_F / 2.0f; + sensor_yaw_body_rad = -M_PI_F / 2.0f; break; case distance_sensor_s::ROTATION_BACKWARD_FACING: - offset = M_PI_F; + sensor_yaw_body_rad = M_PI_F; break; case distance_sensor_s::ROTATION_CUSTOM: - offset = Eulerf(Quatf(distance_sensor.q)).psi(); + sensor_yaw_body_rad = Eulerf(Quatf(distance_sensor.q)).psi(); break; } matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - // convert the sensor orientation from body to local frame - float sensor_orientation = math::degrees(wrap_pi(Eulerf(attitude).psi() + offset)); - - // convert orientation from range [-180, 180] to [0, 360] - if ((sensor_orientation <= FLT_EPSILON) || (sensor_orientation >= 180.0f)) { - sensor_orientation += 360.0f; - } + // convert the sensor orientation from body to local frame in the range [0, 360] + float sensor_yaw_local_deg = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_orientation - math::degrees(distance_sensor.h_fov / 2.0f)) / - obstacle.increment_f); - int upper_bound = (int)floor((sensor_orientation + math::degrees(distance_sensor.h_fov / 2.0f)) / - obstacle.increment_f); + int lower_bound = (int)floor((sensor_yaw_local_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / + obstacle.increment); + int upper_bound = (int)floor((sensor_yaw_local_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / + obstacle.increment); - // if increment_f is lower than 5deg, use an offset + // if increment is lower than 5deg, use an offset const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size - || upper_bound >= distances_array_size)) && obstacle.increment_f < 5.f) { - obstacle.angle_offset = sensor_orientation; + || upper_bound >= distances_array_size)) && obstacle.increment < 5.f) { + obstacle.angle_offset = sensor_yaw_local_deg ; upper_bound = abs(upper_bound - lower_bound); lower_bound = 0; } @@ -177,7 +170,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) if (wrap_bin < 0) { // wrap bin index around the array - wrap_bin = (int)floor(360.f / obstacle.increment_f) + bin; + wrap_bin = (int)floor(360.f / obstacle.increment) + bin; } if (wrap_bin >= distances_array_size) { @@ -216,9 +209,9 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, for (int i = 0; i < distances_array_size; i++) { if (obstacle.distances[i] < obstacle.max_distance && - obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment_f < 360.f) { + obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) { float distance = obstacle.distances[i] / 100.0f; //convert to meters - float angle = math::radians((float)i * obstacle.increment_f); + float angle = math::radians((float)i * obstacle.increment); if (obstacle.angle_offset > 0.f) { angle += math::radians(obstacle.angle_offset); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4a3df2a902..f5cbbd20df 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1677,9 +1677,17 @@ MavlinkReceiver::handle_message_obstacle_distance(mavlink_message_t *msg) obstacle_distance.timestamp = hrt_absolute_time(); obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type; memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances, sizeof(obstacle_distance.distances)); - obstacle_distance.increment = mavlink_obstacle_distance.increment; + + if (mavlink_obstacle_distance.increment_f > 0.f) { + obstacle_distance.increment = mavlink_obstacle_distance.increment_f; + + } else { + obstacle_distance.increment = (float)mavlink_obstacle_distance.increment; + } + obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance; obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance; + obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset; if (_obstacle_distance_pub == nullptr) { _obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &obstacle_distance);