using ProjectBase.Data.BaseDAL;
using ProjectBase.Data.Logs;
using ProjectBase.Data.Redis;
using ProjectBase.Util;
using SIASUN.Autopilot.GNSS;
using SIASUN.Autopilot.Model;
using System;
using System.Collections.Generic;
using System.Threading;
using System.Threading.Tasks;
namespace SIASUN.Autopilot.BLL
{
public class BlDecisionControler
{
///
/// 参考路径起始点
///
private WGS84Points startPointRef = new WGS84Points();
///
/// 当前路径起始点
///
private WGS84Points startPointCur = new WGS84Points();
///
/// 当前点
///
private WGS84Points pointCur = new WGS84Points();
// 当前速度
private double SpeedCur;
///
/// 地球的半径
///
private const double EARTH_RADIUS = 6378.137;
RedisHelper redis = new RedisHelper(0);
List locationList = new List();
private string PointsToOrigin = "pointstoorigin";
///
/// 角度误差
///
private const double angleError = 0.102;
///
/// 速度误差
///
private const double speedError = 0.103;
///
/// 参考距离--用于动作指令判断
///
private double distanceRefAction = 0;
///
/// 参考角度--用于动作指令判断
///
private double angleRefAction = 0;
///
/// 参考速度--用于动作指令判断
///
private double speedRefAction = 0;
///
/// 执行到第几个动作指令
///
private int count = 1;
private bool bLock = true;
//public event EventStartPointHandler StartPointHandler;
public event EventHandler StartPointHandler;
public BlDecisionControler()
{
InitPlanRoute();
BlGNSSPerception.Instance.GNSSHandler += Instance_GNSSHandler;
}
private void Instance_GNSSHandler(object sender, GNSSInfoEventArgs e)
{
LogHelper.log.Debug("控制指令算法执行");
string msg = "";
string start = redis.GetString(SysEnvironment.Start);
if (start.Equals("1"))
{
if (Math.Abs(((double)e.Nmea.gpgga.Latitude - startPointRef.y)) > 0.0000001)// 验证起始点是否在10cm范围内
{
LogHelper.log.Debug("通过纬度验证起始点不在范围内");
return;
}
if (Math.Abs(((double)e.Nmea.gpgga.Longitude - startPointRef.x)) > 0.0000001)
{
LogHelper.log.Debug("通过经度验证起始点不在范围内");
return;
}
msg = "车辆运行到起始点。请开始测试!";
LogHelper.log.Debug(msg);
StartPointEventArgs message = new StartPointEventArgs(msg);
if (StartPointHandler != null)
{
StartPointHandler(sender,message);
}
startPointCur.x = (double)e.Nmea.gpgga.Longitude;
startPointCur.y = (double)e.Nmea.gpgga.Latitude;
redis.SetString(SysEnvironment.Start, "0");
}
pointCur.x = (double)e.Nmea.gpgga.Longitude;
pointCur.y = (double)e.Nmea.gpgga.Latitude;
SpeedCur = (double)e.Nmea.gprmc.Speed;
UpdateDirective();
if (bLock)
{
bLock = false;
Task task = new Task(() =>
{
ExcutePid();
bLock = true;
});
task.Start();
}
}
///
/// 获取规划路径的各个点到起始点的距离
///
private double GetDistance(WGS84Points startPoint, WGS84Points endPoint)
{
double lat = startPoint.y - endPoint.y;
double lon = startPoint.x - endPoint.x;
double dis = 2 * Math.Asin(Math.Sqrt(Math.Pow(Math.Sin(lat / 2), 2) +
Math.Cos(startPoint.y) * Math.Cos(endPoint.y) * Math.Pow(Math.Sin(lon / 2), 2)));
dis = dis * EARTH_RADIUS;
dis = Math.Round(dis * 1e4) / 1e4;
return dis;
}
///
/// 求出任何一点到起始点的角度的tan值
///
///
///
///
private double GetTanAngle(WGS84Points startPoint, WGS84Points endPoint)
{
WGS84Points endPointX = new WGS84Points();
endPointX.x = endPoint.x;
endPointX.y = startPoint.y;
double distanceX = GetDistance(startPoint, endPointX);
WGS84Points endPointY = new WGS84Points();
endPointY.x = startPoint.x;
endPointY.y = endPoint.y;
double distanceY = GetDistance(startPoint, endPointY);
return distanceY / distanceX;
}
///
/// 计算点到线段的距离
///
/// 点的坐标
/// 线段起始点坐标
/// 线段终止点坐标
///
private double GetPointLineDistance(WGS84Points point,WGS84Points startPoint, WGS84Points endPoint)
{
double a = GetDistance(startPoint,endPoint);
double b = GetDistance(endPoint,point);
double c = GetDistance(startPoint,point);
if (b*b >= c*c + a*a)
{
return c;
}
if (c*c >= b*b + a*a)
{
return b;
}
double p = (a + b + c) / 2; //周长的一半
double s = Math.Sqrt(p * (p - a) * (p - b) * (p - c)); //海伦公式求面积
double dis = 2.0 * s / a;
dis = Math.Round(dis * 1e4) / 1e4;
return dis;
}
///
/// 计算各点到原点的距离和角度
///
public void InitPlanRoute()
{
try
{
string condition = string.Format("rule_name = '{0}'", SysEnvironment.CurrentRuleName);
locationList = BLLFactory.Instance.Find(condition);
startPointRef.x = locationList[0].LocationLon;
startPointRef.y = locationList[0].LocationLat;
startPointRef.z = locationList[0].LocationHeight;
for (int i = 1; i < locationList.Count; i++)
{
WGS84Points endPoint = new WGS84Points();
endPoint.x = locationList[i].LocationLon;
endPoint.y = locationList[i].LocationLat;
PointsToOrigin origin = new PointsToOrigin();
origin.Height = endPoint.z = locationList[i].LocationHeight;
origin.distance = GetDistance(startPointRef, endPoint);
origin.TanAngle = GetTanAngle(startPointRef, endPoint);
origin.Speed = locationList[i].LocationSpeed;
redis.SetHash(PointsToOrigin, i.ToString(), origin);
}
PointsToOrigin Reference = new PointsToOrigin();
Reference = redis.GetHash(PointsToOrigin, count.ToString());
distanceRefAction = Reference.distance;
angleRefAction = Reference.TanAngle;
speedRefAction = Reference.Speed;
}
catch (Exception ex)
{
LogHelper.log.Error("计算规划的路径距离原点距离和Tan角度出现错误", ex);
}
}
private int i = 1;
///
/// 执行加减速和方向控制的PID算法
///
private void ExcutePid()
{
double distanceCur = GetDistance(startPointCur, pointCur);
double tan = GetTanAngle(startPointCur, pointCur);
PointsToOrigin Reference = new PointsToOrigin();
int A = 0, B = 0;
do
{
Reference = redis.GetHash(PointsToOrigin, i.ToString());
B = i;
A = i - 1;
i++;
}
while (Reference.distance >= distanceCur && Math.Abs(Reference.TanAngle - tan) <= angleError);
WGS84Points pointA = new WGS84Points();
pointA.x = locationList[A].LocationLon;
pointA.y = locationList[A].LocationLat;
WGS84Points pointB = new WGS84Points();
pointB.x = locationList[B].LocationLon;
pointB.y = locationList[B].LocationLat;
double accFactor = 0, befAccFactor = 0;
PidAlgorithm accPid = new PidAlgorithm(1, 1, 1); // TODO传入比例系数
double tempSpeed = 0, speedLast = 0, curSpeedError = 0, befSpeedError1 = 0, befSpeedError2 = 0;
double angFactor = 0, befAangFactor = 0;
PidAlgorithm angPid = new PidAlgorithm(1, 1, 1); // TODO传入比例系数
double tempAng = 0, curAngError = 0, befAngError1 = 0, befAngError2 = 0;
do
{
double curSpeed = BlCanBusPerception.Instance.GetRealSpeed; // 调用Can总线接口,获取实时速度
double acceleration = (curSpeed - speedLast) / timeSpan / PreNumbers;
double preSpeed = PreSpeed(curSpeed, acceleration);
curSpeedError = preSpeed - Reference.Speed;
speedLast = curSpeed;
double factorx = PrePointFactorX(Math.Atan(tan), angFactor, curSpeed, acceleration, startPointCur, pointCur);
double factory = PrePointFactorY(Math.Atan(tan), angFactor, curSpeed, acceleration, startPointCur, pointCur);
pointCur.x += pointCur.x * factorx;
pointCur.y += pointCur.y * factory;
curAngError = GetPointLineDistance(pointCur, pointA, pointB);
accFactor = accPid.Pid(befAccFactor, curSpeedError, befSpeedError1, befSpeedError2);
// 调用加速接口
//BeckhoffDrive.Accelerator((float)accFactor);
string condition = string.Format("当前油门系数值{0}", accFactor);
LogHelper.log.Debug(condition);
befAccFactor = accFactor;
befSpeedError1 = curSpeedError;
befSpeedError2 = tempSpeed;
tempSpeed = befSpeedError1;
angFactor = angPid.Pid(befAangFactor, curAngError, befAngError1, befAngError2);
// 调用转盘方向角,基于当前角度调整
//BeckhoffDrive.SteerAngCt((float)angFactor);
condition = string.Format("当前转盘方向角{0}", angFactor);
LogHelper.log.Debug(condition);
befAangFactor = angFactor;
befAngError1 = curAngError;
befAngError2 = tempAng;
tempAng = befAngError1;
Thread.Sleep(50);
}
while ((accFactor != 0) && (angFactor != 0));
}
///
/// 更新指令集执行结果
///
private void UpdateDirective()
{
try
{
double distance = GetDistance(startPointCur, pointCur);
double angle = GetTanAngle(startPointCur, pointCur);
if (distanceRefAction <= distance && Math.Abs(angleRefAction - angle) <= angleError)
{
string speed = "00";
MoActionDirective mo = BLLFactory.Instance.FindByID(new object[] { SysEnvironment.CurrentRuleName, count });
//更新数据库
if (Math.Abs(Convert.ToDouble(speed) - speedRefAction) <= speedError)
{
mo.DirectiveResult = 1;
}
else
{
mo.DirectiveResult = 0;
}
BLLFactory.Instance.Update(mo, new object[] { SysEnvironment.CurrentRuleName, count });
//下一个指令信息
count++;
PointsToOrigin Reference = new PointsToOrigin();
Reference = redis.GetHash(PointsToOrigin, count.ToString());
distanceRefAction += Reference.distance;
angleRefAction = Reference.TanAngle;
speedRefAction = Reference.Speed;
}
}
catch (Exception ex)
{
LogHelper.log.Error(string.Format("更新动作指令执行结果失败:{0}", ex.ToString()));
}
}
#region 预测模型
public double timeSpan = 0.05; // 预测时间间隔
public double longf = 2.0; // 前轮到重心的距离
public double longr = 2.0; // 后轮到重心的距离
public int PreNumbers = 10;
///
/// 预测模型中的速度值
///
///
///
///
private double PreSpeed(double speedCur, double acceleration)
{
double speedPre = speedCur;
for (int i = 0; i < PreNumbers; i++)
{
speedPre += acceleration * timeSpan;
}
return speedPre;
}
///
/// 预测模型中的前一个点的角度值
///
///
///
///
///
///
private double PreAngle(double angleCur, double anglePid, double speedCur, double acceleration)
{
double b = Math.Atan(longr / (longf + longr) * Math.Tan(anglePid));
double anglePre = angleCur;
double speedPre = speedCur;
for (int i = 0; i < PreNumbers - 1; i++)
{
speedPre += acceleration * timeSpan;
anglePre += speedPre / longr * Math.Sin(b) * timeSpan;
}
return anglePre;
}
///
/// 预测模型中X坐标比例系数,相对于当前坐标值的比例系数
///
///
///
///
///
///
///
///
///
private double PrePointFactorX(double angleCur, double anglePid, double speedCur,
double acceleration, WGS84Points startPoint, WGS84Points endPoint)
{
double b = Math.Atan(longr / (longf + longr) * Math.Tan(anglePid));
double speedPre = speedCur;
double anglePre = angleCur;
double tempx = 0;
for (int i = 0; i < PreNumbers -1; i++)
{
anglePre += speedPre / longr * Math.Sin(b) * timeSpan;
tempx += speedPre * Math.Cos(anglePre + b) * timeSpan;
speedPre += acceleration * timeSpan;
}
WGS84Points endPointX = new WGS84Points();
endPointX.x = endPoint.x;
endPointX.y = startPoint.y;
double distanceX = GetDistance(startPoint, endPointX);
return tempx / distanceX;
}
///
/// 预测模型中X坐标比例系数,相对于当前坐标值的比例系数
///
///
///
///
///
///
///
///
///
private double PrePointFactorY(double angleCur, double anglePid, double speedCur,
double acceleration, WGS84Points startPoint, WGS84Points endPoint)
{
double b = Math.Atan(longr / (longf + longr) * Math.Tan(anglePid));
double speedPre = speedCur;
double anglePre = angleCur;
double tempy = 0;
for (int i = 0; i < PreNumbers - 1; i++)
{
anglePre += speedPre / longr * Math.Sin(b) * timeSpan;
tempy += speedPre * Math.Sin(anglePre + b) * timeSpan;
speedPre += acceleration * timeSpan;
}
WGS84Points endPointY = new WGS84Points();
endPointY.x = startPoint.x;
endPointY.y = endPoint.y;
double distanceY = GetDistance(startPoint, endPointY);
return tempy / distanceY;
}
#endregion
}
///
/// 起始点事件
///
public delegate void EventStartPointHandler(StartPointEventArgs e);
public class StartPointEventArgs : EventArgs
{
public string Mesg { get; set; }
public StartPointEventArgs(string value)
{
this.Mesg = value;
}
}
}