嵌入式软件自动化测试（第四章）笔记

import math
import matplotlib.pyplot as plt
import numpy as np
# 修复中文字体问题
plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'DejaVu Sans']  # 设置中文字体
plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题
def normalize_coordinate(lon, lat):
    """确保坐标在有效范围内"""
    # 经度归一化到[-180, 180)
    lon = ((lon + 180) % 360) - 180
    # 纬度限制在[-90, 90]
    lat = max(min(lat, 90), -90)
    return lon, lat
def calculate_bearing(lon1, lat1, lon2, lat2):
    """
    计算从点1到点2的方位角（度，相对于正北顺时针0-360）
    修正版：正确处理极点情况
    """
    # 转换为弧度
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    lambda1 = math.radians(lon1)
    lambda2 = math.radians(lon2)
    # 计算差值
    delta_lambda = lambda2 - lambda1
    # 处理北极点特殊情况
    if math.cos(phi1) < 1e-12:  # 在北极点附近
        if lat1 > 0:
            return 180  # 从北极出发，任何方向都是南
    elif math.cos(phi2) < 1e-12:  # 在南极点附近
        if lat2 < 0:
            return 0    # 向南极点前进，任何方向都是北
    # 计算方位角
    y = math.sin(delta_lambda) * math.cos(phi2)
    x = math.cos(phi1) * math.sin(phi2) - math.sin(phi1) * math.cos(phi2) * math.cos(delta_lambda)
    # 防止除以零
    if abs(x) < 1e-12 and abs(y) < 1e-12:
        return 0
    bearing = math.degrees(math.atan2(y, x))
    # 转换为0-360度（正北为0度，顺时针增加）
    bearing = (bearing + 360) % 360
    return bearing
def determine_action(P1, P2, D1, D2, current_direction):
    """
    判断应该执行的动作
    参数:
    - current_direction: 当前朝向与正北的夹角，顺时针为正，范围[-180, 180]
                        0度=正北，90度=正东，-90度=正西，±180度=正南
    返回: (action, details)
    """
    # 1. 计算到目标点的方位角
    target_bearing = calculate_bearing(P1, P2, D1, D2)
    # 2. 将当前朝向归一化到[0, 360)度以便计算
    current_dir_360 = current_direction % 360
    # 3. 计算当前朝向与目标方位的角度差（最短路径）
    angle_diff = target_bearing - current_dir_360
    # 将角度差归一化到[-180, 180]
    if angle_diff > 180:
        angle_diff -= 360
    elif angle_diff < -180:
        angle_diff += 360
    # 4. 判断动作（优化阈值逻辑）
    forward_threshold = 30    # 角度差小于30度可前进
    turn_threshold = 120      # 大角度转弯阈值
    action = ""
    if abs(angle_diff) <= forward_threshold:
        action = "前进"
    elif abs(angle_diff) >= 150:
        # 接近相反方向，判断是否掉头更优
        # 考虑距离因素：近距离掉头，远距离后退
        distance = haversine_distance(P1, P2, D1, D2)
        if distance < 10:  # 小于10公里考虑掉头
            action = "掉头"
        else:
            action = "后退"
    elif angle_diff > 0:
        # 目标在右侧
        if abs(angle_diff) > turn_threshold:
            action = "大角度右转"
        else:
            action = "右转"
    else:
        # 目标在左侧
        if abs(angle_diff) > turn_threshold:
            action = "大角度左转"
        else:
            action = "左转"
    # 计算距离
    distance = haversine_distance(P1, P2, D1, D2)
    details = {
        'target_bearing': round(target_bearing, 2),
        'current_direction': round(current_direction, 2),
        'angle_difference': round(angle_diff, 2),
        'distance_km': round(distance, 2),
        'action_reason': f"当前朝向{current_direction:.1f}°，目标方向{target_bearing:.1f}°，角度差{angle_diff:.1f}°"
    }
    return action, details
def haversine_distance(lon1, lat1, lon2, lat2, R=6371.0):
    """计算两点间距离（公里）"""
    phi1 = math.radians(lat1)
    phi2 = math.radians(lat2)
    delta_phi = math.radians(lat2 - lat1)
    delta_lambda = math.radians(lon2 - lon1)
    a = math.sin(delta_phi/2)**2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda/2)**2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
    return R * c
def plot_navigation(P1, P2, D1, D2, current_direction):
    """可视化导航情况"""
    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 6))
    # 左图：地图视图
    ax1.scatter([P1, D1], [P2, D2], c=['red', 'blue'], s=100, zorder=5, 
                label=[f'当前位置\n({P1:.1f}, {P2:.1f})', f'目标位置\n({D1:.1f}, {D2:.1f})'])
    # 绘制当前朝向箭头
    arrow_length = max(abs(D1-P1), abs(D2-P2)) * 0.2
    if arrow_length == 0:
        arrow_length = 2
    dx = arrow_length * math.sin(math.radians(current_direction))
    dy = arrow_length * math.cos(math.radians(current_direction))
    ax1.arrow(P1, P2, dx, dy, head_width=arrow_length*0.1, head_length=arrow_length*0.15, 
              fc='green', ec='green', label='当前朝向')
    # 绘制目标方向线
    target_bearing = calculate_bearing(P1, P2, D1, D2)
    distance = haversine_distance(P1, P2, D1, D2)
    line_length = min(arrow_length * 1.5, distance * 0.2)
    dx_target = line_length * math.sin(math.radians(target_bearing))
    dy_target = line_length * math.cos(math.radians(target_bearing))
    ax1.plot([P1, P1 + dx_target], [P2, P2 + dy_target], 'b--', alpha=0.7, linewidth=2, label='目标方向')
    ax1.set_xlabel('经度 (Longitude)')
    ax1.set_ylabel('纬度 (Latitude)')
    # 判断动作用于标题
    action, details = determine_action(P1, P2, D1, D2, current_direction)
    ax1.set_title(f'导航地图 - 建议动作: {action}\n距离: {details["distance_km"]} km, 角度差: {details["angle_difference"]:.1f}°')
    ax1.grid(True, alpha=0.3)
    ax1.axhline(y=0, color='k', linestyle='-', alpha=0.2, label='赤道')
    ax1.axvline(x=0, color='k', linestyle='-', alpha=0.2, label='本初子午线')
    ax1.legend(loc='best')
    # 调整坐标轴范围，确保两点都可见
    ax1.set_xlim(min(P1, D1, -180, 0) - 10, max(P1, D1, 180, 0) + 10)
    ax1.set_ylim(min(P2, D2, -90, 0) - 10, max(P2, D2, 90, 0) + 10)
    # 右图：方向指示器
    ax2 = plt.subplot(1, 2, 2, polar=True)
    # 绘制罗盘
    ax2.set_theta_zero_location('N')  # 0度在顶部（北）
    ax2.set_theta_direction(-1)       # 顺时针增加
    ax2.set_ylim(0, 1.2)
    # 绘制当前朝向
    current_rad = math.radians(current_direction % 360)
    ax2.plot([current_rad, current_rad], [0, 0.8], 'g-', linewidth=3, label='当前朝向')
    # 绘制目标方向
    target_rad = math.radians(target_bearing)
    ax2.plot([target_rad, target_rad], [0, 1.0], 'b-', linewidth=3, label='目标方向')
    # 绘制角度差圆弧
    theta = np.linspace(current_rad, target_rad, 100)
    r = 0.4 * np.ones_like(theta)
    ax2.plot(theta, r, 'orange', linewidth=2, label='转向角度')
    # 添加角度差标注
    mid_angle = (current_rad + target_rad) / 2
    angle_diff = details['angle_difference']
    ax2.text(mid_angle, 0.5, f'{abs(angle_diff):.1f}°', 
             horizontalalignment='center', verticalalignment='center',
             fontsize=12, fontweight='bold', color='red')
    # 添加罗盘方向标注
    directions = ['N', 'E', 'S', 'W']
    angles = [0, 90, 180, 270]
    for direction, angle in zip(directions, angles):
        rad = math.radians(angle)
        ax2.text(rad, 1.3, direction, 
                horizontalalignment='center', verticalalignment='center',
                fontsize=14, fontweight='bold')
    ax2.set_title('方向指示器', fontsize=14, fontweight='bold')
    ax2.legend(loc='upper right', bbox_to_anchor=(1.3, 1.0))
    plt.tight_layout()
    plt.show()
def dms_to_decimal(coord_str):
    """
    将格式为 '59N' 或 '18E' 的坐标字符串转换为十进制数值
    参数:
        coord_str: 坐标字符串，如 '59N', '18E', '30S', '120W'
    返回:
        十进制数值，如 59.0, 18.0, -30.0, -120.0
    """
    # 去除空格
    coord_str = coord_str.strip().upper()
    # 分离数值和方向
    # 从字符串末尾开始找第一个非数字字符
    i = len(coord_str) - 1
    while i >= 0 and not coord_str[i].isdigit():
        i -= 1
    if i < 0:
        raise ValueError(f"无效的坐标格式: {coord_str}")
    value_part = coord_str[:i+1]
    direction = coord_str[i+1:]
    # 转换数值部分
    try:
        value = float(value_part)
    except ValueError:
        raise ValueError(f"坐标数值部分无效: {value_part}")
    # 根据方向确定正负
    if direction in ['N', 'E']:
        return value
    elif direction in ['S', 'W']:
        return -value
    else:
        raise ValueError(f"无效的方向指示符: {direction}")
def convert_coordinate(lat_str, lon_str):
    """
    将格式为 (纬度, 经度) 的字符串对转换为十进制数值
    参数:
        lat_str: 纬度字符串，如 '59N', '30S'
        lon_str: 经度字符串，如 '18E', '120W'
    返回:
        (latitude, longitude) 元组，数值范围：
        纬度: [-90, 90]
        经度: [-180, 180]
    """
    latitude = dms_to_decimal(lat_str)
    longitude = dms_to_decimal(lon_str)
    # 验证范围
    if not (-90 <= latitude <= 90):
        raise ValueError(f"纬度超出范围 [-90, 90]: {latitude}")
    if not (-180 <= longitude <= 180):
        raise ValueError(f"经度超出范围 [-180, 180]: {longitude}")
    return (latitude, longitude)
def parse_coordinate_string(coord_string):
    """
    解析格式为 '(59N, 18E)' 的完整坐标字符串
    参数:
        coord_string: 坐标字符串，如 '(59N, 18E)', '59N, 18E'
    返回:
        (latitude, longitude) 元组
    """
    # 去除括号和空格
    coord_string = coord_string.strip().strip('()')
    # 分割纬度和经度部分
    parts = [p.strip() for p in coord_string.split(',')]
    if len(parts) != 2:
        raise ValueError(f"坐标字符串格式错误，应为 '纬度, 经度': {coord_string}")
    return convert_coordinate(parts[0], parts[1])
# 测试示例
# 上海到斯德哥尔摩导航计算
if __name__ == "__main__":
    print("=" * 60)
    print("上海 → 斯德哥尔摩 导航计算")
    print("当前朝向: 西北(-45°)")
    print("=" * 60)
    # 定义坐标 (经度, 纬度)
    # 上海浦东: 东经121.4222°, 北纬31.0971°
    P1, P2 = 121.4222, 31.0971
    # 斯德哥尔摩: 东经18.0686°, 北纬59.3293°
    D1, D2 = 18.0686, 59.3293
    # 当前朝向: 西北方向
    current_direction = -45  # -45° = 西北方向
    print(f"\n📌 起点: 上海浦东")
    print(f"   坐标: 经度 {P1}°, 纬度 {P2}°")
    print(f"   位置: 浦东新区东部，迪士尼乐园附近")
    print(f"\n🎯 终点: 瑞典斯德哥尔摩")
    print(f"   坐标: 经度 {D1}°, 纬度 {D2}°")
    print(f"   位置: 老城区中心，王宫附近")
    print(f"\n🧭 当前朝向: {current_direction}°")
    print(f"   方向描述: 西北方向")
    print(f"   罗盘方位: {current_direction % 360}°")
    # 计算导航动作
    action, details = determine_action(P1, P2, D1, D2, current_direction)
    print(f"\n🚀 导航决策结果:")
    print(f"   建议动作: {action}")
    print(f"\n📊 详细数据:")
    print(f"   目标方位角: {details['target_bearing']}°")
    print(f"   当前朝向: {details['current_direction']}°")
    print(f"   角度差: {details['angle_difference']}°")
    print(f"   直线距离: {details['distance_km']} km")
    print(f"   飞行距离: 约{details['distance_km']*0.9:.0f} km (考虑地球曲率)")
    print(f"   推理: {details['action_reason']}")
    # 计算飞行时间估计
    flight_speed_kmh = 850  # 客机巡航速度
    flight_time_hours = details['distance_km'] / flight_speed_kmh
    flight_hours = int(flight_time_hours)
    flight_minutes = int((flight_time_hours - flight_hours) * 60)
    print(f"\n⏱️  预计飞行时间:")
    print(f"   以{flight_speed_kmh}km/h巡航: {flight_hours}小时{flight_minutes}分钟")
    # 解释角度差含义
    angle_diff = details['angle_difference']
    print(f"\n🔍 角度差解读:")
    if angle_diff > 0:
        print(f"   目标在您当前方向的右侧，需要向右调整 {abs(angle_diff):.1f}°")
    else:
        print(f"   目标在您当前方向的左侧，需要向左调整 {abs(angle_diff):.1f}°")
    # 地球大圆航线分析
    print(f"\n🌍 大圆航线分析:")
    print(f"   这是跨越欧亚大陆的长距离航线")
    print(f"   主要经过: 中国→蒙古→俄罗斯→芬兰→瑞典")
    print(f"   纬度变化: 从北纬31°到北纬59° (向北方飞行)")
    print(f"   经度变化: 从东经121°到东经18° (大幅向西飞行)")
    # 可视化导航
    print(f"\n📈 正在生成导航可视化图表...")
    plot_navigation(P1, P2, D1, D2, current_direction)