diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index 7f16fa5d26..302f5f79cf 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -620,7 +620,8 @@ void SF45LaserSerial::sf45_process_replies() float distance_m = raw_distance * SF45_SCALE_FACTOR; _current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist; - uint8_t current_bin = sf45_convert_angle(scaled_yaw); + // Find bin index for the current sensor yaw angle (in sensor frame) + const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame); if (current_bin != _previous_bin) { PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin, @@ -702,21 +703,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_ } } - -uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw) -{ - uint8_t mapped_sector = 0; - float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset); - mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment); - - return mapped_sector; -} - -float SF45LaserSerial::sf45_wrap_360(float f) -{ - return matrix::wrap(f, 0.f, 360.f); -} - uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val) { uint32_t i; diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index d308a474d4..313b9e60f7 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -74,10 +74,6 @@ enum SF45_PARSED_STATE { }; enum SensorOrientation { // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum - ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE - ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 - ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180 - ROTATION_LEFT_FACING = 6, // MAV_SENSOR_ROTATION_YAW_270 ROTATION_UPWARD_FACING = 24, // MAV_SENSOR_ROTATION_PITCH_90 ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270 }; @@ -95,8 +91,6 @@ public: void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len); uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value); void sf45_process_replies(); - uint8_t sf45_convert_angle(const int16_t yaw); - float sf45_wrap_360(float f); private: obstacle_distance_s _obstacle_distance{}; diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 3f1344ffdd..7e04629915 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -236,14 +236,14 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // corresponding data index (convert to world frame and shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) { - float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - - (float)_obstacle_map_body_frame.increment / 2.f); - float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - + (float)_obstacle_map_body_frame.increment / 2.f); - float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg - - obstacle.increment / 2.f); - float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg + - obstacle.increment / 2.f); + float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, + obstacle.angle_offset - vehicle_orientation_deg); + float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, + obstacle.angle_offset - vehicle_orientation_deg); // if a bin stretches over the 0/360 degree line, adjust the angles if (bin_lower_angle > bin_upper_angle) { @@ -277,12 +277,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst // corresponding data index (shift by msg offset) for (int i = 0; i < BIN_COUNT; i++) { for (int j = 0; j < 360 / obstacle.increment; j++) { - float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - - (float)_obstacle_map_body_frame.increment / 2.f); - float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset - + (float)_obstacle_map_body_frame.increment / 2.f); - float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f); - float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f); + float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment, + _obstacle_map_body_frame.angle_offset); + float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset); + float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset); // if a bin stretches over the 0/360 degree line, adjust the angles if (bin_lower_angle > bin_upper_angle) { @@ -373,7 +373,7 @@ void CollisionPrevention::_transformSetpoint(const Vector2f &setpoint) { const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw; - const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) - + const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); _setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE); // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps @@ -394,7 +394,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, // discard values below min range if (distance_reading > distance_sensor.min_distance) { - float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset); + float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast + (distance_sensor.orientation), distance_sensor.q); float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices @@ -408,7 +409,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, uint16_t sensor_range = static_cast(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm for (int bin = lower_bound; bin <= upper_bound; ++bin) { - int wrapped_bin = _wrap_bin(bin); + int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT); if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) { _obstacle_map_body_frame.distances[wrapped_bin] = static_cast(100.0f * distance_reading + 0.5f); @@ -435,7 +436,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float mean_dist = 0; for (int j = i - filter_size; j <= i + filter_size; j++) { - int bin = _wrap_bin(j); + int bin = ObstacleMath::wrap_bin(j, BIN_COUNT); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { mean_dist += _param_cp_dist.get() * 100.f; @@ -445,7 +446,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi } } - const int bin = _wrap_bin(i); + const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT); mean_dist = mean_dist / (2.f * filter_size + 1.f); const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original); const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin]; @@ -465,52 +466,6 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi } } -float -CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const -{ - float offset = math::max(math::radians(angle_offset), 0.f); - - switch (distance_sensor.orientation) { - case distance_sensor_s::ROTATION_YAW_0: - offset = 0.0f; - break; - - case distance_sensor_s::ROTATION_YAW_45: - offset = M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_90: - offset = M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_YAW_135: - offset = 3.0f * M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_180: - offset = M_PI_F; - break; - - case distance_sensor_s::ROTATION_YAW_225: - offset = -3.0f * M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_YAW_270: - offset = -M_PI_F / 2.0f; - break; - - case distance_sensor_s::ROTATION_YAW_315: - offset = -M_PI_F / 4.0f; - break; - - case distance_sensor_s::ROTATION_CUSTOM: - offset = Eulerf(Quatf(distance_sensor.q)).psi(); - break; - } - - return offset; -} - float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { float obstacle_distance = 0.f; @@ -520,10 +475,10 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) Vector2f dir = direction / direction_norm; const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw; const float sp_angle_with_offset_deg = - _wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); - int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE); - dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1); - obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; + ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset); + + const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg); + obstacle_distance = _obstacle_map_body_frame.distances[dir_index] * 0.01f; } return obstacle_distance; @@ -632,19 +587,3 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter() command.timestamp = getTime(); _vehicle_command_pub.publish(command); } - -float CollisionPrevention::_wrap_360(const float f) -{ - return wrap(f, 0.f, 360.f); -} - -int CollisionPrevention::_wrap_bin(int i) -{ - i = i % BIN_COUNT; - - while (i < 0) { - i += BIN_COUNT; - } - - return i; -} diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 7d9d16cd7f..76bfcb5ea7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -204,13 +204,6 @@ private: (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) - /** - * Transforms the sensor orientation into a yaw in the local frame - * @param distance_sensor, distance sensor message - * @param angle_offset, sensor body frame offset - */ - float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const; - /** * Computes collision free setpoints * @param setpoint, setpoint before collision prevention intervention @@ -238,6 +231,4 @@ private: */ void _publishVehicleCmdDoLoiter(); - static float _wrap_360(const float f); - static int _wrap_bin(int i); }; diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp index 0605b664ec..e75f19d7b7 100644 --- a/src/lib/collision_prevention/ObstacleMath.cpp +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -57,13 +57,19 @@ int get_bin_at_angle(float bin_width, float angle) return wrap_bin(bin_at_angle, 360 / bin_width); } +float get_lower_bound_angle(int bin, float bin_width, float angle_offset) +{ + bin = wrap_bin(bin, 360 / bin_width); + return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f); +} + int get_offset_bin_index(int bin, float bin_width, float angle_offset) { int offset = get_bin_at_angle(bin_width, angle_offset); return wrap_bin(bin - offset, 360 / bin_width); } -float sensor_orientation_to_yaw_offset(const SensorOrientation orientation) +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4]) { float offset = 0.0f; @@ -99,6 +105,13 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation) case SensorOrientation::ROTATION_YAW_315: offset = -M_PI_F / 4.0f; break; + + case SensorOrientation::ROTATION_CUSTOM: + if (q != nullptr) { + offset = Eulerf(Quatf(q)).psi(); + } + + break; } return offset; @@ -109,4 +122,9 @@ int wrap_bin(int bin, int bin_count) return (bin + bin_count) % bin_count; } +float wrap_360(const float angle) +{ + return matrix::wrap(angle, 0.0f, 360.0f); +} + } // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp index f5317aeaa7..8a1db0217a 100644 --- a/src/lib/collision_prevention/ObstacleMath.hpp +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -45,6 +45,7 @@ enum SensorOrientation { ROTATION_YAW_225 = 5, // MAV_SENSOR_ROTATION_YAW_225 ROTATION_YAW_270 = 6, // MAV_SENSOR_ROTATION_YAW_270 ROTATION_YAW_315 = 7, // MAV_SENSOR_ROTATION_YAW_315 + ROTATION_CUSTOM = 100, // MAV_SENSOR_ROTATION_CUSTOM ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90 @@ -56,7 +57,7 @@ enum SensorOrientation { * Converts a sensor orientation to a yaw offset * @param orientation sensor orientation */ -float sensor_orientation_to_yaw_offset(const SensorOrientation orientation); +float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr); /** * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane @@ -73,6 +74,14 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons */ int get_bin_at_angle(float bin_width, float angle); +/** + * Returns lower bound angle of a bin + * @param bin bin index + * @param bin_width width of a bin in degrees + * @param angle_offset clockwise angle offset in degrees + */ +float get_lower_bound_angle(int bin, float bin_width, float angle_offset); + /** * Returns bin index for the current bin after an angle offset * @param bin current bin index @@ -88,4 +97,11 @@ int get_offset_bin_index(int bin, float bin_width, float angle_offset); */ int wrap_bin(int bin, int bin_count); +/** + * Wraps an angle to the range [0, 360) + * @param angle angle in degrees + */ +float wrap_360(const float angle); + + } // ObstacleMath diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp index cea54aa4a2..9f6b00ae21 100644 --- a/src/lib/collision_prevention/ObstacleMathTest.cpp +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -133,6 +133,22 @@ TEST(ObstacleMathTest, GetBinAtAngle) EXPECT_EQ(bin_index, 18); } +TEST(ObstacleMathTest, GetLowerBound) +{ + // GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset + int bin = -1; + float bin_width = 7.5f; + float angle_offset = -4.3f; + + // WHEN: we calculate the lower bound angle of the bin + float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset); + + // THEN: the lower bound angle should be correct. The bin index is wrapped to the end and + // the angle offset is applied in the counter-clockwise direction. + EXPECT_FLOAT_EQ(lower_bound, 344.45); + +} + TEST(ObstacleMathTest, OffsetBinIndex) {