/// <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)));
}
