From 73531ea76d60f80012be4c62c03e13ccc1a30b4f Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 17:49:49 +0200 Subject: [PATCH 01/19] distance_sensor: add horiontal and vertical fov, add quaternion for sensor orientation --- msg/distance_sensor.msg | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index ca688ca79b72..f158828c3e85 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -23,3 +23,8 @@ uint8 ROTATION_BACKWARD_FACING = 12 # MAV_SENSOR_ROTATION_PITCH_180 uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270 uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90 +uint8 ROTATION_CUSTOM =100 # MAV_SENSOR_ROTATION_CUSTOM + +float32 h_fov # Sensor horizontal field of view (rad) +float32 v_fov # Sensor vertical field of view (rad) +float32[4] q # Quaterion sensor orientation to specify the orientation ROTATION_CUSTOM From 4b2eb8b312797870be1d48d1fb20bb6df98a3137 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 17:50:08 +0200 Subject: [PATCH 02/19] obstacle_distance: add fields from mavlink extension --- msg/obstacle_distance.msg | 3 +++ 1 file changed, 3 insertions(+) diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg index 5f637e4af0bb..a42e03a1839a 100644 --- a/msg/obstacle_distance.msg +++ b/msg/obstacle_distance.msg @@ -13,3 +13,6 @@ uint8 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. From 1c3f45aee12cdaab9c857c49a84f495054783630 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 21:35:52 +0200 Subject: [PATCH 03/19] simulator mavlink: add horizontal and vertical fov + quaterion orientation to distance sensor --- src/modules/simulator/simulator_mavlink.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 18738b9cbc8e..a829512d0063 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -1090,6 +1090,13 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2 dist.signal_quality = -1; + dist.h_fov = dist_mavlink->horizontal_fov; + dist.v_fov = dist_mavlink->vertical_fov; + dist.q[0] = dist_mavlink->quaternion[0]; + dist.q[1] = dist_mavlink->quaternion[1]; + dist.q[2] = dist_mavlink->quaternion[2]; + dist.q[3] = dist_mavlink->quaternion[3]; + int dist_multi; orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH); From 5e45ecbbed83c1a11662ae20eaeabb509224a4d8 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 21:36:35 +0200 Subject: [PATCH 04/19] CollisionPrevention: map distance_sensor data to obstacle distance --- .../CollisionPrevention.cpp | 139 ++++++++++++++++-- .../CollisionPrevention.hpp | 7 + 2 files changed, 136 insertions(+), 10 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index fb304d23600e..60fd7f85c702 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -45,7 +45,6 @@ using namespace time_literals; CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { - } CollisionPrevention::~CollisionPrevention() @@ -78,12 +77,129 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se } } -void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, - const Vector2f &curr_pos, const Vector2f &curr_vel) +void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle) { _sub_obstacle_distance.update(); const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); + // Update with offboard data if the data is not stale + if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + obstacle = obstacle_distance; + } +} + +void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) +{ + for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { + // _sub_distance_sensor[i].update(); + // const distance_sensor_s &distance_sensor = _sub_distance_sensor[i].get(); + distance_sensor_s distance_sensor; + _sub_distance_sensor[i].copy(&distance_sensor); + + // consider only instaces with updated, valid data and orientations useful for collision prevention + if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING) && + (distance_sensor.current_distance > distance_sensor.min_distance) && + (distance_sensor.current_distance < distance_sensor.max_distance)) { + + + if (obstacle.increment_f > 0.f || obstacle.increment > 0) { + // data from companion + obstacle.timestamp = math::min(obstacle.timestamp, distance_sensor.timestamp); + obstacle.max_distance = math::max((int)obstacle.max_distance, + (int)distance_sensor.max_distance * 100); + obstacle.min_distance = math::min((int)obstacle.min_distance, + (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 { + obstacle.timestamp = distance_sensor.timestamp; + 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.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; + + switch (distance_sensor.orientation) { + case distance_sensor_s::ROTATION_RIGHT_FACING: + offset = M_PI_F / 2.0f; + break; + + case distance_sensor_s::ROTATION_LEFT_FACING: + offset = -M_PI_F / 2.0f; + break; + + case distance_sensor_s::ROTATION_BACKWARD_FACING: + offset = M_PI_F; + break; + + case distance_sensor_s::ROTATION_CUSTOM: + offset = 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; + } + + // 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); + + // if increment_f 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 = abs(upper_bound - lower_bound); + lower_bound = 0; + } + + for (int bin = lower_bound; bin <= upper_bound; ++bin) { + int wrap_bin = bin; + + if (wrap_bin < 0) { + // wrap bin index around the array + wrap_bin = (int)floor(360.f / obstacle.increment_f) + bin; + } + + if (wrap_bin >= distances_array_size) { + // wrap bin index around the array + wrap_bin = bin - distances_array_size; + } + + // compensate measurement for vehicle tilt and convert to cm + obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin], + (int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude).theta()))); + } + } + } +} + +void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, + const Vector2f &curr_pos, const Vector2f &curr_vel) +{ + obstacle_distance_s obstacle = {}; + updateOffboardObstacleDistance(obstacle); + updateDistanceSensor(obstacle); + //The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms. //that way the root does not have to be calculated for every range bin but once at the end. float setpoint_length = setpoint.norm(); @@ -92,18 +208,21 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, //Limit the deviation of the adapted setpoint from the originally given joystick input (slightly less than 90 degrees) float max_slide_angle_rad = 0.5f; - if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) { if (setpoint_length > 0.001f) { - int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]); + int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); for (int i = 0; i < distances_array_size; i++) { - //determine if distance bin is valid and contains a valid distance measurement - if (obstacle_distance.distances[i] < obstacle_distance.max_distance && - obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) { - float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters - float angle = math::radians((float)i * obstacle_distance.increment); + if (obstacle.distances[i] < obstacle.max_distance && + obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment_f < 360.f) { + float distance = obstacle.distances[i] / 100.0f; //convert to meters + float angle = math::radians((float)i * obstacle.increment_f); + + if (obstacle.angle_offset > 0.f) { + angle += math::radians(obstacle.angle_offset); + } //split current setpoint into parallel and orthogonal components with respect to the current bin direction Vector2f bin_direction = {cos(angle), sin(angle)}; diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index f75fc8e5c097..7bf703fbf445 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -44,8 +44,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -72,6 +74,8 @@ class CollisionPrevention : public ModuleParams orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ + 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 */ + uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500000}; static constexpr uint64_t MESSAGE_THROTTLE_US{5000000}; @@ -91,4 +95,7 @@ class CollisionPrevention : public ModuleParams void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + void updateOffboardObstacleDistance(obstacle_distance_s &obstacle); + void updateDistanceSensor(obstacle_distance_s &obstacle); + }; From 77d3b4d15874a5098fa7d0c7de0fbc8be35593c6 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 21:38:01 +0200 Subject: [PATCH 05/19] cm8jl65: add field of view --- src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp index 01c21cdf1fa2..dc5411367f16 100644 --- a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp @@ -327,9 +327,10 @@ CM8JL65::collect() bytes_read = OK; - distance_sensor_s report; + distance_sensor_s report = {}; report.current_distance = static_cast(distance_mm) / 1000.0f; report.id = 0; // TODO: set proper ID. + report.h_fov = 0.0488692f; report.max_distance = _max_distance; report.min_distance = _min_distance; report.orientation = _rotation; From a8bfcbacda61a03124f22859c6c45f4a4d5479e3 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 4 Jun 2019 21:39:04 +0200 Subject: [PATCH 06/19] cm8jl65: use paramter to set sensor orientation --- .../distance_sensor/cm8jl65/cm8jl65.cpp | 15 +++++++++--- .../distance_sensor/cm8jl65/module.yaml | 23 ++++++++++++++++++- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp index dc5411367f16..07aa1c2866ea 100644 --- a/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/cm8jl65.cpp @@ -42,6 +42,7 @@ */ #include +#include #include #include #include @@ -685,9 +686,17 @@ extern "C" __EXPORT int cm8jl65_main(int argc, char *argv[]) while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) { switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - break; + case 'R': { + int rot = -1; + + if (px4_get_parameter_value(myoptarg, rot) != 0) { + PX4_ERR("rotation parsing failed"); + return -1; + } + + rotation = (uint8_t)rot; + break; + } case 'd': device_path = myoptarg; diff --git a/src/drivers/distance_sensor/cm8jl65/module.yaml b/src/drivers/distance_sensor/cm8jl65/module.yaml index a1984001cd1c..f8242c54fa9d 100644 --- a/src/drivers/distance_sensor/cm8jl65/module.yaml +++ b/src/drivers/distance_sensor/cm8jl65/module.yaml @@ -1,6 +1,27 @@ module_name: Lanbao PSK-CM8JL65-CC5 serial_config: - - command: cm8jl65 start -d ${SERIAL_DEV} + - command: cm8jl65 start -d ${SERIAL_DEV} -R p:SENS_CM8JL65_R_0 port_config_param: name: SENS_CM8JL65_CFG group: Sensors + +parameters: + - group: Sensors + definitions: + SENS_CM8JL65_R_0: + description: + short: Distance Sensor Rotation + long: | + Distance Sensor Rotation as MAV_SENSOR_ORIENTATION enum + + type: enum + values: + 25: ROTATION_DOWNWARD_FACING + 24: ROTATION_UPWARD_FACING + 12: ROTATION_BACKWARD_FACING + 0: ROTATION_FORWARD_FACING + 6: ROTATION_LEFT_FACING + 2: ROTATION_RIGHT_FACING + 100: ROTATION_CUSTOM + reboot_required: true + default: 25 From f8ca8b405bd2073f1fb20f17e2ec68687bffc47f Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 5 Jun 2019 17:51:06 +0200 Subject: [PATCH 07/19] 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 a42e03a1839a..d4ab2abaeb05 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 60fd7f85c702..51772353e98c 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 4a3df2a902d9..f5cbbd20df67 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); From 6b700217c518f14983aadc232e2d14c64bafa81d Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 5 Jun 2019 18:31:00 +0200 Subject: [PATCH 08/19] CollisionPrevention: make sure that the timestamp is updated for distance sensors even if they are out of range --- .../CollisionPrevention.cpp | 98 ++++++++++--------- 1 file changed, 50 insertions(+), 48 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 51772353e98c..eda48d3dfdf8 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -99,9 +99,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) // consider only instaces with updated, valid data and orientations useful for collision prevention if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) && - (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING) && - (distance_sensor.current_distance > distance_sensor.min_distance) && - (distance_sensor.current_distance < distance_sensor.max_distance)) { + (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) { if (obstacle.increment > 0) { @@ -124,63 +122,67 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) obstacle.angle_offset = 0.f; } - // init offset for sensor orientation distance_sensor_s::ROTATION_FORWARD_FACING or with offset coming from the companion - float sensor_yaw_body_rad = obstacle.angle_offset > 0.f ? math::radians(obstacle.angle_offset) : 0.0f; + if ((distance_sensor.current_distance > distance_sensor.min_distance) && + (distance_sensor.current_distance < distance_sensor.max_distance)) { - switch (distance_sensor.orientation) { - case distance_sensor_s::ROTATION_RIGHT_FACING: - sensor_yaw_body_rad = M_PI_F / 2.0f; - break; + // init offset for sensor orientation distance_sensor_s::ROTATION_FORWARD_FACING or with offset coming from the companion + float sensor_yaw_body_rad = obstacle.angle_offset > 0.f ? math::radians(obstacle.angle_offset) : 0.0f; - case distance_sensor_s::ROTATION_LEFT_FACING: - sensor_yaw_body_rad = -M_PI_F / 2.0f; - break; + switch (distance_sensor.orientation) { + case distance_sensor_s::ROTATION_RIGHT_FACING: + sensor_yaw_body_rad = M_PI_F / 2.0f; + break; - case distance_sensor_s::ROTATION_BACKWARD_FACING: - sensor_yaw_body_rad = M_PI_F; - break; + case distance_sensor_s::ROTATION_LEFT_FACING: + sensor_yaw_body_rad = -M_PI_F / 2.0f; + break; - case distance_sensor_s::ROTATION_CUSTOM: - sensor_yaw_body_rad = Eulerf(Quatf(distance_sensor.q)).psi(); - break; - } + case distance_sensor_s::ROTATION_BACKWARD_FACING: + sensor_yaw_body_rad = M_PI_F; + break; - matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - // 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)); + case distance_sensor_s::ROTATION_CUSTOM: + sensor_yaw_body_rad = Eulerf(Quatf(distance_sensor.q)).psi(); + break; + } - // calculate the field of view boundary bin indices - 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); + matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); + // 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)); - // if increment is lower than 5deg, use an offset - const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); + // calculate the field of view boundary bin indices + 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 (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size - || 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; - } + // if increment is lower than 5deg, use an offset + const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]); - for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrap_bin = bin; - - if (wrap_bin < 0) { - // wrap bin index around the array - wrap_bin = (int)floor(360.f / obstacle.increment) + bin; + if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size + || 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; } - if (wrap_bin >= distances_array_size) { - // wrap bin index around the array - wrap_bin = bin - distances_array_size; - } + for (int bin = lower_bound; bin <= upper_bound; ++bin) { + int wrap_bin = bin; - // compensate measurement for vehicle tilt and convert to cm - obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin], - (int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude).theta()))); + if (wrap_bin < 0) { + // wrap bin index around the array + wrap_bin = (int)floor(360.f / obstacle.increment) + bin; + } + + if (wrap_bin >= distances_array_size) { + // wrap bin index around the array + wrap_bin = bin - distances_array_size; + } + + // compensate measurement for vehicle tilt and convert to cm + obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin], + (int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude).theta()))); + } } } } From bdd57a97d7182d99954b067fd6d9dfaa2c2e9900 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 5 Jun 2019 18:43:15 +0200 Subject: [PATCH 09/19] CollisionPrevention: refactor code to make it more readable --- .../CollisionPrevention.cpp | 21 +-------------- .../CollisionPrevention.hpp | 26 +++++++++++++++++++ 2 files changed, 27 insertions(+), 20 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index eda48d3dfdf8..c097a596537e 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -125,26 +125,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) if ((distance_sensor.current_distance > distance_sensor.min_distance) && (distance_sensor.current_distance < distance_sensor.max_distance)) { - // init offset for sensor orientation distance_sensor_s::ROTATION_FORWARD_FACING or with offset coming from the companion - 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: - sensor_yaw_body_rad = M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_LEFT_FACING: - sensor_yaw_body_rad = -M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_BACKWARD_FACING: - sensor_yaw_body_rad = M_PI_F; - break; - - case distance_sensor_s::ROTATION_CUSTOM: - sensor_yaw_body_rad = Eulerf(Quatf(distance_sensor.q)).psi(); - break; - } + float sensor_yaw_body_rad = sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset); matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); // convert the sensor orientation from body to local frame in the range [0, 360] diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 7bf703fbf445..4246f563b257 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -90,6 +90,32 @@ class CollisionPrevention : public ModuleParams void update(); + inline float sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) + { + + float offset = angle_offset > 0.f ? math::radians(angle_offset) : 0.0f; + + switch (distance_sensor.orientation) { + case distance_sensor_s::ROTATION_RIGHT_FACING: + offset = M_PI_F / 2.0f; + break; + + case distance_sensor_s::ROTATION_LEFT_FACING: + offset = -M_PI_F / 2.0f; + break; + + case distance_sensor_s::ROTATION_BACKWARD_FACING: + offset = M_PI_F; + break; + + case distance_sensor_s::ROTATION_CUSTOM: + offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi(); + break; + } + + return offset; + } + void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); From df0abc4d4a94bb4a46f83e6eaade4f417c5def28 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 13 Jun 2019 14:50:51 +0200 Subject: [PATCH 10/19] CollisionPrevention: make sure that vehicle tilt compensation is correct for all sensor orientation --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index c097a596537e..f6cb8b3d73a4 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -160,9 +160,13 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) wrap_bin = bin - distances_array_size; } + // rotate vehicle attitude into the sensor body frame + matrix::Quatf attitude_sensor_frame = attitude; + attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); + // compensate measurement for vehicle tilt and convert to cm obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin], - (int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude).theta()))); + (int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude_sensor_frame).theta()))); } } } From a98b776a947d4502e568b45c95cbb27245899f24 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Mon, 17 Jun 2019 11:52:27 +0200 Subject: [PATCH 11/19] add uORB message obstacle_distance_fused with data from offboard obstacle_distance and distance sensor --- msg/obstacle_distance.msg | 2 ++ src/lib/CollisionPrevention/CollisionPrevention.cpp | 13 +++++++++++++ src/lib/CollisionPrevention/CollisionPrevention.hpp | 2 ++ 3 files changed, 17 insertions(+) diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg index d4ab2abaeb05..73e9d3bc2279 100644 --- a/msg/obstacle_distance.msg +++ b/msg/obstacle_distance.msg @@ -15,3 +15,5 @@ uint16 min_distance # Minimum distance the sensor can measure in centimeters. uint16 max_distance # Maximum distance the sensor can measure in centimeters. 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. + +# TOPICS obstacle_distance obstacle_distance_fused diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index f6cb8b3d73a4..d1f1407695dc 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -77,6 +77,17 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se } } +void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle) +{ + // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor + if (_obstacle_distance_pub != nullptr) { + orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle); + + } else { + _obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle); + } +} + void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle) { _sub_obstacle_distance.update(); @@ -171,6 +182,8 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) } } } + + publishObstacleDistance(obstacle); } void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 4246f563b257..83aba8a8093b 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -72,6 +72,7 @@ class CollisionPrevention : public ModuleParams orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ + orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ 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 */ @@ -120,6 +121,7 @@ class CollisionPrevention : public ModuleParams const matrix::Vector2f &curr_vel); void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + void publishObstacleDistance(obstacle_distance_s &obstacle); void updateOffboardObstacleDistance(obstacle_distance_s &obstacle); void updateDistanceSensor(obstacle_distance_s &obstacle); From 99ec5058c6e4c77e455cf395afabc14b21d194ea Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Mon, 17 Jun 2019 13:50:08 +0200 Subject: [PATCH 12/19] stream mavlink message OBSTACLE DISTANCE --- src/modules/mavlink/mavlink_messages.cpp | 78 +++++++++++++++++++++++- 1 file changed, 77 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2eba396d7090..45e2e3dc64d1 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -4834,6 +4835,80 @@ class MavlinkStreamOrbitStatus : public MavlinkStream } }; +class MavlinkStreamObstacleDistance : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamObstacleDistance::get_name_static(); + } + + static const char *get_name_static() + { + return "OBSTACLE_DISTANCE"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_OBSTACLE_DISTANCE; + } + + uint16_t get_id() + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamObstacleDistance(mavlink); + } + + unsigned get_size() + { + return _obstacle_distance_fused_sub->is_published() ? (MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN + + MAVLINK_NUM_NON_PAYLOAD_BYTES) : + 0; + } + +private: + MavlinkOrbSubscription *_obstacle_distance_fused_sub; + uint64_t _obstacle_distance_time; + + /* do not allow top copying this class */ + MavlinkStreamObstacleDistance(MavlinkStreamObstacleDistance &) = delete; + MavlinkStreamObstacleDistance &operator = (const MavlinkStreamObstacleDistance &) = delete; + +protected: + explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink), + _obstacle_distance_fused_sub(_mavlink->add_orb_subscription(ORB_ID(obstacle_distance_fused))), + _obstacle_distance_time(0) + {} + + bool send(const hrt_abstime t) + { + obstacle_distance_s obstacke_distance; + + if (_obstacle_distance_fused_sub->update(&_obstacle_distance_time, &obstacke_distance)) { + mavlink_obstacle_distance_t msg = {}; + + msg.time_usec = obstacke_distance.timestamp; + msg.sensor_type = obstacke_distance.sensor_type; + memcpy(msg.distances, obstacke_distance.distances, sizeof(msg.distances)); + msg.increment = 0; + msg.min_distance = obstacke_distance.min_distance; + msg.max_distance = obstacke_distance.max_distance; + msg.angle_offset = obstacke_distance.angle_offset; + msg.increment_f = obstacke_distance.increment; + + mavlink_msg_obstacle_distance_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } +}; + static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), @@ -4891,7 +4966,8 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamHighLatency2::new_instance, &MavlinkStreamHighLatency2::get_name_static, &MavlinkStreamHighLatency2::get_id_static), StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static), StreamListItem(&MavlinkStreamPing::new_instance, &MavlinkStreamPing::get_name_static, &MavlinkStreamPing::get_id_static), - StreamListItem(&MavlinkStreamOrbitStatus::new_instance, &MavlinkStreamOrbitStatus::get_name_static, &MavlinkStreamOrbitStatus::get_id_static) + StreamListItem(&MavlinkStreamOrbitStatus::new_instance, &MavlinkStreamOrbitStatus::get_name_static, &MavlinkStreamOrbitStatus::get_id_static), + StreamListItem(&MavlinkStreamObstacleDistance::new_instance, &MavlinkStreamObstacleDistance::get_name_static, &MavlinkStreamObstacleDistance::get_id_static) }; const char *get_stream_name(const uint16_t msg_id) From 322572edc94572d7786bea3c0fc5ef3694d55421 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 25 Jun 2019 08:52:11 +0200 Subject: [PATCH 13/19] logger: log obstacle_distance_fused instead of obstacle_distance --- src/modules/logger/logger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ea30ca59e86c..f3f986dfebab 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -708,7 +708,7 @@ void Logger::add_sensor_comparison_topics() void Logger::add_vision_and_avoidance_topics() { add_topic("collision_constraints"); - add_topic("obstacle_distance"); + add_topic("obstacle_distance_fused"); add_topic("vehicle_mocap_odometry", 30); add_topic("vehicle_trajectory_waypoint", 200); add_topic("vehicle_trajectory_waypoint_desired", 200); From 494311f6971e68abe5d8963c6a30538584107aa8 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 25 Jun 2019 09:42:17 +0200 Subject: [PATCH 14/19] CollisionPrevention: add failsafe for stale distance data --- .../CollisionPrevention.cpp | 36 +++++++++++++++++-- .../CollisionPrevention.hpp | 7 ++-- 2 files changed, 37 insertions(+), 6 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index d1f1407695dc..b8067231c4b5 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -265,9 +265,14 @@ void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, } } - } else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) { - mavlink_log_critical(&_mavlink_log_pub, "No range data received"); - _last_message = hrt_absolute_time(); + } else { + // if distance data are stale, switch to Loiter and disable Collision Prevention + // such that it is still possible to fly in Position Control Mode + _publishVehicleCmdDoLoiter(); + mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering."); + float col_prev_d = -1.f; + param_set(param_find("MPC_COL_PREV_D"), &col_prev_d); + mavlink_log_critical(&_mavlink_log_pub, "Collision Prevention disabled."); } } @@ -292,3 +297,28 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa publishConstrainedSetpoint(original_setpoint, new_setpoint); original_setpoint = new_setpoint; } + +void CollisionPrevention::_publishVehicleCmdDoLoiter() +{ + vehicle_command_s command{}; + command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; + command.param1 = (float)1; // base mode + command.param3 = (float)0; // sub mode + command.target_system = 1; + command.target_component = 1; + command.source_system = 1; + command.source_component = 1; + command.confirmation = false; + command.from_external = false; + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + + // publish the vehicle command + if (_pub_vehicle_command == nullptr) { + _pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command, + vehicle_command_s::ORB_QUEUE_LENGTH); + + } else { + orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command); + } +} diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 83aba8a8093b..f49d95006287 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -48,11 +48,13 @@ #include #include #include +#include #include #include #include #include #include +#include class CollisionPrevention : public ModuleParams { @@ -73,15 +75,13 @@ class CollisionPrevention : public ModuleParams orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */ + orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ 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 */ uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500000}; - static constexpr uint64_t MESSAGE_THROTTLE_US{5000000}; - - hrt_abstime _last_message{0}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_col_prev_d, /**< collision prevention keep minimum distance */ @@ -125,5 +125,6 @@ class CollisionPrevention : public ModuleParams void updateOffboardObstacleDistance(obstacle_distance_s &obstacle); void updateDistanceSensor(obstacle_distance_s &obstacle); + void _publishVehicleCmdDoLoiter(); }; From 54049a048ed336395a83b793e1eba3a76223bec6 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 25 Jun 2019 14:58:08 +0200 Subject: [PATCH 15/19] CollisionPrevention: use FlightTasks convention for private/public methods, add doxygen on header file --- .../CollisionPrevention.cpp | 24 ++++---- .../CollisionPrevention.hpp | 60 +++++++++++++++---- 2 files changed, 59 insertions(+), 25 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index b8067231c4b5..13d990b14dab 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -55,7 +55,7 @@ CollisionPrevention::~CollisionPrevention() } } -void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint, +void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) { collision_constraints_s constraints{}; /**< collision constraints message */ @@ -77,7 +77,7 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se } } -void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle) +void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle) { // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor if (_obstacle_distance_pub != nullptr) { @@ -88,7 +88,7 @@ void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle) } } -void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle) +void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle) { _sub_obstacle_distance.update(); const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); @@ -99,11 +99,9 @@ void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &ob } } -void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) +void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) { for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - // _sub_distance_sensor[i].update(); - // const distance_sensor_s &distance_sensor = _sub_distance_sensor[i].get(); distance_sensor_s distance_sensor; _sub_distance_sensor[i].copy(&distance_sensor); @@ -136,7 +134,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) if ((distance_sensor.current_distance > distance_sensor.min_distance) && (distance_sensor.current_distance < distance_sensor.max_distance)) { - float sensor_yaw_body_rad = sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset); + float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset); matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); // convert the sensor orientation from body to local frame in the range [0, 360] @@ -183,15 +181,15 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) } } - publishObstacleDistance(obstacle); + _publishObstacleDistance(obstacle); } -void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, +void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, const Vector2f &curr_vel) { obstacle_distance_s obstacle = {}; - updateOffboardObstacleDistance(obstacle); - updateDistanceSensor(obstacle); + _updateOffboardObstacleDistance(obstacle); + _updateDistanceSensor(obstacle); //The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms. //that way the root does not have to be calculated for every range bin but once at the end. @@ -281,7 +279,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa { //calculate movement constraints based on range data Vector2f new_setpoint = original_setpoint; - calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); + _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); //warn user if collision prevention starts to interfere bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed @@ -294,7 +292,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa } _interfering = currently_interfering; - publishConstrainedSetpoint(original_setpoint, new_setpoint); + _publishConstrainedSetpoint(original_setpoint, new_setpoint); original_setpoint = new_setpoint; } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index f49d95006287..bd17c45020e9 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -62,9 +62,18 @@ class CollisionPrevention : public ModuleParams CollisionPrevention(ModuleParams *parent); ~CollisionPrevention(); - + /** + * Returs true if Collision Prevention is running + */ bool is_active() { return _param_mpc_col_prev_d.get() > 0; } + /** + * Computes collision free setpoints + * @param original_setpoint, setpoint before collision prevention intervention + * @param max_speed, maximum xy speed + * @param curr_pos, current vehicle position + * @param curr_vel, current vehicle velocity + */ void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); @@ -89,9 +98,12 @@ class CollisionPrevention : public ModuleParams (ParamFloat) _param_mpc_col_prev_dly /**< delay of the range measurement data*/ ) - void update(); - - inline float sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) + /** + * Transforms the sensor orientation into a yaw in the local frame + * @param distance_sensor, distance sensor message + * @param angle_offset, sensor body frame offset + */ + inline float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) { float offset = angle_offset > 0.f ? math::radians(angle_offset) : 0.0f; @@ -117,14 +129,38 @@ class CollisionPrevention : public ModuleParams return offset; } - void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, - const matrix::Vector2f &curr_vel); - - void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); - void publishObstacleDistance(obstacle_distance_s &obstacle); - - void updateOffboardObstacleDistance(obstacle_distance_s &obstacle); - void updateDistanceSensor(obstacle_distance_s &obstacle); + /** + * Computes collision free setpoints + * @param setpoint, setpoint before collision prevention intervention + * @param curr_pos, current vehicle position + * @param curr_vel, current vehicle velocity + */ + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, + const matrix::Vector2f &curr_vel); + /** + * Publishes collision_constraints message + * @param original_setpoint, setpoint before collision prevention intervention + * @param adapted_setpoint, collision prevention adaped setpoint + */ + void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + /** + * Publishes obstacle_distance message with fused data from offboard and from distance sensors + * @param obstacle, obstacle_distance message to be publsihed + */ + void _publishObstacleDistance(obstacle_distance_s &obstacle); + /** + * Updates obstacle distance message with measurement from offboard + * @param obstacle, obstacle_distance message to be updated + */ + void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle); + /** + * Updates obstacle distance message with measurement from distance sensors + * @param obstacle, obstacle_distance message to be updated + */ + void _updateDistanceSensor(obstacle_distance_s &obstacle); + /** + * Publishes vehicle command. + */ void _publishVehicleCmdDoLoiter(); }; From b5cffd1c717e286cdd82026506d6f58be936cd6e Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Fri, 28 Jun 2019 17:18:06 +0200 Subject: [PATCH 16/19] mc_pos_control: increase stack size by 100 bytes --- src/modules/mc_pos_control/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 08c93243c292..b7654d6a748b 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -37,7 +37,7 @@ px4_add_module( MODULE modules__mc_pos_control MAIN mc_pos_control COMPILE_FLAGS - STACK_MAIN 1200 + STACK_MAIN 1300 SRCS mc_pos_control_main.cpp PositionControl.cpp From 655d92c08f07236c94b4340fd3915fa536475d78 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Mon, 1 Jul 2019 18:46:01 +0200 Subject: [PATCH 17/19] CollisionPrevention: use the maximum timestamp between offboard and distance sensor such that if one of the two fails the vehicle goes into failsafe, do not switch off CollisionPrevention if it fails --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 13d990b14dab..e2cfc26226c9 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -113,7 +113,7 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) if (obstacle.increment > 0) { // data from companion - obstacle.timestamp = math::min(obstacle.timestamp, distance_sensor.timestamp); + obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp); obstacle.max_distance = math::max((int)obstacle.max_distance, (int)distance_sensor.max_distance * 100); obstacle.min_distance = math::min((int)obstacle.min_distance, @@ -264,13 +264,9 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, } } else { - // if distance data are stale, switch to Loiter and disable Collision Prevention - // such that it is still possible to fly in Position Control Mode + // if distance data are stale, switch to Loiter _publishVehicleCmdDoLoiter(); mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering."); - float col_prev_d = -1.f; - param_set(param_find("MPC_COL_PREV_D"), &col_prev_d); - mavlink_log_critical(&_mavlink_log_pub, "Collision Prevention disabled."); } } From e3eb5dca00773e3068530b519aff3163334b76c5 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Mon, 8 Jul 2019 17:28:43 +0200 Subject: [PATCH 18/19] CollisionPrevention: compute the attitude_sensor_frame outside for loop --- src/lib/CollisionPrevention/CollisionPrevention.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index e2cfc26226c9..38dc61208bc7 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -156,6 +156,11 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) lower_bound = 0; } + // rotate vehicle attitude into the sensor body frame + matrix::Quatf attitude_sensor_frame = attitude; + attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); + float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta()); + for (int bin = lower_bound; bin <= upper_bound; ++bin) { int wrap_bin = bin; @@ -169,13 +174,9 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) wrap_bin = bin - distances_array_size; } - // rotate vehicle attitude into the sensor body frame - matrix::Quatf attitude_sensor_frame = attitude; - attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); - // compensate measurement for vehicle tilt and convert to cm obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin], - (int)(100 * distance_sensor.current_distance * cosf(Eulerf(attitude_sensor_frame).theta()))); + (int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch)); } } } @@ -187,7 +188,7 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, const Vector2f &curr_vel) { - obstacle_distance_s obstacle = {}; + obstacle_distance_s obstacle{}; _updateOffboardObstacleDistance(obstacle); _updateDistanceSensor(obstacle); From f31981349fe4c22ca33b083a0c8623679ed438fc Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 11 Jul 2019 13:20:27 +0200 Subject: [PATCH 19/19] CollisionPrevention: address https://github.com/PX4/Firmware/pull/12179 review comments --- msg/distance_sensor.msg | 2 +- src/drivers/distance_sensor/cm8jl65/module.yaml | 1 - src/lib/CollisionPrevention/CollisionPrevention.cpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index f158828c3e85..460a47e8396c 100644 --- a/msg/distance_sensor.msg +++ b/msg/distance_sensor.msg @@ -27,4 +27,4 @@ uint8 ROTATION_CUSTOM =100 # MAV_SENSOR_ROTATION_CUSTOM float32 h_fov # Sensor horizontal field of view (rad) float32 v_fov # Sensor vertical field of view (rad) -float32[4] q # Quaterion sensor orientation to specify the orientation ROTATION_CUSTOM +float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM diff --git a/src/drivers/distance_sensor/cm8jl65/module.yaml b/src/drivers/distance_sensor/cm8jl65/module.yaml index f8242c54fa9d..886dce7e3a78 100644 --- a/src/drivers/distance_sensor/cm8jl65/module.yaml +++ b/src/drivers/distance_sensor/cm8jl65/module.yaml @@ -22,6 +22,5 @@ parameters: 0: ROTATION_FORWARD_FACING 6: ROTATION_LEFT_FACING 2: ROTATION_RIGHT_FACING - 100: ROTATION_CUSTOM reboot_required: true default: 25 diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index 38dc61208bc7..a7fffebe3c2b 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -126,7 +126,7 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) obstacle.timestamp = distance_sensor.timestamp; 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)); + memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances)); obstacle.increment = math::degrees(distance_sensor.h_fov); obstacle.angle_offset = 0.f; }