Calibration for ADAS Systems

import cv2, numpy as np, canape_com as canape

# Camera extrinsic calibration using checkerboard target
# Board: 9x6 inner corners, 100mm squares

BOARD_SIZE = (9, 6)
SQUARE_SIZE_MM = 100.0

# 3D coordinates of checkerboard corners in world frame
obj_pts = np.zeros((BOARD_SIZE[0]*BOARD_SIZE[1], 3), np.float32)
obj_pts[:, :2] = np.mgrid[0:BOARD_SIZE[0], 0:BOARD_SIZE[1]].T.reshape(-1, 2)
obj_pts *= SQUARE_SIZE_MM

# Capture images at 5 board positions (known distances: 5m, 10m, 15m, 20m, 25m)
all_obj_pts, all_img_pts = [], []
for i, distance_m in enumerate([5.0, 10.0, 15.0, 20.0, 25.0]):
    # Retrieve image from ADAS ECU via XCP (MEASUREMENT: camera_raw_image_ptr)
    img = capture_image_from_adas_ecu()
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, BOARD_SIZE)
    if ret:
        all_obj_pts.append(obj_pts)
        all_img_pts.append(corners)

# Load camera intrinsic matrix (factory-calibrated)
K = np.load("camera_intrinsic_K.npy")
dist_coeffs = np.load("camera_dist_coeffs.npy")

# Solve for extrinsic rotation + translation
_, rvec, tvec = cv2.solvePnP(
    np.vstack(all_obj_pts), np.vstack(all_img_pts), K, dist_coeffs)

R, _ = cv2.Rodrigues(rvec)
print(f"Rotation matrix R:\n{R}")
print(f"Translation vector t: {tvec.T} mm")

# Validate: compute RMS reprojection error
projected, _ = cv2.projectPoints(np.vstack(all_obj_pts), rvec, tvec, K, dist_coeffs)
rms_error = np.sqrt(np.mean((np.vstack(all_img_pts) - projected)**2))
print(f"RMS reprojection error: {rms_error:.3f} px", "PASS" if rms_error < 0.5 else "FAIL")

# Write extrinsic parameters to ADAS ECU via XCP
device = canape.OpenDevice("ECU_ADAS")
for i in range(3):
    for j in range(3):
        device.SetValue(f"camera_R_{i}{j}", float(R[i,j]))
for i in range(3):
    device.SetValue(f"camera_t_{i}", float(tvec[i,0]))
device.CopyCalPage(src=0, dst=1)

# AEB threshold calibration with ISO 22737 validation
import canape_com as canape, time

device = canape.OpenDevice("ECU_ADAS")
device.Connect()

results = []
for closing_speed_kmh in [20.0, 30.0, 40.0]:
    # Set scenario parameters in dyno controller
    setup_aeb_scenario(closing_speed_kmh)

    # Sweep TTC_brake_threshold
    for ttc in [0.7, 0.8, 0.9, 1.0, 1.1]:
        device.SetValue("TTC_brake_threshold", ttc)
        time.sleep(1.0)

        # Run scenario, measure actual intervention TTC
        run_aeb_scenario()
        actual_ttc = device.GetMeasurement("aeb_intervention_TTC_s")
        print(f"Speed={closing_speed_kmh} km/h, "
              f"threshold={ttc:.1f}s → actual TTC={actual_ttc:.2f}s")
        results.append((closing_speed_kmh, ttc, actual_ttc))

# Select threshold meeting ISO 22737 at all speeds
optimal_ttc = max(ttc for speed, ttc, actual in results
                  if actual >= 0.9 and actual <= 1.4)
device.SetValue("TTC_brake_threshold", optimal_ttc)
device.CopyCalPage(src=0, dst=1)
print(f"Final TTC_brake_threshold = {optimal_ttc:.1f}s")

# Radar bore-sight calibration: align antenna to vehicle longitudinal axis
import canape_com as canape, time

device = canape.OpenDevice("ECU_ADAS")
device.Connect()

# Position corner reflector at 10m, 0° azimuth (directly ahead)
print("Place corner reflector at 10m straight ahead, on flat surface")
input("Press Enter when ready...")

# Read raw azimuth offset from radar ECU
# azimuth_raw = measured angle to reflector before calibration correction
time.sleep(2.0)  # stabilise
readings = [device.GetMeasurement("radar_azimuth_raw_deg") for _ in range(10)]
raw_azimuth = sum(readings) / len(readings)  # average 10 samples
print(f"Raw azimuth to straight-ahead target: {raw_azimuth:.3f} deg")

# The raw offset IS the bore-sight correction to apply
# Write negated value as calibration offset
cal_offset = -raw_azimuth
device.SetValue("radar_azimuth_cal_offset_deg", cal_offset)
device.CopyCalPage(src=0, dst=1)

# Validate: corrected azimuth should be 0 ± 0.1°
time.sleep(1.0)
readings_after = [device.GetMeasurement("radar_azimuth_corrected_deg") for _ in range(10)]
corrected = sum(readings_after) / len(readings_after)
print(f"Corrected azimuth: {corrected:.3f} deg", "PASS" if abs(corrected) < 0.1 else "FAIL")