diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index c5909381780f..65d290e0cc79 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -75,7 +75,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index 090415e5ec49..d60de46e7bea 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -67,7 +67,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index a73c357437f4..c9c3e5843ac7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -68,7 +68,6 @@ param set-default MPC_Z_VEL_P_ACC 5 param set-default MPC_Z_VEL_I_ACC 3 param set-default MPC_LAND_ALT1 3 param set-default MPC_LAND_ALT2 1 -param set-default MPC_POS_MODE 3 param set-default CP_GO_NO_DATA 1 # Navigator Parameters diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index d4e4273b6982..0733f1b289d4 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -41,51 +41,19 @@ #include using namespace matrix; -using namespace time_literals; - -namespace -{ -static constexpr int INTERNAL_MAP_INCREMENT_DEG = 10; //cannot be lower than 5 degrees, should divide 360 evenly -static constexpr int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG; - -static float wrap_360(float f) -{ - return wrap(f, 0.f, 360.f); -} - -static int wrap_bin(int i) -{ - i = i % INTERNAL_MAP_USED_BINS; - - while (i < 0) { - i += INTERNAL_MAP_USED_BINS; - } - - return i; -} - -} // namespace CollisionPrevention::CollisionPrevention(ModuleParams *parent) : ModuleParams(parent) { - static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5, "INTERNAL_MAP_INCREMENT_DEG needs to be at least 5"); - static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0, "INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly"); + static_assert(BIN_SIZE >= 5, "BIN_SIZE must be at least 5"); + static_assert(360 % BIN_SIZE == 0, "BIN_SIZE must divide 360 evenly"); // initialize internal obstacle map - _obstacle_map_body_frame.timestamp = getTime(); _obstacle_map_body_frame.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - _obstacle_map_body_frame.increment = INTERNAL_MAP_INCREMENT_DEG; + _obstacle_map_body_frame.increment = BIN_SIZE; _obstacle_map_body_frame.min_distance = UINT16_MAX; - _obstacle_map_body_frame.max_distance = 0; - _obstacle_map_body_frame.angle_offset = 0.f; - uint32_t internal_bins = sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0]); - uint64_t current_time = getTime(); - - for (uint32_t i = 0 ; i < internal_bins; i++) { - _data_timestamps[i] = current_time; - _data_maxranges[i] = 0; - _data_fov[i] = 0; + + for (uint32_t i = 0 ; i < BIN_COUNT; i++) { _obstacle_map_body_frame.distances[i] = UINT16_MAX; } } @@ -112,48 +80,234 @@ bool CollisionPrevention::is_active() return activated; } -void -CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude) +void CollisionPrevention::modifySetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) +{ + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude; + + if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { + _vehicle_attitude = Quatf(vehicle_attitude.q); + _vehicle_yaw = Eulerf(_vehicle_attitude).psi(); + } + } + + //calculate movement constraints based on range data + const Vector2f original_setpoint = setpoint_accel; + _updateObstacleMap(); + _updateObstacleData(); + _calculateConstrainedSetpoint(setpoint_accel, setpoint_vel); + + // publish constraints + collision_constraints_s constraints{}; + original_setpoint.copyTo(constraints.original_setpoint); + setpoint_accel.copyTo(constraints.adapted_setpoint); + constraints.timestamp = getTime(); + _constraints_pub.publish(constraints); +} + +void CollisionPrevention::_updateObstacleMap() { - int msg_index = 0; - float vehicle_orientation_deg = math::degrees(Eulerf(vehicle_attitude).psi()); - float increment_factor = 1.f / obstacle.increment; + // add distance sensor data + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; + + if (dist_sens_sub.update(&distance_sensor)) { + // consider only instances with valid data and orientations useful for collision prevention + if ((getElapsedTime(&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)) { + + // update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); + _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, + (uint16_t)(distance_sensor.max_distance * 100.0f)); + _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, + (uint16_t)(distance_sensor.min_distance * 100.0f)); + + _addDistanceSensorData(distance_sensor, _vehicle_attitude); + } + } + } + + // add obstacle distance data + if (_sub_obstacle_distance.update()) { + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); + + // Update map with obstacle data if the data is not stale + if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { + //update message description + _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); + _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, + obstacle_distance.max_distance); + _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, + obstacle_distance.min_distance); + _addObstacleSensorData(obstacle_distance, _vehicle_yaw); + } + } + + // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor + _obstacle_distance_fused_pub.publish(_obstacle_map_body_frame); +} + +void CollisionPrevention::_updateObstacleData() +{ + _obstacle_data_present = false; + _closest_dist = UINT16_MAX; + _closest_dist_dir.setZero(); + + for (int i = 0; i < BIN_COUNT; i++) { + // if the data is stale, reset the bin + if (getTime() - _data_timestamps[i] > RANGE_STREAM_TIMEOUT_US) { + _obstacle_map_body_frame.distances[i] = UINT16_MAX; + } + + float angle = wrap_2pi(_vehicle_yaw + math::radians((float)i * BIN_SIZE + + _obstacle_map_body_frame.angle_offset)); + const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + const uint16_t bin_distance = _obstacle_map_body_frame.distances[i]; + + // check if there is avaliable data and the data of the map is not stale + if (bin_distance < UINT16_MAX + && (getTime() - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { + _obstacle_data_present = true; + } + + if (bin_distance * 0.01f < _closest_dist) { + _closest_dist = bin_distance * 0.01f; + _closest_dist_dir = bin_direction; + } + } +} + +void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint_accel, const Vector2f &setpoint_vel) +{ + const float setpoint_length = setpoint_accel.norm(); + _min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, _param_cp_dist.get()); + + const hrt_abstime now = getTime(); + + const bool is_stick_deflected = setpoint_length > 0.001f; + + if (_obstacle_data_present && is_stick_deflected) { + + _transformSetpoint(setpoint_accel); + + float vel_comp_accel = INFINITY; + Vector2f vel_comp_accel_dir{}; + + _getVelocityCompensationAcceleration(_vehicle_yaw, setpoint_vel, now, + vel_comp_accel, vel_comp_accel_dir); + + Vector2f constr_accel_setpoint{}; + + if (_checkSetpointDirectionFeasability()) { + constr_accel_setpoint = _constrainAccelerationSetpoint(setpoint_length); + } + + setpoint_accel = constr_accel_setpoint + vel_comp_accel * vel_comp_accel_dir; + + } else if (!_obstacle_data_present) { + // allow no movement + setpoint_accel.setZero(); + + // if distance data is stale, switch to Loiter + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + if ((now - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US && + getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + } + + PX4_WARN("No obstacle data, not moving..."); + _last_timeout_warning = now; + } + } +} + +// TODO this gives false output if the offset is not a multiple of the resolution. to be fixed... +void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) +{ + + float vehicle_orientation_deg = math::degrees(vehicle_yaw); + if (obstacle.frame == obstacle.MAV_FRAME_GLOBAL || obstacle.frame == obstacle.MAV_FRAME_LOCAL_NED) { // Obstacle message arrives in local_origin frame (north aligned) // corresponding data index (convert to world frame and shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.angle_offset) * increment_factor); - - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + 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); + + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; + } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } } else if (obstacle.frame == obstacle.MAV_FRAME_BODY_FRD) { // Obstacle message arrives in body frame (front aligned) // corresponding data index (shift by msg offset) - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { - float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG + - _obstacle_map_body_frame.angle_offset; - msg_index = ceil(wrap_360(bin_angle_deg - obstacle.angle_offset) * increment_factor); - - //add all data points inside to FOV - if (obstacle.distances[msg_index] != UINT16_MAX) { - - if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[msg_index] * 0.01f)) { - _obstacle_map_body_frame.distances[i] = obstacle.distances[msg_index]; - _data_timestamps[i] = _obstacle_map_body_frame.timestamp; - _data_maxranges[i] = obstacle.max_distance; - _data_fov[i] = 1; + 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); + + // if a bin stretches over the 0/360 degree line, adjust the angles + if (bin_lower_angle > bin_upper_angle) { + bin_lower_angle -= 360; + } + + if (msg_lower_angle > msg_upper_angle) { + msg_lower_angle -= 360; } + + // Check for overlaps. + if ((msg_lower_angle > bin_lower_angle && msg_lower_angle < bin_upper_angle) || + (msg_upper_angle > bin_lower_angle && msg_upper_angle < bin_upper_angle) || + (msg_lower_angle <= bin_lower_angle && msg_upper_angle >= bin_upper_angle) || + (msg_lower_angle >= bin_lower_angle && msg_upper_angle <= bin_upper_angle)) { + if (obstacle.distances[j] != UINT16_MAX) { + + if (_enterData(i, obstacle.max_distance * 0.01f, obstacle.distances[j] * 0.01f)) { + _obstacle_map_body_frame.distances[i] = obstacle.distances[j]; + _data_timestamps[i] = _obstacle_map_body_frame.timestamp; + _data_maxranges[i] = obstacle.max_distance; + _data_fov[i] = 1; + } + } + } + } } @@ -197,80 +351,60 @@ CollisionPrevention::_enterData(int map_index, float sensor_range, float sensor_ return false; } -void -CollisionPrevention::_updateObstacleMap() +bool +CollisionPrevention::_checkSetpointDirectionFeasability() { - _sub_vehicle_attitude.update(); + bool setpoint_feasible = true; - // add distance sensor data - for (auto &dist_sens_sub : _distance_sensor_subs) { - distance_sensor_s distance_sensor; + for (int i = 0; i < BIN_COUNT; i++) { + // check if our setpoint is either pointing in a direction where data exists, or if not, wether we are allowed to go where there is no data + if ((_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == _setpoint_index) && (!_param_cp_go_no_data.get() + || (_param_cp_go_no_data.get() && _data_fov[i]))) { + setpoint_feasible = false; - if (dist_sens_sub.update(&distance_sensor)) { - // consider only instances with valid data and orientations useful for collision prevention - if ((getElapsedTime(&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)) { - - // update message description - _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, distance_sensor.timestamp); - _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, - (uint16_t)(distance_sensor.max_distance * 100.0f)); - _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, - (uint16_t)(distance_sensor.min_distance * 100.0f)); - - _addDistanceSensorData(distance_sensor, Quatf(_sub_vehicle_attitude.get().q)); - } } } - // add obstacle distance data - if (_sub_obstacle_distance.update()) { - const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); - - // Update map with obstacle data if the data is not stale - if (getElapsedTime(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US && obstacle_distance.increment > 0.f) { - //update message description - _obstacle_map_body_frame.timestamp = math::max(_obstacle_map_body_frame.timestamp, obstacle_distance.timestamp); - _obstacle_map_body_frame.max_distance = math::max(_obstacle_map_body_frame.max_distance, - obstacle_distance.max_distance); - _obstacle_map_body_frame.min_distance = math::min(_obstacle_map_body_frame.min_distance, - obstacle_distance.min_distance); - _addObstacleSensorData(obstacle_distance, Quatf(_sub_vehicle_attitude.get().q)); - } - } + return setpoint_feasible; +} - // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor - _obstacle_distance_pub.publish(_obstacle_map_body_frame); +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) - + _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 + _setpoint_dir = setpoint.unit_or_zero(); + _adaptSetpointDirection(_setpoint_dir, _setpoint_index, _vehicle_yaw); } void -CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude) +CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &vehicle_attitude) { // clamp at maximum sensor range float distance_reading = math::min(distance_sensor.current_distance, distance_sensor.max_distance); - // discard values below min range - if ((distance_reading > distance_sensor.min_distance)) { + // negative values indicate out of range but valid measurements. + if (fabsf(distance_sensor.current_distance - -1.f) < FLT_EPSILON && distance_sensor.signal_quality == 0) { + distance_reading = distance_sensor.max_distance; + } + // 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_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad)); // calculate the field of view boundary bin indices - int lower_bound = (int)floor((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); - int upper_bound = (int)floor((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / - INTERNAL_MAP_INCREMENT_DEG); + int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); + int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE); - // floor values above zero, ceil values below zero - if (lower_bound < 0) { lower_bound++; } - - if (upper_bound < 0) { upper_bound++; } // rotate vehicle attitude into the sensor body frame - matrix::Quatf attitude_sensor_frame = vehicle_attitude; + Quatf attitude_sensor_frame = vehicle_attitude; attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad)); - float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); + float sensor_dist_scale = cosf(Eulerf(attitude_sensor_frame).theta()); // verify if (distance_reading < distance_sensor.max_distance) { distance_reading = distance_reading * sensor_dist_scale; @@ -279,7 +413,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 = _wrap_bin(bin); 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); @@ -294,8 +428,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor, void CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { - const float col_prev_d = _param_cp_dist.get(); - const int guidance_bins = floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG); + const int guidance_bins = floor(_param_cp_guide_ang.get() / BIN_SIZE); const int sp_index_original = setpoint_index; float best_cost = 9999.f; int new_sp_index = setpoint_index; @@ -307,19 +440,19 @@ 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 = _wrap_bin(j); if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) { - mean_dist += col_prev_d * 100.f; + mean_dist += _param_cp_dist.get() * 100.f; } else { mean_dist += _obstacle_map_body_frame.distances[bin]; } } - const int bin = wrap_bin(i); + const int bin = _wrap_bin(i); mean_dist = mean_dist / (2.f * filter_size + 1.f); - const float deviation_cost = col_prev_d * 50.f * abs(i - sp_index_original); + 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]; if (bin_cost < best_cost && _obstacle_map_body_frame.distances[bin] != UINT16_MAX) { @@ -330,7 +463,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi //only change setpoint direction if it was moved to a different bin if (new_sp_index != setpoint_index) { - float angle = math::radians((float)new_sp_index * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); + float angle = math::radians((float)new_sp_index * BIN_SIZE + _obstacle_map_body_frame.angle_offset); angle = wrap_2pi(vehicle_yaw_angle_rad + angle); setpoint_dir = {cosf(angle), sinf(angle)}; setpoint_index = new_sp_index; @@ -340,7 +473,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi float CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const { - float offset = angle_offset > 0.0f ? math::radians(angle_offset) : 0.0f; + float offset = math::max(math::radians(angle_offset), 0.f); switch (distance_sensor.orientation) { case distance_sensor_s::ROTATION_YAW_0: @@ -376,180 +509,147 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist break; case distance_sensor_s::ROTATION_CUSTOM: - offset = matrix::Eulerf(matrix::Quatf(distance_sensor.q)).psi(); + offset = Eulerf(Quatf(distance_sensor.q)).psi(); break; } return offset; } -void -CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, - const Vector2f &curr_vel) +float CollisionPrevention::_getObstacleDistance(const Vector2f &direction) { - _updateObstacleMap(); - - // read parameters - const float col_prev_d = _param_cp_dist.get(); - const float col_prev_dly = _param_cp_delay.get(); - const bool move_no_data = _param_cp_go_nodata.get(); - const float xy_p = _param_mpc_xy_p.get(); - const float max_jerk = _param_mpc_jerk_max.get(); - const float max_accel = _param_mpc_acc_hor.get(); - const matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); - const float vehicle_yaw_angle_rad = Eulerf(attitude).psi(); - - const float setpoint_length = setpoint.norm(); - - const hrt_abstime constrain_time = getTime(); - int num_fov_bins = 0; - - if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { - if (setpoint_length > 0.001f) { - - Vector2f setpoint_dir = setpoint / setpoint_length; - float vel_max = setpoint_length; - const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d); + float obstacle_distance = 0.f; + const float direction_norm = direction.norm(); + + if (direction_norm > FLT_EPSILON) { + 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; + } - const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) - - _obstacle_map_body_frame.angle_offset); - int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG); + return obstacle_distance; +} - // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps - _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); +Vector2f +CollisionPrevention::_constrainAccelerationSetpoint(const float &setpoint_length) +{ + Vector2f new_setpoint{}; + const Vector2f normal_component = _closest_dist_dir * (_setpoint_dir.dot(_closest_dist_dir)); + const Vector2f tangential_component = _setpoint_dir - normal_component; - // limit speed for safe flight - for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message + const float normal_scale = _getScale(_closest_dist); - // delete stale values - const hrt_abstime data_age = constrain_time - _data_timestamps[i]; - if (data_age > RANGE_STREAM_TIMEOUT_US) { - _obstacle_map_body_frame.distances[i] = UINT16_MAX; - } + const float closest_dist_tangential = _getObstacleDistance(tangential_component); + const float tangential_scale = _getScale(closest_dist_tangential); - const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters - const float max_range = _data_maxranges[i] * 0.01f; // convert to meters - float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset); - // convert from body to local frame in the range [0, 2*pi] - angle = wrap_2pi(vehicle_yaw_angle_rad + angle); + // only scale accelerations towards the obstacle + if (_closest_dist_dir.dot(_setpoint_dir) > 0) { + new_setpoint = (tangential_component * tangential_scale + normal_component * normal_scale) * setpoint_length; - // get direction of current bin - const Vector2f bin_direction = {cosf(angle), sinf(angle)}; + } else { + new_setpoint = _setpoint_dir * setpoint_length; + } - //count number of bins in the field of valid_new - if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) { - num_fov_bins ++; - } + return new_setpoint; +} - if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance - && _obstacle_map_body_frame.distances[i] < UINT16_MAX) { +float +CollisionPrevention::_getScale(const float &reference_distance) +{ + float scale = (reference_distance - _min_dist_to_keep); + const float scale_distance = math::max(_min_dist_to_keep, _param_mpc_vel_manual.get() / _param_mpc_xy_p.get()); - if (setpoint_dir.dot(bin_direction) > 0) { - // calculate max allowed velocity with a P-controller (same gain as in the position controller) - const float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction)); - float delay_distance = curr_vel_parallel * col_prev_dly; + // if scale is positive, square it and scale it with the scale_distance + scale = scale > 0 ? powf(scale / scale_distance, 2) : scale; + scale = math::min(scale, 1.0f); + return scale; +} - if (distance < max_range) { - delay_distance += curr_vel_parallel * (data_age * 1e-6f); - } +void CollisionPrevention::_getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, + const Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, Vector2f &vel_comp_accel_dir) +{ + for (int i = 0; i < BIN_COUNT; i++) { + const float max_range = _data_maxranges[i] * 0.01f; - const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); - const float vel_max_posctrl = xy_p * stop_distance; + // get the vector pointing into the direction of current bin + float bin_angle = wrap_2pi(vehicle_yaw_angle_rad + + math::radians((float)i * BIN_SIZE + _obstacle_map_body_frame.angle_offset)); - const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); - const float projection = bin_direction.dot(setpoint_dir); - float vel_max_bin = vel_max; + const Vector2f bin_direction = { cosf(bin_angle), sinf(bin_angle) }; + float bin_distance = _obstacle_map_body_frame.distances[i]; - if (projection > 0.01f) { - vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection; - } + // only consider bins which are between min and max values + if (bin_distance > _obstacle_map_body_frame.min_distance && bin_distance < UINT16_MAX) { + const float distance = bin_distance * 0.01f; - // constrain the velocity - if (vel_max_bin >= 0) { - vel_max = math::min(vel_max, vel_max_bin); - } - } + // Assume current velocity is sufficiently close to the setpoint velocity, this breaks down if flying high + // acceleration maneuvers + const float curr_vel_parallel = math::max(0.f, setpoint_vel.dot(bin_direction)); + float delay_distance = curr_vel_parallel * _param_cp_delay.get(); - } else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) { - if (!move_no_data || (move_no_data && _data_fov[i])) { - vel_max = 0.f; - } - } - } + const hrt_abstime data_age = now - _data_timestamps[i]; - //if the sensor field of view is zero, never allow to move (even if move_no_data=1) - if (num_fov_bins == 0) { - vel_max = 0.f; + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); } - setpoint = setpoint_dir * vel_max; - } + const float stop_distance = distance - _min_dist_to_keep - delay_distance; - } else { - //allow no movement - float vel_max = 0.f; - setpoint = setpoint * vel_max; + float curr_acc_vel_constraint; - // if distance data is stale, switch to Loiter - if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + if (stop_distance >= 0.f) { + const float max_vel = math::trajectory::computeMaxSpeedFromDistance(_param_mpc_jerk_max.get(), + _param_mpc_acc_hor.get(), stop_distance, 0.f); + curr_acc_vel_constraint = _param_mpc_xy_vel_p_acc.get() * math::min(max_vel - curr_vel_parallel, 0.f); - if ((constrain_time - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US - && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { - _publishVehicleCmdDoLoiter(); + } else { + curr_acc_vel_constraint = -1.f * _param_mpc_xy_vel_p_acc.get() * curr_vel_parallel; } - _last_timeout_warning = getTime(); + if (curr_acc_vel_constraint < vel_comp_accel) { + vel_comp_accel = curr_acc_vel_constraint; + vel_comp_accel_dir = bin_direction; + } } - - } } -void -CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos, - const Vector2f &curr_vel) -{ - //calculate movement constraints based on range data - Vector2f new_setpoint = original_setpoint; - _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 - || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed - || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed - || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed); - - _interfering = currently_interfering; - - // publish constraints - collision_constraints_s constraints{}; - constraints.timestamp = getTime(); - original_setpoint.copyTo(constraints.original_setpoint); - new_setpoint.copyTo(constraints.adapted_setpoint); - _constraints_pub.publish(constraints); - - original_setpoint = new_setpoint; -} - void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; - command.timestamp = getTime(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode + command.param1 = 1.f; // base mode VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; + command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; 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 + 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 aaff04e2cbdc..8fcc7c28f3ec 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -34,6 +34,7 @@ /** * @file CollisionPrevention.hpp * @author Tanja Baumann + * @author Claudio Chies * * CollisionPrevention controller. * @@ -74,21 +75,29 @@ class CollisionPrevention : public ModuleParams /** * 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 + * @param setpoint_accel setpoint purely based on sticks, to be modified + * @param setpoint_vel current velocity setpoint as information to be able to stop in time, does not get changed */ - void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, - const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); + void modifySetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); + + static constexpr int BIN_COUNT = + sizeof(obstacle_distance_s::distances) / sizeof(obstacle_distance_s::distances[0]); // 72 + static constexpr int BIN_SIZE = 360 / BIN_COUNT; // cannot be lower than 5 degrees, should divide 360 evenly protected: + /** Aggregates the sensor data into an internal obstacle map in body frame */ + void _updateObstacleMap(); + + /** Updates the obstacle data based on stale data and calculates values from the map */ + void _updateObstacleData(); + + /** Calculate the constrained setpoint considering the current obstacle distances, acceleration setpoint and velocity setpoint */ + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint_accel, const matrix::Vector2f &setpoint_vel); - obstacle_distance_s _obstacle_map_body_frame {}; - bool _data_fov[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances) / sizeof(_obstacle_map_body_frame.distances[0])]; - uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances) / sizeof( - _obstacle_map_body_frame.distances[0])]; /**< in cm */ + obstacle_distance_s _obstacle_map_body_frame{}; + bool _data_fov[BIN_COUNT] {}; + uint64_t _data_timestamps[BIN_COUNT] {}; + uint16_t _data_maxranges[BIN_COUNT] {}; /**< in cm */ void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude); @@ -96,7 +105,7 @@ class CollisionPrevention : public ModuleParams * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ - void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude); + void _addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw); /** * Computes an adaption to the setpoint direction to guide towards free space @@ -106,6 +115,34 @@ class CollisionPrevention : public ModuleParams */ void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad); + /** + * Constrain the acceleration setpoint based on the distance to the obstacle + * The Scaling of the acceleration setpoint is linear below the min_dist_to_keep and quadratic until the scale_distance above + * +1 ________ _ _ + * ┌─┐ │ // + * │X│ │ // + * │X│ │ // + * │X│ │ /// + * │X│ │ // + * │X│ │///// + * │X│──────┼─────────────┬───────────── + * │X│ /│ scale_distance + * │X│ / │ + * │X│ / │ + * │X│ / │ + * │X│ / │ + * └─┘/ │ + * -1 + */ + matrix::Vector2f _constrainAccelerationSetpoint(const float &setpoint_length); + + void _getVelocityCompensationAcceleration(const float vehicle_yaw_angle_rad, const matrix::Vector2f &setpoint_vel, + const hrt_abstime now, float &vel_comp_accel, matrix::Vector2f &vel_comp_accel_dir); + + float _getObstacleDistance(const matrix::Vector2f &direction); + + float _getScale(const float &reference_distance); + /** * Determines whether a new sensor measurement is used * @param map_index, index of the bin in the internal map the measurement belongs in @@ -114,25 +151,39 @@ class CollisionPrevention : public ModuleParams */ bool _enterData(int map_index, float sensor_range, float sensor_reading); + bool _checkSetpointDirectionFeasability(); + + void _transformSetpoint(const matrix::Vector2f &setpoint); + //Timing functions. Necessary to mock time in the tests virtual hrt_abstime getTime(); virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr); - private: - - bool _interfering{false}; /**< states if the collision prevention interferes with the user input */ + bool _data_stale{true}; /**< states if the data is stale */ bool _was_active{false}; /**< states if the collision prevention interferes with the user input */ + bool _obstacle_data_present{false}; /**< states if obstacle data is present */ + + int _setpoint_index{}; /**< index of the setpoint*/ + matrix::Vector2f _setpoint_dir{}; /**< direction of the setpoint*/ + + float _closest_dist{}; /**< closest distance to an obstacle */ + matrix::Vector2f _closest_dist_dir{NAN, NAN}; /**< direction of the closest obstacle */ + + float _min_dist_to_keep{}; orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */ + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + matrix::Quatf _vehicle_attitude{}; + float _vehicle_yaw{0.f}; + uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ - uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ + uORB::Publication _obstacle_distance_fused_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ uORB::SubscriptionData _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */ - uORB::SubscriptionData _sub_vehicle_attitude{ORB_ID(vehicle_attitude)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; static constexpr uint64_t RANGE_STREAM_TIMEOUT_US{500_ms}; @@ -142,13 +193,15 @@ class CollisionPrevention : public ModuleParams hrt_abstime _time_activated{0}; DEFINE_PARAMETERS( - (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ - (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ - (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ - (ParamBool) _param_cp_go_nodata, /**< movement allowed where no data*/ - (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ - (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ - (ParamFloat) _param_mpc_acc_hor /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_cp_dist, /**< collision prevention keep minimum distance */ + (ParamFloat) _param_cp_delay, /**< delay of the range measurement data*/ + (ParamFloat) _param_cp_guide_ang, /**< collision prevention change setpoint angle */ + (ParamBool) _param_cp_go_no_data, /**< movement allowed where no data*/ + (ParamFloat) _param_mpc_xy_p, /**< p gain from position controller*/ + (ParamFloat) _param_mpc_jerk_max, /**< vehicle maximum jerk*/ + (ParamFloat) _param_mpc_acc_hor, /**< vehicle maximum horizontal acceleration*/ + (ParamFloat) _param_mpc_xy_vel_p_acc, /**< p gain from velocity controller*/ + (ParamFloat) _param_mpc_vel_manual /**< maximum velocity in manual flight mode*/ ) /** @@ -180,14 +233,11 @@ class CollisionPrevention : public ModuleParams */ void _publishObstacleDistance(obstacle_distance_s &obstacle); - /** - * Aggregates the sensor data into a internal obstacle map in body frame - */ - void _updateObstacleMap(); - /** * Publishes vehicle command. */ void _publishVehicleCmdDoLoiter(); + static float _wrap_360(const float f); + static int _wrap_bin(int i); }; diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa00..aa98ef821d0a 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -34,8 +34,12 @@ #include #include "CollisionPrevention.hpp" +using namespace matrix; + // to run: make tests TESTFILTER=CollisionPrevention hrt_abstime mocked_time = 0; +const uint bin_size = CollisionPrevention::BIN_SIZE; +const uint bin_count = CollisionPrevention::BIN_COUNT; class CollisionPreventionTest : public ::testing::Test { @@ -53,15 +57,15 @@ class TestCollisionPrevention : public CollisionPrevention TestCollisionPrevention() : CollisionPrevention(nullptr) {} void paramsChanged() {CollisionPrevention::updateParamsImpl();} obstacle_distance_s &getObstacleMap() {return _obstacle_map_body_frame;} - void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &attitude) + void test_addDistanceSensorData(distance_sensor_s &distance_sensor, const Quatf &attitude) { _addDistanceSensorData(distance_sensor, attitude); } - void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &attitude) + void test_addObstacleSensorData(const obstacle_distance_s &obstacle, const float vehicle_yaw) { - _addObstacleSensorData(obstacle, attitude); + _addObstacleSensorData(obstacle, vehicle_yaw); } - void test_adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, + void test_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad) { _adaptSetpointDirection(setpoint_dir, setpoint_index, vehicle_yaw_angle_rad); @@ -103,10 +107,8 @@ TEST_F(CollisionPreventionTest, noSensorData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3.f; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -116,8 +118,8 @@ TEST_F(CollisionPreventionTest, noSensorData) param_set(param, &value); cp.paramsChanged(); - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: collision prevention should be enabled and limit the speed to zero EXPECT_TRUE(cp.is_active()); @@ -128,11 +130,9 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -141,9 +141,12 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) attitude.q[3] = 0.0f; // AND: a parameter handle - param_t param = param_handle(px4::params::CP_DIST); - float value = 10; // try to keep 10m distance - param_set(param, &value); + param_t param1 = param_handle(px4::params::CP_DIST); + float value1 = 10; // try to keep 10m distance + param_set(param1, &value1); + param_t param2 = param_handle(px4::params::CP_GUIDE_ANG); + float value2 = 0; // dont guide sideways + param_set(param2, &value2); cp.paramsChanged(); // AND: an obstacle message @@ -164,7 +167,6 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) } else { message.distances[i] = 10001; } - } // WHEN: we publish the message and set the parameter and then run the setpoint modification @@ -172,32 +174,31 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; - cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); - cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; + cp.modifySetpoint(modified_setpoint1, curr_vel); + cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(obstacle_distance_pub); orb_unadvertise(vehicle_attitude_pub); // THEN: the internal map should know the obstacle - // case 1: the velocity setpoint should be cut down to zero - // case 2: the velocity setpoint should stay the same as the input + // case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low + // case 2: the acceleration setpoint should be lower EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); - - EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); - EXPECT_FLOAT_EQ(original_setpoint2.norm(), modified_setpoint2.norm()); + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); + EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1); } TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint1(10, 0); - matrix::Vector2f original_setpoint2(-10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint1(10, 0); + Vector2f original_setpoint2(-10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -232,21 +233,26 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); //WHEN: We run the setpoint modification - matrix::Vector2f modified_setpoint1 = original_setpoint1; - matrix::Vector2f modified_setpoint2 = original_setpoint2; - cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); - cp.modifySetpoint(modified_setpoint2, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint1 = original_setpoint1; + Vector2f modified_setpoint2 = original_setpoint2; + cp.modifySetpoint(modified_setpoint1, curr_vel); + cp.modifySetpoint(modified_setpoint2, curr_vel); orb_unadvertise(distance_sensor_pub); orb_unadvertise(vehicle_attitude_pub); // THEN: the internal map should know the obstacle - // case 1: the velocity setpoint should be cut down to zero because there is an obstacle - // case 2: the velocity setpoint should be cut down to zero because there is no data + // case 1: the acceleration setpoint should be negative as its pushing you away from the obstacle, and sideways acceleration should be low + // case 2: the acceleration setpoint should be lower EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); - EXPECT_FLOAT_EQ(0.f, modified_setpoint1.norm()) << modified_setpoint1(0) << "," << modified_setpoint1(1); - EXPECT_FLOAT_EQ(0.f, modified_setpoint2.norm()) << modified_setpoint2(0) << "," << modified_setpoint2(1); + EXPECT_FLOAT_EQ(cp.getObstacleMap().min_distance, 100); + EXPECT_FLOAT_EQ(cp.getObstacleMap().max_distance, 10000); + + EXPECT_GT(0.f, modified_setpoint1(0)) << modified_setpoint1(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint1(1))) << modified_setpoint1(1); + EXPECT_GT(0.f, modified_setpoint2(0)) << original_setpoint2(0); + EXPECT_EQ(0.f, fabsf(modified_setpoint2(1))) << modified_setpoint2(1); } TEST_F(CollisionPreventionTest, testPurgeOldData) @@ -255,10 +261,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -268,7 +272,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 10; // try to keep 10m distance + float value = 1; // try to keep 10m distance param_set(param, &value); cp.paramsChanged(); @@ -306,9 +310,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); mocked_time = mocked_time + 100000; //advance time by 0.1 seconds message_lost_data.timestamp = mocked_time; @@ -344,10 +347,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) TestTimingCollisionPrevention cp; hrt_abstime start_time = hrt_absolute_time(); mocked_time = start_time; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = start_time; attitude.q[0] = 1.0f; @@ -384,9 +385,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { - - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); //advance time by 0.1 seconds but no new message comes in mocked_time = mocked_time + 100000; @@ -410,14 +410,12 @@ TEST_F(CollisionPreventionTest, noBias) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 5; // try to keep 5m distance + float value = 2; // try to keep 2m distance param_set(param, &value); cp.paramsChanged(); @@ -438,8 +436,8 @@ TEST_F(CollisionPreventionTest, noBias) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); orb_unadvertise(obstacle_distance_pub); // THEN: setpoint should go into the same direction as the stick input @@ -451,9 +449,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); @@ -486,22 +482,21 @@ TEST_F(CollisionPreventionTest, outsideFOV) orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); for (int i = 0; i < distances_array_size; i++) { - float angle_deg = (float)i * message.increment; float angle_rad = math::radians(angle_deg); - matrix::Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; + Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); //THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV float setpoint_length = modified_setpoint.norm(); if (setpoint_length > 0.f) { - matrix::Vector2f setpoint_dir = modified_setpoint / setpoint_length; + Vector2f setpoint_dir = modified_setpoint / setpoint_length; float sp_angle_body_frame = atan2(setpoint_dir(1), setpoint_dir(0)); - float sp_angle_deg = math::degrees(matrix::wrap_2pi(sp_angle_body_frame)); + float sp_angle_deg = math::degrees(wrap_2pi(sp_angle_body_frame)); EXPECT_GE(sp_angle_deg, 45.f); EXPECT_LE(sp_angle_deg, 225.f); } @@ -514,9 +509,7 @@ TEST_F(CollisionPreventionTest, goNoData) { // GIVEN: a simple setup condition with the initial state (no distance data) TestCollisionPrevention cp; - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f curr_vel(2, 0); // AND: an obstacle message obstacle_distance_s message; @@ -532,7 +525,7 @@ TEST_F(CollisionPreventionTest, goNoData) float angle = i * message.increment; if (angle > 0.f && angle < 40.f) { - message.distances[i] = 700; + message.distances[i] = 1000; } else { message.distances[i] = UINT16_MAX; @@ -541,16 +534,16 @@ TEST_F(CollisionPreventionTest, goNoData) // AND: a parameter handle param_t param = param_handle(px4::params::CP_DIST); - float value = 5; // try to keep 5m distance + float value = 2; // try to keep 5m distance param_set(param, &value); cp.paramsChanged(); // AND: a setpoint outside the field of view - matrix::Vector2f original_setpoint = {-5, 0}; - matrix::Vector2f modified_setpoint = original_setpoint; + Vector2f original_setpoint = {-5, 0}; + Vector2f modified_setpoint = original_setpoint; - //THEN: the modified setpoint should be zero velocity - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + //THEN: the modified setpoint should be zero acceleration + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); //WHEN: we change the parameter CP_GO_NO_DATA to allow flying ouside the FOV @@ -561,7 +554,7 @@ TEST_F(CollisionPreventionTest, goNoData) //THEN: When all bins contain UINT_16MAX the setpoint should be zero even if CP_GO_NO_DATA=1 modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), 0.f); //THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed @@ -570,7 +563,7 @@ TEST_F(CollisionPreventionTest, goNoData) orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); EXPECT_FLOAT_EQ(modified_setpoint.norm(), original_setpoint.norm()); orb_unadvertise(obstacle_distance_pub); } @@ -579,10 +572,8 @@ TEST_F(CollisionPreventionTest, jerkLimit) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); // AND: distance set to 5m param_t param = param_handle(px4::params::CP_DIST); @@ -607,8 +598,8 @@ TEST_F(CollisionPreventionTest, jerkLimit) // AND: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; - cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint_default_jerk = original_setpoint; + cp.modifySetpoint(modified_setpoint_default_jerk, curr_vel); orb_unadvertise(obstacle_distance_pub); // AND: we now set max jerk to 0.1 @@ -618,19 +609,101 @@ TEST_F(CollisionPreventionTest, jerkLimit) cp.paramsChanged(); // WHEN: we run the setpoint modification again - matrix::Vector2f modified_setpoint_limited_jerk = original_setpoint; - cp.modifySetpoint(modified_setpoint_limited_jerk, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint_limited_jerk = original_setpoint; + cp.modifySetpoint(modified_setpoint_limited_jerk, curr_vel); + + // THEN: the new setpoint should be much higher than the one with default jerk, as the rate of change in acceleration is more limmited + EXPECT_GT(modified_setpoint_limited_jerk.norm(), modified_setpoint_default_jerk.norm()); + +} +TEST_F(CollisionPreventionTest, addOutOfRangeDistanceSensorData) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + // Distance is out of Range + distance_sensor.current_distance = -1.f; + distance_sensor.signal_quality = 0; + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], distance_sensor.max_distance * 100.f); + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + } + } +} + +TEST_F(CollisionPreventionTest, addDistanceSensorDataNarrow) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(0.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the correct bins in the map should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 0) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - // THEN: the new setpoint should be much slower than the one with default jerk - EXPECT_LT(modified_setpoint_limited_jerk.norm() * 10, modified_setpoint_default_jerk.norm()); + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } +} +TEST_F(CollisionPreventionTest, addDistanceSensorDataSlightlyLarger) +{ + // GIVEN: a vehicle attitude and a distance sensor message + TestCollisionPrevention cp; + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + distance_sensor_s distance_sensor {}; + distance_sensor.min_distance = 0.2f; + distance_sensor.max_distance = 20.f; + distance_sensor.current_distance = 5.f; + distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + distance_sensor.h_fov = math::radians(1.1 * bin_size); + + uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + // WHEN the sensor has a very narrow field of view + cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + + //THEN: the the bins corresponding to -5°, 0° and 5° should be filled + for (uint32_t i = 0; i < distances_array_size; i++) { + if (i == 71 || i <= 1) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + } } TEST_F(CollisionPreventionTest, addDistanceSensorData) { // GIVEN: a vehicle attitude and a distance sensor message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform distance_sensor_s distance_sensor {}; distance_sensor.min_distance = 0.2f; distance_sensor.max_distance = 20.f; @@ -640,21 +713,24 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) uint32_t distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (uint32_t i = 0; i < distances_array_size; i++) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //WHEN: we add distance sensor data to the right distance_sensor.orientation = distance_sensor_s::ROTATION_RIGHT_FACING; distance_sensor.h_fov = math::radians(19.99f); cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + uint fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start = (90 - fov) / bin_size; + uint end = (90 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -663,17 +739,20 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(50.f); distance_sensor.current_distance = 8.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start2 = (270 - fov) / bin_size; + uint end2 = (270 + fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -682,20 +761,23 @@ TEST_F(CollisionPreventionTest, addDistanceSensorData) distance_sensor.h_fov = math::radians(10.1f); distance_sensor.current_distance = 3.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); + fov = round(distance_sensor.h_fov * M_RAD_TO_DEG_F / 2); + uint start3 = (360 - fov) / bin_size; + uint end3 = (fov) / bin_size; //THEN: the correct bins in the map should be filled for (uint32_t i = 0; i < distances_array_size; i++) { - if (i == 8 || i == 9) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; - } else if (i >= 24 && i <= 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800); + } else if (i >= start2 && i <= end2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 800) << i; - } else if (i == 0) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300); + } else if (i >= start3 || i <= end3) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 300) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } } @@ -706,7 +788,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 5.f; @@ -714,22 +795,18 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 2; i < 6 ; i++) { + int start = 2; + int end = 6; + + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } + //THEN: at initialization the internal obstacle map should only contain UINT16_MAX int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); @@ -738,15 +815,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } //WHEN: we add distance sensor data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -754,44 +831,50 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } - //WHEN: we add distance sensor data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + //WHEN: we add obstacle distance sensor data while vehicle yaw 90deg to the right + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled + int offset = bin_count - 90 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 28 || i == 29) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + //WHEN: we add obstacle distance sensor data while vehicle yaw 45deg to the left + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled + offset = 45 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 6 || i == 7) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= offset + start && i <= offset + end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + //WHEN: we add obstacle distance sensor data while vehicle yaw 180deg + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled + offset = 180 / bin_size; + for (int i = 0; i < distances_array_size; i++) { - if (i == 19 || i == 20) { + if (i >= offset + start && i <= offset + end) { EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); } else { @@ -803,11 +886,84 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_attitude) } } +TEST_F(CollisionPreventionTest, addObstacleSensorData_offset_bodyframe) +{ + // GIVEN: a vehicle attitude and obstacle distance message + TestCollisionPrevention cp; + obstacle_distance_s obstacle_msg {}; + obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; // Body Frame + obstacle_msg.increment = 6.f; + obstacle_msg.min_distance = 20; + obstacle_msg.max_distance = 2000; + obstacle_msg.angle_offset = 0.f; + + //obstacle at 363°-39° deg world frame, distance 5 meters + memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment + obstacle_msg.distances[i] = 500; + } + + //WHEN: we add distance sensor data + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° + int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° + for (int i = 0; i < distances_array_size; i++) { + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } +} + TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_BODY_FRD; //north aligned obstacle_msg.increment = 5.f; @@ -815,18 +971,12 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude1(1, 0, 0, 0); //unit transform - matrix::Euler attitude2_euler(0, 0, M_PI / 2.0); - matrix::Quaternion vehicle_attitude2(attitude2_euler); //90 deg yaw - matrix::Euler attitude3_euler(0, 0, -M_PI / 4.0); - matrix::Quaternion vehicle_attitude3(attitude3_euler); // -45 deg yaw - matrix::Euler attitude4_euler(0, 0, M_PI); - matrix::Quaternion vehicle_attitude4(attitude4_euler); // 180 deg yaw - //obstacle at 10-30 deg body frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); + int start = 2; + int end = 6; - for (int i = 2; i < 6 ; i++) { + for (int i = start; i <= end ; i++) { obstacle_msg.distances[i] = 500; } @@ -839,15 +989,16 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle has zero yaw - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude1); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); //THEN: the correct bins in the map should be filled + for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -855,15 +1006,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 90deg to the right - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude2); + cp.test_addObstacleSensorData(obstacle_msg, M_PI_2); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -871,15 +1022,15 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 45deg to the left - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude3); + cp.test_addObstacleSensorData(obstacle_msg, -M_PI_4); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -887,20 +1038,21 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_bodyframe) } //WHEN: we add obstacle data while vehicle yaw 180deg - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude4); + cp.test_addObstacleSensorData(obstacle_msg, M_PI); //THEN: the correct bins in the map should be filled for (int i = 0; i < distances_array_size; i++) { - if (i == 1 || i == 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= start && i <= end) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } + } @@ -908,7 +1060,6 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned obstacle_msg.increment = 6.f; @@ -916,44 +1067,62 @@ TEST_F(CollisionPreventionTest, addObstacleSensorData_resolution_offset) obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - - //obstacle at 0-30 deg world frame, distance 5 meters + //obstacle at 363°-39° deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 5 ; i++) { + for (int i = 0; i <= 6 ; i++) { // 36° at 6° increment obstacle_msg.distances[i] = 500; } //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the the bins from 0 to 40 in map should be filled, which correspond to the angles from -2.5° to 42.5° int distances_array_size = sizeof(cp.getObstacleMap().distances) / sizeof(cp.getObstacleMap().distances[0]); for (int i = 0; i < distances_array_size; i++) { - if (i >= 0 && i <= 2) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 0 && i <= 8) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX cp.getObstacleMap().distances[i] = UINT16_MAX; } - //WHEN: we add distance sensor data with an angle offset - obstacle_msg.angle_offset = 30.f; - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); + //WHEN: we add the same obstacle distance sensor data with an angle offset of 30.5° + obstacle_msg.angle_offset = 30.5f; + // This then means our obstacle is between 27.5° and 69.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); - //THEN: the correct bins in the map should be filled + //THEN: the bins from 30° to 70° in map should be filled, which correspond to the angles from 27.5° to 72.5° for (int i = 0; i < distances_array_size; i++) { - if (i >= 3 && i <= 5) { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500); + if (i >= 6 && i <= 14) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; } else { - EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX); + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; + } + + //reset array to UINT16_MAX + cp.getObstacleMap().distances[i] = UINT16_MAX; + } + + //WHEN: we increase the offset to -30.5° + obstacle_msg.angle_offset = -30.5f; + // This then means our obstacle is between 326.5° and 8.5° + cp.test_addObstacleSensorData(obstacle_msg, 0.f); + + //THEN: the bins from 325° to 10° in map should be filled, which correspond to the angles from 322.5° to 12.5° + + for (int i = 0; i < distances_array_size; i++) { + if (i >= 65 || i <= 2) { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], 500) << i; + + } else { + EXPECT_FLOAT_EQ(cp.getObstacleMap().distances[i], UINT16_MAX) << i; } //reset array to UINT16_MAX @@ -965,31 +1134,29 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); - for (int i = 0; i < 7 ; i++) { + for (int i = 0; i <= 6 ; i++) { obstacle_msg.distances[i] = 500; } obstacle_msg.distances[2] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + Vector2f setpoint_dir(1, 0); + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -999,29 +1166,27 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_distinct_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) { // GIVEN: a vehicle attitude and obstacle distance message TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; obstacle_distance_s obstacle_msg {}; obstacle_msg.frame = obstacle_msg.MAV_FRAME_GLOBAL; //north aligned - obstacle_msg.increment = 10.f; + obstacle_msg.increment = 5.f; obstacle_msg.min_distance = 20; obstacle_msg.max_distance = 2000; obstacle_msg.angle_offset = 0.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform - float vehicle_yaw_angle_rad = matrix::Eulerf(vehicle_attitude).psi(); + const float vehicle_yaw = 0.f; //obstacle at 0-30 deg world frame, distance 5 meters memset(&obstacle_msg.distances[0], UINT16_MAX, sizeof(obstacle_msg.distances)); @@ -1035,10 +1200,10 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) obstacle_msg.distances[3] = 1000; //define setpoint - matrix::Vector2f setpoint_dir(1, 0); - float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad; - float sp_angle_with_offset_deg = matrix::wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, - 0.f, 360.f); + Vector2f setpoint_dir(1, 0); + float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw; + float sp_angle_with_offset_deg = wrap(math::degrees(sp_angle_body_frame) - cp.getObstacleMap().angle_offset, + 0.f, 360.f); int sp_index = floor(sp_angle_with_offset_deg / cp.getObstacleMap().increment); //set parameter @@ -1048,23 +1213,21 @@ TEST_F(CollisionPreventionTest, adaptSetpointDirection_flat_minimum) cp.paramsChanged(); //WHEN: we add distance sensor data - cp.test_addObstacleSensorData(obstacle_msg, vehicle_attitude); - cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + cp.test_addObstacleSensorData(obstacle_msg, vehicle_yaw); + cp.test_adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw); //THEN: the setpoint direction should be modified correctly EXPECT_EQ(sp_index, 2); - EXPECT_FLOAT_EQ(setpoint_dir(0), 0.93969262f); - EXPECT_FLOAT_EQ(setpoint_dir(1), 0.34202012f); + EXPECT_FLOAT_EQ(setpoint_dir(0), 0.98480773f); + EXPECT_FLOAT_EQ(setpoint_dir(1), 0.17364818f); } TEST_F(CollisionPreventionTest, overlappingSensors) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - matrix::Vector2f original_setpoint(10, 0); - float max_speed = 3; - matrix::Vector2f curr_pos(0, 0); - matrix::Vector2f curr_vel(2, 0); + Vector2f original_setpoint(10, 0); + Vector2f curr_vel(2, 0); vehicle_attitude_s attitude; attitude.timestamp = hrt_absolute_time(); attitude.q[0] = 1.0f; @@ -1114,8 +1277,8 @@ TEST_F(CollisionPreventionTest, overlappingSensors) orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); - matrix::Vector2f modified_setpoint = original_setpoint; - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + Vector2f modified_setpoint = original_setpoint; + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the long range measurement EXPECT_EQ(500, cp.getObstacleMap().distances[2]); @@ -1124,10 +1287,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // WHEN: we publish the short range message followed by a long range message short_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the short range measurement EXPECT_EQ(150, cp.getObstacleMap().distances[2]); @@ -1136,10 +1299,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // WHEN: we publish the short range message with values out of range followed by a long range message short_range_msg_no_obstacle.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); + cp.modifySetpoint(modified_setpoint, curr_vel); // THEN: the internal map data should contain the short range measurement EXPECT_EQ(500, cp.getObstacleMap().distances[2]); @@ -1152,16 +1315,15 @@ TEST_F(CollisionPreventionTest, enterData) { // GIVEN: a simple setup condition TestCollisionPrevention cp; - cp.getObstacleMap().increment = 10.f; - matrix::Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform + Quaternion vehicle_attitude(1, 0, 0, 0); //unit transform //THEN: just after initialization all bins are at UINT16_MAX and any data should be accepted - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with a valid reading distance_sensor_s distance_sensor {}; @@ -1173,36 +1335,38 @@ TEST_F(CollisionPreventionTest, enterData) cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(500, cp.getObstacleMap().distances[8]); - EXPECT_EQ(500, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(500, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain valid readings // a valid reading should only be accepted from sensors with shorter or equal range // a out of range reading should only be accepted from sensors with the same range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range //WHEN: we add distance sensor data to the right with an out of range reading distance_sensor.current_distance = 21.f; cp.test_addDistanceSensorData(distance_sensor, vehicle_attitude); //THEN: the internal map should contain the distance sensor readings - EXPECT_EQ(2000, cp.getObstacleMap().distances[8]); - EXPECT_EQ(2000, cp.getObstacleMap().distances[9]); + for (int i = 16; i < 20; i++) { + EXPECT_EQ(2000, cp.getObstacleMap().distances[i]) << i; + } //THEN: bins 8 & 9 contain readings out of range // a reading in range will be accepted in any case // out of range readings will only be accepted from sensors with bigger or equal range - EXPECT_TRUE(cp.test_enterData(8, 2.f, 1.5f)); //shorter range, reading in range - EXPECT_FALSE(cp.test_enterData(8, 2.f, 3.f)); //shorter range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 1.5f)); //same range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 20.f, 21.f)); //same range, reading out of range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 1.5f)); //longer range, reading in range - EXPECT_TRUE(cp.test_enterData(8, 30.f, 31.f)); //longer range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 2.f, 1.5f)); //shorter range, reading in range + EXPECT_FALSE(cp.test_enterData(16, 2.f, 3.f)); //shorter range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 1.5f)); //same range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 20.f, 21.f)); //same range, reading out of range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 1.5f)); //longer range, reading in range + EXPECT_TRUE(cp.test_enterData(16, 30.f, 31.f)); //longer range, reading out of range } diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt index 81ff6a7a334e..84ccba6ac052 100644 --- a/src/modules/flight_mode_manager/CMakeLists.txt +++ b/src/modules/flight_mode_manager/CMakeLists.txt @@ -47,7 +47,6 @@ list(APPEND flight_tasks_all ManualAltitude ManualAltitudeSmoothVel ManualPosition - ManualPositionSmoothVel Transition ) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 3d7072607358..c58a7fa1572b 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -213,10 +213,6 @@ void FlightModeManager::start_flight_task() error = switchTask(FlightTaskIndex::ManualPosition); break; - case 3: - error = switchTask(FlightTaskIndex::ManualPositionSmoothVel); - break; - case 4: default: if (_param_mpc_pos_mode.get() != 4) { diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 71dedaf0566a..352875764e30 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -94,11 +94,6 @@ void FlightTaskManualPosition::_scaleSticks() // Rotate setpoint into local frame Sticks::rotateIntoHeadingFrameXY(vel_sp_xy, _yaw, _yaw_setpoint); - // collision prevention - if (_collision_prevention.is_active()) { - _collision_prevention.modifySetpoint(vel_sp_xy, velocity_scale, _position.xy(), _velocity.xy()); - } - _velocity_setpoint.xy() = vel_sp_xy; } diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt deleted file mode 100644 index 8d4489fa3e55..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(FlightTaskManualPositionSmoothVel - FlightTaskManualPositionSmoothVel.cpp -) - -target_link_libraries(FlightTaskManualPositionSmoothVel PUBLIC FlightTaskManualPosition FlightTaskUtility) -target_include_directories(FlightTaskManualPositionSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp deleted file mode 100644 index a89283e14619..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ /dev/null @@ -1,185 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "FlightTaskManualPositionSmoothVel.hpp" - -#include -#include - -using namespace matrix; - -bool FlightTaskManualPositionSmoothVel::activate(const trajectory_setpoint_s &last_setpoint) -{ - bool ret = FlightTaskManualPosition::activate(last_setpoint); - - // Check if the previous FlightTask provided setpoints - Vector3f accel_prev{last_setpoint.acceleration}; - Vector3f vel_prev{last_setpoint.velocity}; - Vector3f pos_prev{last_setpoint.position}; - - for (int i = 0; i < 3; i++) { - // If the position setpoint is unknown, set to the current postion - if (!PX4_ISFINITE(pos_prev(i))) { pos_prev(i) = _position(i); } - - // If the velocity setpoint is unknown, set to the current velocity - if (!PX4_ISFINITE(vel_prev(i))) { vel_prev(i) = _velocity(i); } - - // No acceleration estimate available, set to zero if the setpoint is NAN - if (!PX4_ISFINITE(accel_prev(i))) { accel_prev(i) = 0.f; } - } - - _smoothing_xy.reset(Vector2f{accel_prev}, Vector2f{vel_prev}, Vector2f{pos_prev}); - _smoothing_z.reset(accel_prev(2), vel_prev(2), pos_prev(2)); - - return ret; -} - -void FlightTaskManualPositionSmoothVel::reActivate() -{ - FlightTaskManualPosition::reActivate(); - // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly - // using the generated jerk, reset the z derivatives to zero - _smoothing_xy.reset(Vector2f(), _velocity.xy(), _position.xy()); - _smoothing_z.reset(0.f, 0.f, _position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) -{ - _smoothing_xy.setCurrentPosition(_position.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) -{ - _smoothing_xy.setCurrentVelocity(_velocity.xy()); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerPositionZ(float delta_z) -{ - _smoothing_z.setCurrentPosition(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_ekfResetHandlerVelocityZ(float delta_vz) -{ - _smoothing_z.setCurrentVelocity(_velocity(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateSetpoints() -{ - // Set max accel/vel/jerk - // Has to be done before _updateTrajectories() - _updateTrajConstraints(); - _updateTrajVelFeedback(); - _updateTrajCurrentPositionEstimate(); - - // Get yaw setpoint, un-smoothed position setpoints - FlightTaskManualPosition::_updateSetpoints(); - - _updateTrajectories(_velocity_setpoint); - - // Fill the jerk, acceleration, velocity and position setpoint vectors - _setOutputState(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraints() -{ - _updateTrajConstraintsXY(); - _updateTrajConstraintsZ(); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsXY() -{ - _smoothing_xy.setMaxJerk(_param_mpc_jerk_max.get()); - _smoothing_xy.setMaxAccel(_param_mpc_acc_hor_max.get()); - _smoothing_xy.setMaxVel(_param_mpc_vel_manual.get()); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajConstraintsZ() -{ - _smoothing_z.setMaxJerk(_param_mpc_jerk_max.get()); - - _smoothing_z.setMaxAccelUp(_param_mpc_acc_up_max.get()); - _smoothing_z.setMaxVelUp(_constraints.speed_up); - - _smoothing_z.setMaxAccelDown(_param_mpc_acc_down_max.get()); - _smoothing_z.setMaxVelDown(_constraints.speed_down); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajVelFeedback() -{ - _smoothing_xy.setVelSpFeedback(_velocity_setpoint_feedback.xy()); - _smoothing_z.setVelSpFeedback(_velocity_setpoint_feedback(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajCurrentPositionEstimate() -{ - _smoothing_xy.setCurrentPositionEstimate(_position.xy()); - _smoothing_z.setCurrentPositionEstimate(_position(2)); -} - -void FlightTaskManualPositionSmoothVel::_updateTrajectories(Vector3f vel_target) -{ - _smoothing_xy.update(_deltatime, vel_target.xy()); - _smoothing_z.update(_deltatime, vel_target(2)); -} - -void FlightTaskManualPositionSmoothVel::_setOutputState() -{ - _setOutputStateXY(); - _setOutputStateZ(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateXY() -{ - _jerk_setpoint.xy() = _smoothing_xy.getCurrentJerk(); - _acceleration_setpoint.xy() = _smoothing_xy.getCurrentAcceleration(); - _velocity_setpoint.xy() = _smoothing_xy.getCurrentVelocity(); - _position_setpoint.xy() = _smoothing_xy.getCurrentPosition(); -} - -void FlightTaskManualPositionSmoothVel::_setOutputStateZ() -{ - _jerk_setpoint(2) = _smoothing_z.getCurrentJerk(); - _acceleration_setpoint(2) = _smoothing_z.getCurrentAcceleration(); - _velocity_setpoint(2) = _smoothing_z.getCurrentVelocity(); - - if (!_terrain_hold) { - if (_terrain_hold_previous) { - // Reset position setpoint to current position when switching from terrain hold to non-terrain hold - _smoothing_z.setCurrentPosition(_position(2)); - } - - _position_setpoint(2) = _smoothing_z.getCurrentPosition(); - } - - _terrain_hold_previous = _terrain_hold; -} diff --git a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp deleted file mode 100644 index c8fbdf42c95a..000000000000 --- a/src/modules/flight_mode_manager/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file FlightTaskManualPositionSmoothVel.hpp - * - * Flight task for smooth manual controlled position. - */ - -#pragma once - -#include "FlightTaskManualPosition.hpp" -#include -#include - -using matrix::Vector2f; -using matrix::Vector3f; - -class FlightTaskManualPositionSmoothVel : public FlightTaskManualPosition -{ -public: - FlightTaskManualPositionSmoothVel() = default; - virtual ~FlightTaskManualPositionSmoothVel() = default; - - bool activate(const trajectory_setpoint_s &last_setpoint) override; - void reActivate() override; - -protected: - - virtual void _updateSetpoints() override; - - /** Reset position or velocity setpoints in case of EKF reset event */ - void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) override; - void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; - void _ekfResetHandlerPositionZ(float delta_z) override; - void _ekfResetHandlerVelocityZ(float delta_vz) override; - - DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualPosition, - (ParamFloat) _param_mpc_jerk_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_acc_down_max - ) - -private: - void _updateTrajConstraints(); - void _updateTrajConstraintsXY(); - void _updateTrajConstraintsZ(); - - void _updateTrajVelFeedback(); - void _updateTrajCurrentPositionEstimate(); - - void _updateTrajectories(Vector3f vel_target); - - void _setOutputState(); - void _setOutputStateXY(); - void _setOutputStateZ(); - - ManualVelocitySmoothingXY _smoothing_xy; ///< Smoothing in x and y directions - ManualVelocitySmoothingZ _smoothing_z; ///< Smoothing in z direction - - bool _terrain_hold_previous{false}; /**< true when vehicle was controlling height above a static ground position in the previous iteration */ -}; diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index c0fe9b315b67..f823f929e819 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -96,6 +96,10 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp); _acceleration_setpoint = stick_xy.emult(acceleration_scale); + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(_acceleration_setpoint, _velocity_setpoint); + } + // Add drag to limit speed and brake again Vector2f drag = calculateDrag(acceleration_scale.edivide(velocity_scale), dt, stick_xy, _velocity_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 8bc2bb75313d..9fff1da1c426 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -65,6 +66,8 @@ class StickAccelerationXY : public ModuleParams void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; private: + CollisionPrevention _collision_prevention{this}; + void applyJerkLimit(const float dt); matrix::Vector2f calculateDrag(matrix::Vector2f drag_coefficient, const float dt, const matrix::Vector2f &stick_xy, const matrix::Vector2f &vel_sp); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index f7cd9bbbe09a..6d58b85cd7ad 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -167,7 +167,6 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_vel_man_side, (ParamFloat) _param_mpc_xy_cruise, (ParamFloat) _param_mpc_land_alt2, /**< downwards speed limited below this altitude */ - (ParamInt) _param_mpc_pos_mode, (ParamInt) _param_mpc_alt_mode, (ParamFloat) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */ (ParamFloat) _param_mpc_thr_min, diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index a65ef4f5713d..9fce344319c8 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -39,14 +39,10 @@ * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * - "Smoothed velocity": - * Sticks map to velocity but with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). * - "Acceleration based": * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity - * @value 3 Smoothed velocity * @value 4 Acceleration based * @group Multicopter Position Control */ @@ -104,7 +100,6 @@ PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); * * MPC_POS_MODE * 1 just deceleration - * 3 acceleration and deceleration * 4 not used, use MPC_ACC_HOR instead * * @unit m/s^2 @@ -125,7 +120,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. + * Only used with MPC_POS_MODE Acceleration based. * * @unit m/s^3 * @min 0.5 diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 358a4e703226..a094298fab90 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -90,6 +90,7 @@ if(gz-transport_FOUND) lawn aruco rover + walls ) # find corresponding airframes diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 77858bb96452..95be696835e2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -816,7 +816,7 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) { - static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each + static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors double angle_min_deg = scan.angle_min() * 180 / M_PI; double angle_step_deg = scan.angle_step() * 180 / M_PI;