/// <summary> /// Detect the Parking Space Edge /// </summary> /// <param name="m_313Data"></param> /// <param name="vehicle_center"></param> /// <param name="detection_state"></param> /// <param name="OutEdge"></param> /// <param name="InnerEdge"></param> public void ParkingEdgeDetection(LIN_STP313_ReadData[] m_313Data, LocationPoint vehicle_center, ref ParkingEdgeAxis [] parking_pack) { UltrasonicAxis [] current_data_point_vehicle = new UltrasonicAxis[2]; UltrasonicAxis[] current_data_point_ground = new UltrasonicAxis[2]; for (int i = 0; i < 2; i++) { if (m_313Data[i].TOF1 != 0) { STP313_Process(m_313Data[i], UltrasonicVehicleLocation[8 + i], ref current_data_point_vehicle[i]); UltrasonicDataGroundLocationSingle(vehicle_center, current_data_point_vehicle[i], ref current_data_point_ground[i]); if (last_valid_data[i].TOF1 != 0) { if ((m_313Data[i].TOF1 - last_valid_data[i].TOF1) > adjacent_tof_threshold)//up { parking_pack[i].state = -1; parking_pack[i].OutEdge = current_data_point_ground[i]; parking_pack[i].InnerEdge = last_valid_point[i]; } else if ((m_313Data[i].TOF1 - last_valid_data[i].TOF1) < -adjacent_tof_threshold)//down { parking_pack[i].state = 1; parking_pack[i].OutEdge = last_valid_point[i]; parking_pack[i].InnerEdge = current_data_point_ground[i]; } else { parking_pack[i].state = 0; parking_pack[i].OutEdge = current_data_point_ground[i]; } } last_valid_data[i] = m_313Data[i]; last_valid_point[i] = current_data_point_ground[i]; } } }
/// <summary> /// the vehicle coordinate translate to the ground coordinate with one data /// </summary> /// <param name="vehicle_center_point"></param> /// <param name="ultrasonic_vehicle_location"></param> /// <param name="ultrasonic_ground_location"></param> public void UltrasonicDataGroundLocationSingle(LocationPoint vehicle_center_point, UltrasonicAxis ultrasonic_vehicle_location, ref UltrasonicAxis ultrasonic_ground_location) { ultrasonic_ground_location.x = vehicle_center_point.x + ultrasonic_vehicle_location.x * Math.Cos(vehicle_center_point.yaw) - ultrasonic_vehicle_location.y * Math.Sin(vehicle_center_point.yaw); ultrasonic_ground_location.y = vehicle_center_point.y + ultrasonic_vehicle_location.x * Math.Sin(vehicle_center_point.yaw) + ultrasonic_vehicle_location.y * Math.Cos(vehicle_center_point.yaw); ultrasonic_ground_location.state = ultrasonic_vehicle_location.state; }
/// <summary> /// 目标位置坐标点计算(三角定位) /// </summary> /// <param name="base_position">基准点坐标点</param> /// <param name="base_angle">基准角度(弧度)</param> /// <param name="base_L">基准长度(cm)</param> /// <param name="distance1">临边长度(cm)</param> /// <param name="distance2">对角长度(cm)</param> /// <param name="TargetPosition">输出最终的目标位置坐标</param> private void TargetPositionCalculate(LocationPoint base_position, double base_angle, double base_L, Distance distance1, Distance distance2, ref UltrasonicAxis TargetPosition, bool check_direction) { if (distance1.state == UltrasonicStatus.BlindZone || distance2.state == UltrasonicStatus.BlindZone) { TargetPosition.x = 0; TargetPosition.y = 0; TargetPosition.state = UltrasonicStatus.BlindZone; } else { if (distance1.state == UltrasonicStatus.Normal && distance2.state == UltrasonicStatus.Normal) { double beta = Math.Acos((Math.Pow(base_L, 2) + Math.Pow(distance1.distance, 2) - Math.Pow(distance2.distance, 2)) / (2 * base_L * distance1.distance)); double gama = check_direction ? (base_angle + beta) : (base_angle - beta); TargetPosition.x = base_position.x + distance1.distance * Math.Cos(gama); TargetPosition.y = base_position.y + distance1.distance * Math.Sin(gama); TargetPosition.state = UltrasonicStatus.Normal; } else { TargetPosition.x = 0; TargetPosition.y = 0; TargetPosition.state = UltrasonicStatus.OverDetection; } } TargetPosition.x *= 0.01; TargetPosition.y *= 0.01; }
private void STP318_Process(LIN_STP318_ReadData m_318Data, LocationPoint InstallationPosition, ref UltrasonicAxis ultrasonic_location) { if (m_318Data.status == 0) { if (m_318Data.TOF != 0) { double distance = m_318Data.TOF * TemperatureCompensation(25);//cm ultrasonic_location.x = InstallationPosition.x + distance * Math.Cos(InstallationPosition.yaw); ultrasonic_location.y = InstallationPosition.y + distance * Math.Sin(InstallationPosition.yaw); ultrasonic_location.state = UltrasonicStatus.Normal; } else { ultrasonic_location.x = 0; ultrasonic_location.y = 0; ultrasonic_location.state = UltrasonicStatus.OverDetection; } } else if (m_318Data.status == 16) { ultrasonic_location.x = 0; ultrasonic_location.y = 0; ultrasonic_location.state = UltrasonicStatus.BlindZone; } ultrasonic_location.x *= 0.01; ultrasonic_location.y *= 0.01; }
public bool CheckVectorAngleSymbol(LocationPoint vehicle1, LocationPoint vehicle2, UltrasonicAxis ultrasonic) { LocationPoint vector1, vector2; vector1.x = ultrasonic.x - vehicle1.x; vector1.y = ultrasonic.y - vehicle1.y; vector2.x = vehicle2.x - vehicle1.x; vector2.y = vehicle2.y - vehicle1.y; double vector_s1 = vector1.x * vector2.x + vector1.y * vector2.y; vector1.x = ultrasonic.x - vehicle2.x; vector1.y = ultrasonic.y - vehicle2.y; vector2.x = vehicle1.x - vehicle2.x; vector2.y = vehicle1.y - vehicle2.y; double vector_s2 = vector1.x * vector2.x + vector1.y * vector2.y; return((vector_s1 >= 0 && vector_s2 >= 0) ? true : false); }
public double CalculateSlope(UltrasonicAxis ultrasonic1, UltrasonicAxis ultrasonic2) { return(Math.Atan2(ultrasonic2.y - ultrasonic1.y, ultrasonic2.x - ultrasonic1.x)); }
public double CalculateUltrasonicDistance(UltrasonicAxis ultrasonic1, UltrasonicAxis ultrasonic2) { return(Math.Sqrt(Math.Pow(ultrasonic1.x - ultrasonic2.x, 2) + Math.Pow(ultrasonic1.y - ultrasonic2.y, 2))); }
public double CalculateVehicleUltrasonicDistance(LocationPoint vehicle, UltrasonicAxis ultrasonic) { return(Math.Sqrt(Math.Pow(vehicle.x - ultrasonic.x, 2) + Math.Pow(vehicle.y - ultrasonic.y, 2))); }