Ultrasonic & Proximity Sensors: Drone Collision Avoidance

class UltrasonicSensor:
    """Simulated ultrasonic sensor with temperature compensation."""

    def __init__(self, trigger_pin=None, echo_pin=None):
        self.trigger_pin = trigger_pin
        self.echo_pin = echo_pin
        self.temperature = 20.0  # °C

    def speed_of_sound(self):
        """Calculate speed of sound based on temperature."""
        return 331.3 + 0.606 * self.temperature

    def measure_distance(self, echo_time_us):
        """Calculate distance from echo time in microseconds."""
        speed = self.speed_of_sound()  # m/s
        time_s = echo_time_us / 1_000_000  # Convert μs to seconds
        distance = (speed * time_s) / 2
        return distance

    def measure_with_timeout(self, max_distance=4.0, timeout_ms=30):
        """Simulate a measurement with timeout."""
        # In real code: trigger pulse, wait for echo
        # This simulates the timing
        max_time_us = (2 * max_distance / self.speed_of_sound()) * 1_000_000
        return max_time_us

# Demo
sensor = UltrasonicSensor()
sensor.temperature = 25.0

# Simulated echo times (microseconds)
echo_times = [580, 1160, 2900, 5800]
for et in echo_times:
    dist = sensor.measure_distance(et)
    print(f"Echo time: {et}μs → Distance: {dist:.2f}m")

class ObstacleAvoidance:
    """Simple ultrasonic-based obstacle avoidance."""

    def __init__(self, sensors=None):
        self.sensors = sensors or ['front', 'left', 'right', 'rear', 'bottom']
        self.stop_distance = 0.3    # meters
        self.slow_distance = 1.0    # meters
        self.cautious_distance = 2.0  # meters

    def analyze_readings(self, readings):
        """Determine action based on sensor readings."""
        min_dist = min(readings.values())
        closest_sensor = min(readings, key=readings.get)

        if min_dist < self.stop_distance:
            return "EMERGENCY_STOP", closest_sensor, min_dist
        elif min_dist < self.slow_distance:
            return "REDUCE_SPEED", closest_sensor, min_dist
        elif min_dist < self.cautious_distance:
            return "CAUTIOUS", closest_sensor, min_dist
        else:
            return "CLEAR", None, min_dist

    def compute_avoidance_vector(self, readings):
        """Compute a avoidance velocity vector."""
        vector = np.array([0.0, 0.0])

        for direction, distance in readings.items():
            if distance < self.cautious_distance:
                # Compute repulsive force (inverse of distance)
                force_magnitude = 1.0 / max(distance, 0.1)

                if direction == 'front':
                    vector += np.array([0, -force_magnitude])
                elif direction == 'rear':
                    vector += np.array([0, force_magnitude])
                elif direction == 'left':
                    vector += np.array([-force_magnitude, 0])
                elif direction == 'right':
                    vector += np.array([force_magnitude, 0])

        # Normalize
        magnitude = np.linalg.norm(vector)
        if magnitude > 0:
            vector = vector / magnitude * min(magnitude, 1.0)

        return vector

# Demo
avoidance = ObstacleAvoidance()
readings = {
    'front': 0.8,
    'left': 2.5,
    'right': 1.5,
    'rear': 3.0,
    'bottom': 1.2
}

action, closest, dist = avoidance.analyze_readings(readings)
vector = avoidance.compute_avoidance_vector(readings)

print(f"Action: {action}")
print(f"Closest obstacle: {closest} at {dist:.2f}m")
print(f"Avoidance vector: ({vector[0]:.2f}, {vector[1]:.2f})")

from collections import deque

class UltrasonicMapper:
    """Create a 2D obstacle map from ultrasonic readings."""

    def __init__(self, grid_size=100, resolution=0.1):
        self.grid_size = grid_size
        self.resolution = resolution  # meters per cell
        self.occupancy_grid = np.full((grid_size, grid_size), 0.5)  # 0.5 = unknown

    def ray_cast(self, start_pos, angle, distance, confidence=1.0):
        """Update grid along a single ray."""
        x, y = start_pos
        end_x = x + distance * np.cos(angle)
        end_y = y + distance * np.sin(angle)

        # Convert to grid coordinates
        gx_start = int(x / self.resolution) + self.grid_size // 2
        gy_start = int(y / self.resolution) + self.grid_size // 2
        gx_end = int(end_x / self.resolution) + self.grid_size // 2
        gy_end = int(end_y / self.resolution) + self.grid_size // 2

        # Bresenham's line algorithm (simplified)
        steps = max(abs(gx_end - gx_start), abs(gy_end - gy_start), 1)
        for i in range(steps):
            t = i / steps
            gx = int(gx_start + (gx_end - gx_start) * t)
            gy = int(gy_start + (gy_end - gy_start) * t)

            if 0 <= gx < self.grid_size and 0 <= gy < self.grid_size:
                if i < steps - 1:
                    # Free space
                    self.occupancy_grid[gy, gx] = max(0.1, self.occupancy_grid[gy, gx] - 0.05)
                else:
                    # Occupied
                    self.occupancy_grid[gy, gx] = min(0.9, self.occupancy_grid[gy, gx] + 0.3 * confidence)

    def update_from_sensors(self, position, heading, readings):
        """Update map from multiple sensor readings."""
        directions = {
            'front': heading,
            'left': heading + np.pi/2,
            'right': heading - np.pi/2,
            'rear': heading + np.pi,
        }

        for direction_name, distance in readings.items():
            if direction_name in directions and distance > 0:
                angle = directions[direction_name]
                confidence = max(0.3, 1.0 - distance / 5.0)
                self.ray_cast(position, angle, distance, confidence)

    def get_obstacles(self, threshold=0.7):
        """Extract obstacle positions from grid."""
        obstacles = np.where(self.occupancy_grid > threshold)
        return list(zip(obstacles[1], obstacles[0]))

# Usage
mapper = UltrasonicMapper(grid_size=100, resolution=0.1)

# Simulate drone flying and scanning
position = (5.0, 5.0)
heading = 0.0
readings = {'front': 1.5, 'left': 3.0, 'right': 2.0, 'rear': 4.0}
mapper.update_from_sensors(position, heading, readings)

obstacles = mapper.get_obstacles()
print(f"Mapped {len(obstacles)} obstacle cells")