/// <summary>
/// 向下位机发送指令
/// </summary>
/// <param name="startbit">起始位</param>
/// <param name="MyRov">参数结构体</param>
public static void SendCommand(byte startbit, ROVDatabase MyRov)
{
byte[] TXData = DataTX_Handler(startbit, MyRov);
if (ComDevice.IsOpen)
{
try
{
ComDevice.Write(TXData, 0, TXData.Length);
}
catch (Exception e)
{
MessageBox.Show(e.Message);
}
}
}
/// <summary>
/// 对接收到的下位机传来的数据包进行解包
/// </summary>
/// <param name="Buffer">数据包</param>
/// <param name="MyRov">ROV数据结构体</param>
public static void DataRX_Handler(List <byte> Buffer, ref ROVDatabase MyRov)
{
byte[] RxBuffer = Buffer.ToArray(); //数据包数组
switch (RxBuffer[0])
{
case (RX_StartBit_JY901): //表示待解析的是JY901姿态传感器的数据包
{
MyRov.JY901Data.AccelerationX = BitConverter.ToSingle(RxBuffer, 1);
MyRov.JY901Data.AccelerationY = BitConverter.ToSingle(RxBuffer, 5);
MyRov.JY901Data.AccelerationZ = BitConverter.ToSingle(RxBuffer, 9);
MyRov.JY901Data.PitchAngle = BitConverter.ToSingle(RxBuffer, 13);
MyRov.JY901Data.YawAngle = BitConverter.ToSingle(RxBuffer, 17);
MyRov.JY901Data.RollAngle = BitConverter.ToSingle(RxBuffer, 21);
MyRov.JY901Data.AngleSpeedX = BitConverter.ToSingle(RxBuffer, 25);
MyRov.JY901Data.AngleSpeedY = BitConverter.ToSingle(RxBuffer, 29);
MyRov.JY901Data.AngleSpeedZ = BitConverter.ToSingle(RxBuffer, 33);
break;
}
case (RX_StartBit_MS5837): //表示待解析的是MS5837深度传感器的数据包
{
MyRov.MS5837Data.Pressure = BitConverter.ToSingle(RxBuffer, 1);
MyRov.MS5837Data.Depth = BitConverter.ToSingle(RxBuffer, 5);
MyRov.MS5837Data.Temperature = BitConverter.ToSingle(RxBuffer, 9);
MyRov.MS5837Data.Offset = BitConverter.ToSingle(RxBuffer, 13);
break;
}
case (RX_StartBit_SERVO): //表示待解析的是舵机的数据包
{
MyRov.ServoData.FinTail_Front_StartingPosition = BitConverter.ToUInt16(RxBuffer, 1); //尾部推进舵机 起始位置
MyRov.ServoData.FinTail_Front_EndingPosition = BitConverter.ToUInt16(RxBuffer, 3); //尾部推进舵机 终止位置
MyRov.ServoData.FinTail_Front_EachCCR = BitConverter.ToUInt16(RxBuffer, 5); //尾部推进舵机 每次改变的占空比
MyRov.ServoData.FinTail_Front_DelayTime = BitConverter.ToUInt16(RxBuffer, 7); //尾部推进舵机 延时长度
MyRov.ServoData.FinTail_Rear_StartingPosition = BitConverter.ToUInt16(RxBuffer, 9); //尾部推进舵机 起始位置
MyRov.ServoData.FinTail_Rear_EndingPosition = BitConverter.ToUInt16(RxBuffer, 11); //尾部推进舵机 终止位置
MyRov.ServoData.FinTail_Rear_EachCCR = BitConverter.ToUInt16(RxBuffer, 13); //尾部推进舵机 每次改变的占空比
MyRov.ServoData.FinTail_Rear_DelayTime = BitConverter.ToUInt16(RxBuffer, 15); //尾部推进舵机 延时长度
MyRov.ServoData.Camera_Position = BitConverter.ToUInt16(RxBuffer, 17); //摄像机云台舵机 位置
MyRov.ServoData.Pulse_Num = BitConverter.ToUInt16(RxBuffer, 19); //脉冲数
break;
}
case (RX_StartBit_PID): //表示待解析的是PID系数的数据包
{
MyRov.pidData_Depth.Kp = BitConverter.ToSingle(RxBuffer, 1);
MyRov.pidData_Depth.Ki = BitConverter.ToSingle(RxBuffer, 5);
MyRov.pidData_Depth.Kd = BitConverter.ToSingle(RxBuffer, 9);
MyRov.pidData_Depth.Target = BitConverter.ToSingle(RxBuffer, 13);
MyRov.pidData_Pitch.Kp = BitConverter.ToSingle(RxBuffer, 17);
MyRov.pidData_Pitch.Ki = BitConverter.ToSingle(RxBuffer, 21);
MyRov.pidData_Pitch.Kd = BitConverter.ToSingle(RxBuffer, 25);
MyRov.pidData_Pitch.Target = BitConverter.ToSingle(RxBuffer, 29);
MyRov.pidData_Yaw.Kp = BitConverter.ToSingle(RxBuffer, 33);
MyRov.pidData_Yaw.Ki = BitConverter.ToSingle(RxBuffer, 37);
MyRov.pidData_Yaw.Kd = BitConverter.ToSingle(RxBuffer, 41);
MyRov.pidData_Yaw.Target = BitConverter.ToSingle(RxBuffer, 45);
MyRov.pidData_AccelerationY.Kp = BitConverter.ToSingle(RxBuffer, 49);
MyRov.pidData_AccelerationY.Ki = BitConverter.ToSingle(RxBuffer, 53);
MyRov.pidData_AccelerationY.Kd = BitConverter.ToSingle(RxBuffer, 57);
MyRov.pidData_AccelerationY.Target = BitConverter.ToSingle(RxBuffer, 61);
break;
}
}
}
/// <summary>
/// 向下位机发送数据包
/// </summary>
/// <param name="startbit"></param>
/// <param name="MyRov"></param>
/// <returns></returns>
public static byte[] DataTX_Handler(byte startbit, ROVDatabase MyRov)
{
byte CheckBit_TX = 0x00; //数据发送校验位
//初始化存储发送数据的集合，添加数据头
List <byte> List_TxBuffer = new List <byte>
{
startbit
};
switch (startbit)
{
case (TX_StartBit_MODE): //表示发送的是控制模式
{
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.Mode.ControlMode));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.Mode.PitchMode));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.Mode.AngleSpeedYMode));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.Mode.DepthMode));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.Mode.AccelerationYMode));
break;
}
case (TX_StartBit_SERVO): //表示发送的是舵机相关的指令
{
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Front_StartingPosition));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Front_EndingPosition));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Front_EachCCR));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Front_DelayTime));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Rear_StartingPosition));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Rear_EndingPosition));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Rear_EachCCR));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.FinTail_Rear_DelayTime));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.Camera_Position));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.ServoData.Pulse_Num));
break;
}
case (TX_StartBit_PID):
{
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Depth.Kp));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Depth.Ki));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Depth.Kd));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Depth.Target));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Pitch.Kp));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Pitch.Ki));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Pitch.Kd));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Pitch.Target));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Yaw.Kp));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Yaw.Ki));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Yaw.Kd));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_Yaw.Target));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_AccelerationY.Kp));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_AccelerationY.Ki));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_AccelerationY.Kd));
List_TxBuffer.AddRange(BitConverter.GetBytes(MyRov.pidData_AccelerationY.Target));
break;
}
}
for (int i = 0; i < List_TxBuffer.Count; i++)
{
CheckBit_TX ^= List_TxBuffer[i];
}
List_TxBuffer.Add(CheckBit_TX);
byte[] TxBuffer = List_TxBuffer.ToArray();
return(TxBuffer);
}
