automotive-adas

Automotive Adas

Adas Features Implementation

ADAS Features Implementation

Overview

Concrete implementations of ADAS features: Adaptive Cruise Control (ACC), Lane Keep Assist (LKA), Automatic Emergency Braking (AEB), Blind Spot Detection (BSD), Park Assist, and Traffic Sign Recognition (TSR). Production-ready code for L0-L2+ systems.

Adaptive Cruise Control (ACC)

Full ACC Implementation

#include <Eigen/Dense>
#include <algorithm>
#include <cmath>

class AdaptiveCruiseControl {
public:
    struct ACCParams {
        double time_gap = 2.0;              // seconds (ISO 22179)
        double min_distance = 5.0;          // meters
        double max_acceleration = 2.0;      // m/s²
        double max_deceleration = -3.0;     // m/s²
        double comfort_decel = -2.0;        // m/s²
        double set_speed = 30.0;            // m/s (108 km/h)
        double speed_tolerance = 2.0;       // m/s
    };

    enum class ACCMode {
        OFF,
        STANDBY,
        ACTIVE_CRUISE,
        ACTIVE_FOLLOWING,
        EMERGENCY_BRAKE
    };

    AdaptiveCruiseControl(const ACCParams& params) : params_(params), mode_(ACCMode::STANDBY) {}

    struct ACCOutput {
        double acceleration;        // Commanded acceleration (m/s²)
        ACCMode mode;
        double target_speed;
        double target_distance;
        bool warning_issued;
    };

    ACCOutput compute(double ego_velocity, double ego_acceleration,
                     const std::vector<DetectedObject>& objects) {
        ACCOutput output;
        output.mode = mode_;
        output.warning_issued = false;

        // Find lead vehicle
        auto lead_vehicle = find_lead_vehicle(objects, ego_velocity);

        if (!lead_vehicle.has_value()) {
            // No lead vehicle - cruise control mode
            output.acceleration = cruise_control(ego_velocity);
            output.target_speed = params_.set_speed;
            output.target_distance = 0.0;
            mode_ = ACCMode::ACTIVE_CRUISE;
        } else {
            // Following mode
            double relative_velocity = ego_velocity - lead_vehicle->velocity;
            double distance = lead_vehicle->distance;
            double desired_distance = calculate_desired_distance(ego_velocity);

            // Calculate acceleration using Intelligent Driver Model (IDM)
            output.acceleration = intelligent_driver_model(
                ego_velocity, distance, relative_velocity, desired_distance
            );

            output.target_speed = lead_vehicle->velocity;
            output.target_distance = desired_distance;

            // Check for emergency
            double ttc = time_to_collision(distance, relative_velocity);
            if (ttc > 0 && ttc < 2.0 && relative_velocity > 0) {
                output.acceleration = params_.max_deceleration;
                output.warning_issued = true;
                mode_ = ACCMode::EMERGENCY_BRAKE;
            } else {
                mode_ = ACCMode::ACTIVE_FOLLOWING;
            }
        }

        // Clamp acceleration
        output.acceleration = std::clamp(output.acceleration,
                                        params_.max_deceleration,
                                        params_.max_acceleration);

        return output;
    }

private:
    ACCParams params_;
    ACCMode mode_;

    struct DetectedObject {
        double distance;   // meters (longitudinal)
        double velocity;   // m/s
        double lateral_offset;  // meters
        std::string object_class;
    };

    std::optional<DetectedObject> find_lead_vehicle(
        const std::vector<DetectedObject>& objects, double ego_velocity)
    {
        std::optional<DetectedObject> lead;
        double min_distance = std::numeric_limits<double>::max();

        for (const auto& obj : objects) {
            // Filter: only consider vehicles in same lane
            if (std::abs(obj.lateral_offset) > 1.5) continue;

            // Filter: only vehicles ahead
            if (obj.distance < 0) continue;

            // Find closest
            if (obj.distance < min_distance) {
                min_distance = obj.distance;
                lead = obj;
            }
        }

        return lead;
    }

    double cruise_control(double ego_velocity) {
        // Simple P controller to reach set speed
        double error = params_.set_speed - ego_velocity;
        double kp = 0.5;
        return std::clamp(kp * error, params_.max_deceleration, params_.max_acceleration);
    }

    double calculate_desired_distance(double ego_velocity) {
        // Time gap policy: d = d_min + v * T
        return params_.min_distance + ego_velocity * params_.time_gap;
    }

    double intelligent_driver_model(double velocity, double distance,
                                    double relative_velocity, double desired_distance) {
        // IDM parameters
        const double a_max = params_.max_acceleration;
        const double b_comfortable = -params_.comfort_decel;
        const double delta = 4.0;  // Acceleration exponent

        // Desired dynamical distance
        double v_approach_term = velocity * relative_velocity / (2 * std::sqrt(a_max * b_comfortable));
        double s_star = params_.min_distance + std::max(0.0, velocity * params_.time_gap + v_approach_term);

        // IDM acceleration
        double accel = a_max * (1.0 - std::pow(velocity / params_.set_speed, delta) -
                               std::pow(s_star / distance, 2.0));

        return accel;
    }

    double time_to_collision(double distance, double relative_velocity) {
        if (relative_velocity <= 0) return -1.0;  // No collision
        return distance / relative_velocity;
    }
};