/* **********************************************************************************
*
* Function: private bool updateAHRS
* Inputs: int index
* Outputs: None
* Return Value: bool success
* Dependencies: None
* Description:
*
* Causes a packet to be sent to the AHRS to update its state to conform with the internal
* state of the AHRS class.
*
* Returns 'true' on success, 'false' otherwise
*
* *********************************************************************************/
private bool updateAHRS(int index)
{
Packet AHRSPacket = new Packet();
byte_conversion_array DConvert = new byte_conversion_array();
AHRSPacket.Ch_Status = mChBox_CH;
if (index == (int)StateName.STATE_WHO_AM_I)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_MCU_INFORMATION];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = PID[(int)PName.WHO_AM_I];
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_FIRMWARE_VERSION)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_MCU_INFORMATION];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = PID[(int)PName.FIRMWARE_VERSION];
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_ID_STATUS)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_MCU_INFORMATION];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = PID[(int)PName.ID_STATUS];
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_REPORT_MCU_INFORMATION)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_MCU_INFORMATION];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = PID[(int)PName.CONTROLLER_STATUS];
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_PWM_PARAMETER)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_MCU_INFORMATION];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = PID[(int)PName.PWM_PARAMETER];
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_KICK_OFF_CONTROLLER)
{
AHRSPacket.PacketType = PID[(int)PName.KICK_OFF_CONTROLLER];
AHRSPacket.DataLength = 1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_RESET_CONTROLLER)
{
AHRSPacket.PacketType = PID[(int)PName.RESET_CONTROLLER];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = 0x00;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_REPORT_SENSOR_ADC)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_MCU_INFORMATION];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
AHRSPacket.Data[0] = PID[(int)PName.REPORT_SENSOR_ADC];
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_HOME_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.HOME_CMD];
AHRSPacket.DataLength = 1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_PAUSE_REPORT_INFO)
{
AHRSPacket.PacketType = PID[(int)PName.PAUSE_REPORT_INFO];
AHRSPacket.DataLength = 1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_CONTINUE_REPORT_INFO)
{
AHRSPacket.PacketType = PID[(int)PName.CONTINUE_REPORT_INFO];
AHRSPacket.DataLength = 1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_ALL_OF_PID_PARAM)
{
AHRSPacket.PacketType = PID[(int)PName.ALL_OF_PID_PARAM];
AHRSPacket.DataLength = 49;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Pos.Kp;
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.Data[2] = DConvert.byte2;
AHRSPacket.Data[3] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Pos.Ki;
AHRSPacket.Data[4] = DConvert.byte0;
AHRSPacket.Data[5] = DConvert.byte1;
AHRSPacket.Data[6] = DConvert.byte2;
AHRSPacket.Data[7] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Pos.Kd;
AHRSPacket.Data[8] = DConvert.byte0;
AHRSPacket.Data[9] = DConvert.byte1;
AHRSPacket.Data[10] = DConvert.byte2;
AHRSPacket.Data[11] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Pos.Int_sat;
AHRSPacket.Data[12] = DConvert.byte0;
AHRSPacket.Data[13] = DConvert.byte1;
AHRSPacket.Data[14] = DConvert.byte2;
AHRSPacket.Data[15] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Vel.Kp;
AHRSPacket.Data[16] = DConvert.byte0;
AHRSPacket.Data[17] = DConvert.byte1;
AHRSPacket.Data[18] = DConvert.byte2;
AHRSPacket.Data[19] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Vel.Ki;
AHRSPacket.Data[20] = DConvert.byte0;
AHRSPacket.Data[21] = DConvert.byte1;
AHRSPacket.Data[22] = DConvert.byte2;
AHRSPacket.Data[23] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Vel.Kd;
AHRSPacket.Data[24] = DConvert.byte0;
AHRSPacket.Data[25] = DConvert.byte1;
AHRSPacket.Data[26] = DConvert.byte2;
AHRSPacket.Data[27] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Vel.Int_sat;
AHRSPacket.Data[28] = DConvert.byte0;
AHRSPacket.Data[29] = DConvert.byte1;
AHRSPacket.Data[30] = DConvert.byte2;
AHRSPacket.Data[31] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Tor.Kp;
AHRSPacket.Data[32] = DConvert.byte0;
AHRSPacket.Data[33] = DConvert.byte1;
AHRSPacket.Data[34] = DConvert.byte2;
AHRSPacket.Data[35] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Tor.Ki;
AHRSPacket.Data[36] = DConvert.byte0;
AHRSPacket.Data[37] = DConvert.byte1;
AHRSPacket.Data[38] = DConvert.byte2;
AHRSPacket.Data[39] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Tor.Kd;
AHRSPacket.Data[40] = DConvert.byte0;
AHRSPacket.Data[41] = DConvert.byte1;
AHRSPacket.Data[42] = DConvert.byte2;
AHRSPacket.Data[43] = DConvert.byte3;
DConvert.int32 = m_Motor_PID_Member[mChBox_CH].Tor.Int_sat;
AHRSPacket.Data[44] = DConvert.byte0;
AHRSPacket.Data[45] = DConvert.byte1;
AHRSPacket.Data[46] = DConvert.byte2;
AHRSPacket.Data[47] = DConvert.byte3;
//AHRSPacket.Data[48] = 0x00;
//AHRSPacket.Data[49] = 0x00;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_MAX_OF_POSITION_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.MAX_OF_POSITION_CMD];
AHRSPacket.DataLength = 3;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int16_0 = m_Pos_SoftStart[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_MAX_OF_VELOCITY_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.MAX_OF_VELOCITY_CMD];
AHRSPacket.DataLength = 3;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int16_0 = m_Max_Vel_Cmd[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_MAX_OF_TORQUE_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.MAX_OF_TORQUE_CMD];
AHRSPacket.DataLength = 3;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int16_0 = m_Max_Tor_Cmd[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_MAX_OF_PWM_DUTYCYCLE)
{
AHRSPacket.PacketType = PID[(int)PName.MAX_OF_PWM_DUTYCYCLE];
AHRSPacket.DataLength = 3;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int16_0 = m_Max_PWM_Cmd[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_POSITION_TARGET_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.POSITION_TARGET_CMD];
AHRSPacket.DataLength = 5;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.uint32 = m_Position_Target[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.Data[2] = DConvert.byte2;
AHRSPacket.Data[3] = DConvert.byte3;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_VELOCITY_EXT_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.VELOCITY_EXT_CMD];
AHRSPacket.DataLength = 3;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int16_0 = m_Velocity_External[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_TORQUE_EXT_CMD)
{
AHRSPacket.PacketType = PID[(int)PName.TORQUE_EXT_CMD];
AHRSPacket.DataLength = 3;
AHRSPacket.Data = new byte[AHRSPacket.DataLength - 1];
DConvert.int16_0 = m_Torque_External[mChBox_CH];
AHRSPacket.Data[0] = DConvert.byte0;
AHRSPacket.Data[1] = DConvert.byte1;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_REPORT_INFO_PERIOD)
{
AHRSPacket.PacketType = PID[(int)PName.REPORT_INFO_PERIOD];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[1];
AHRSPacket.Data[0] = Convert.ToByte(50);
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
else if (index == (int)StateName.STATE_CONTROLLER_STATUS)
{
AHRSPacket.PacketType = 0x12;
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[1];
AHRSPacket.Data[0] = 0x0f;
AHRSPacket.CRC8 = Check_CRC8(AHRSPacket); // calculation CRC-8
if (!sendPacket(AHRSPacket))
return false;
}
return true;
}
private byte Check_CRC8(Packet packet)
{
byte CRC8_Temp = 0;
byte Index;
CRC8_Temp = updcrc8(CRC8_Temp, (byte)'E');
CRC8_Temp = updcrc8(CRC8_Temp, (byte)'C');
CRC8_Temp = updcrc8(CRC8_Temp, (byte)'S');
CRC8_Temp = updcrc8(CRC8_Temp, packet.PacketType);
CRC8_Temp = updcrc8(CRC8_Temp, packet.Ch_Status);
CRC8_Temp = updcrc8(CRC8_Temp, packet.DataLength);
for (Index = 0; Index < (packet.DataLength - 1); Index++)
{
CRC8_Temp = updcrc8(CRC8_Temp, packet.Data[Index]);
}
return CRC8_Temp;
}
/* **********************************************************************************
*
* Function: private void serialPort_DataReceived
* Inputs: object sender, SerialDataReceivedEventArgs e
* Outputs: None
* Return Value: bool success
* Dependencies: None
* Description:
*
* Event handler for serial communication
*
* *********************************************************************************/
private void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
int continue_parsing = 1;
int bytes_to_read;
try
{
bytes_to_read = serialPort.BytesToRead;
if ((RXbufPtr + bytes_to_read) >= RX_BUF_SIZE)
{
RXbufPtr = 0;
}
if (bytes_to_read >= RX_BUF_SIZE)
{
bytes_to_read = RX_BUF_SIZE - 1;
}
// Get serial data
serialPort.Read(RXbuffer, RXbufPtr, bytes_to_read);
//serialPort.Read(RXbuffer, 0, bytes_to_read);
RXbufPtr += bytes_to_read;
}
catch
{
COMFailedEvent();
return;
}
bool found_packet;
int packet_start_index;
int packet_index;
// If there are enough bytes in the buffer to construct a full packet, then check data.
// There are RXbufPtr bytes in the buffer at any given time
while (RXbufPtr >= 8 && (continue_parsing == 1))
{
// Search for the packet start sequence
found_packet = false;
packet_start_index = 0;
for (packet_index = 0; packet_index < (RXbufPtr - 2); packet_index++)
{
if (RXbuffer[packet_index] == 'E' && RXbuffer[packet_index + 1] == 'C' && RXbuffer[packet_index + 2] == 'S')
{
found_packet = true;
packet_start_index = packet_index;
break;
}
}
// If start sequence found, try to recover all the data in the packet
if (found_packet && ((RXbufPtr - packet_start_index) >= 8))
{
int i;
Packet DataPacket = new Packet();
DataPacket.PacketType = RXbuffer[packet_start_index + 3];
DataPacket.Ch_Status = RXbuffer[packet_start_index + 4];
DataPacket.DataLength = RXbuffer[packet_start_index + 5];
DataPacket.Data = new byte[DataPacket.DataLength];
// Only process packet if data_size is not too large.
if (DataPacket.DataLength <= MAX_PACKET_SIZE)
{
// If a full packet has been received, then the full packet size should be
// 3 + 1 + 1 + 1 + [data_size] + 1
// that is, 3 bytes for the start sequence, 1 byte for type, 1 byte for status, 1 byte for data length,
// data_size bytes for packet data inculde 1 bytes for the CRC-8.
// If enough data has been received, go ahead and recover the packet. If not, wait until the
// rest of the data arrives
int buffer_length = (RXbufPtr - packet_start_index);
int packet_length = (6 + DataPacket.DataLength);
if (buffer_length >= packet_length)
{
if (DataPacket.DataLength == 0)
{
// this packet length is wrong!!!
}
else
{
// A full packet has been received. Retrieve the data.
for (i = 0; i < (DataPacket.DataLength - 1); i++)
{
DataPacket.Data[i] = RXbuffer[packet_start_index + 6 + i];
}
DataPacket.CRC8 = RXbuffer[packet_start_index + 6 + i];
handle_packet(DataPacket);
//continue_parsing = 0;//Delay的嫌疑犯?
}
// Copy all received bytes that weren't part of this packet into the beginning of the
// buffer. Then, reset RXbufPtr.
for (int index = 0; index < (buffer_length - packet_length); index++)
{
RXbuffer[index] = RXbuffer[(packet_start_index + packet_length) + index];
}
RXbufPtr = (buffer_length - packet_length);
}
else
{
continue_parsing = 0;
}
}
else
{
// data_size was too large - the packet data is invalid. Clear the RX buffer.
RXbufPtr = 0;
continue_parsing = 0;
PacketReceivedEvent(PName.CMD_OVER_DATA_LENGTH, -1, DataPacket.Ch_Status);
}
}
else
{
continue_parsing = 0;
}
}
/*
if (k > 2)
k = 0;
else
k = k + 1;
mChBox_CH = Convert.ToByte(k);
//if (k==0)
updateAHRS((int)StateName.STATE_CONTROLLER_STATUS);
System.Threading.Thread.Sleep(300);*/
}
/* **********************************************************************************
*
* Function: private bool sendPacket
* Inputs: Packet AHRSPacket - the packet to be transmitted
* Outputs: None
* Return Value: bool success
* Dependencies: None
* Description:
*
* Sends the specified packet to the AHRS.
*
* Returns 'true' on success, 'false' otherwise
*
* *********************************************************************************/
private bool sendPacket(Packet AHRSPacket)
{
int i;
if (!connected)
return false;
if (AHRSPacket.DataLength == 0)
{
// this command is failed!!!
return false;
}
byte[] packet = new byte[AHRSPacket.DataLength + 6];
AHRSPacket.Ch_Status = mChBox_CH;
// Build packet header
packet[0] = (byte)'E';
packet[1] = (byte)'C';
packet[2] = (byte)'S';
packet[3] = AHRSPacket.PacketType;
packet[4] = AHRSPacket.Ch_Status; // channle/status
packet[5] = AHRSPacket.DataLength;
// Fill data section
for (i = 0; i < (AHRSPacket.DataLength - 1); i++)
{
packet[6 + i] = AHRSPacket.Data[i];
}
// Add CRC-8 to end of packet
packet[6 + i] = AHRSPacket.CRC8;
// Now write the packet to the serial port
try
{
serialPort.Write(packet, 0, AHRSPacket.DataLength + 6);
PacketSentEvent((PName)getTypeIndex(AHRSPacket.PacketType), 0, mChBox_CH);
}
catch
{
return false;
}
return true;
}
/* **********************************************************************************
*
* Function: private void handle_packet
* Inputs: byte type, int length, byte[] data
* Outputs: None
* Return Value: None
* Dependencies: None
* Description:
*
* Handles data packets received over the serial port.
*
* *********************************************************************************/
private void handle_packet(Packet DataPacket)
{
int type_index = -1;
byte CRC8_Temp;
byte_conversion_array DConvert = new byte_conversion_array();
type_index = getTypeIndex(DataPacket.PacketType);
// For the packet received, update 'dataPending' and 'ElapsedTime' flags
if (type_index != -1)
{
CRC8_Temp = Check_CRC8(DataPacket); // calculation CRC-8
if (CRC8_Temp != DataPacket.CRC8)
{
PacketReceivedEvent(PName.CMD_CRC_FAILED, -1 , DataPacket.Ch_Status);
}
else
{
updatePacketSynch((PName)type_index, DataPacket.Data);
}
}
else
{
// Generate a COMMAND_COMPLETE event with a -1 flag. The -1 indicates that the packet
// wasn't recognized.
PacketReceivedEvent(PName.CMD_COMPLETE, -1, DataPacket.Ch_Status);
return;
}
switch (type_index)
{
case (int)PName.WHO_AM_I:
PacketLabelEvent(DataPacket.Data, 1);
PacketReceivedEvent(PName.WHO_AM_I, type_index, DataPacket.Ch_Status);
break;
case (int)PName.FIRMWARE_VERSION:
PacketLabelEvent(DataPacket.Data, 2);
PacketReceivedEvent(PName.FIRMWARE_VERSION, type_index, DataPacket.Ch_Status);
break;
case (int)PName.REPORT_SENSOR_ADC:
PacketLabelEvent(DataPacket.Data, 23);
PacketReceivedEvent(PName.REPORT_SENSOR_ADC, type_index, DataPacket.Ch_Status);
break;
case (int)PName.ID_STATUS:
PacketReceivedEvent(PName.ID_STATUS, type_index, DataPacket.Ch_Status);
break;
case (int)PName.PWM_PARAMETER:
PacketReceivedEvent(PName.PWM_PARAMETER, type_index, DataPacket.Ch_Status);
break;
case (int)PName.CMD_COMPLETE:
type_index = getTypeIndex(DataPacket.Data[0]);
PacketReceivedEvent(PName.CMD_COMPLETE, type_index, DataPacket.Ch_Status);
break;
case (int)PName.CMD_NO_SUPPORT:
PacketReceivedEvent(PName.CMD_NO_SUPPORT, DataPacket.Data[0], DataPacket.Ch_Status);
break;
case (int)PName.CMD_CRC_FAILED:
PacketReceivedEvent(PName.CMD_CRC_FAILED, 0, DataPacket.Ch_Status);
break;
case (int)PName.CMD_OVER_DATA_LENGTH:
PacketReceivedEvent(PName.CMD_OVER_DATA_LENGTH, 0, DataPacket.Ch_Status);
break;
case (int)PName.CONTROLLER_STATUS:
if (DataPacket.DataLength != 23)
{
// this packet is wrong!!!
return;
}
this.ch_tmp = DataPacket.Ch_Status;
switch (ch_tmp)
{
case 0:
m_Motor_Member[0].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[0].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[0].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[0].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[0].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[0].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[0].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[0].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[0].Motor_Current = DConvert.int16_0;
m_Motor_Member[0].PWM_Output = DataPacket.Data[21];
break;
case 1:
m_Motor_Member[1].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[1].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[1].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[1].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[1].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[1].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[1].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[1].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[1].Motor_Current = DConvert.int16_0;
m_Motor_Member[1].PWM_Output = DataPacket.Data[21];
break;
case 2:
m_Motor_Member[2].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[2].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[2].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[2].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[2].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[2].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[2].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[2].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[2].Motor_Current = DConvert.int16_0;
m_Motor_Member[2].PWM_Output = DataPacket.Data[21];
break;
case 3:
m_Motor_Member[3].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[3].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[3].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[3].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[3].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[3].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[3].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[3].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[3].Motor_Current = DConvert.int16_0;
m_Motor_Member[3].PWM_Output = DataPacket.Data[21];
break;
case 4:
m_Motor_Member[4].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[4].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[4].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[4].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[4].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[4].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[4].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[4].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[4].Motor_Current = DConvert.int16_0;
m_Motor_Member[4].PWM_Output = DataPacket.Data[21];
break;
case 5:
m_Motor_Member[5].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[5].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[5].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[5].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[5].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[5].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[5].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[5].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[5].Motor_Current = DConvert.int16_0;
m_Motor_Member[5].PWM_Output = DataPacket.Data[21];
break;
case 6:
m_Motor_Member[6].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[6].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[6].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[6].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[6].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[6].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[6].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[6].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[6].Motor_Current = DConvert.int16_0;
m_Motor_Member[6].PWM_Output = DataPacket.Data[21];
break;
default:
break;
}
/*
m_Motor_Member[mChBox_CH].ticker = DataPacket.Data[0];
DConvert.byte0 = DataPacket.Data[1];
DConvert.byte1 = DataPacket.Data[2];
DConvert.byte2 = DataPacket.Data[3];
DConvert.byte3 = DataPacket.Data[4];
m_Motor_Member[mChBox_CH].Position_Target = DConvert.uint32; // command
DConvert.byte0 = DataPacket.Data[5];
DConvert.byte1 = DataPacket.Data[6];
DConvert.byte2 = DataPacket.Data[7];
DConvert.byte3 = DataPacket.Data[8];
m_Motor_Member[mChBox_CH].QEI32 = DConvert.uint32;
DConvert.byte0 = DataPacket.Data[9];
DConvert.byte1 = DataPacket.Data[10];
m_Motor_Member[mChBox_CH].Velocity_External = DConvert.int16_0; // command
DConvert.byte0 = DataPacket.Data[11];
DConvert.byte1 = DataPacket.Data[12];
m_Motor_Member[mChBox_CH].Velocity_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[13];
DConvert.byte1 = DataPacket.Data[14];
m_Motor_Member[mChBox_CH].QEI_Diff16 = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[15];
DConvert.byte1 = DataPacket.Data[16];
m_Motor_Member[mChBox_CH].Torque_External = DConvert.int16_0; //command
DConvert.byte0 = DataPacket.Data[17];
DConvert.byte1 = DataPacket.Data[18];
m_Motor_Member[mChBox_CH].Torque_Internal = DConvert.int16_0;
DConvert.byte0 = DataPacket.Data[19];
DConvert.byte1 = DataPacket.Data[20];
m_Motor_Member[mChBox_CH].Motor_Current = DConvert.int16_0;
m_Motor_Member[mChBox_CH].PWM_Output = DataPacket.Data[21];
*/
PacketReceivedEvent(PName.CONTROLLER_STATUS, 0, DataPacket.Ch_Status);
bitstatue = Convert.ToByte(Convert.ToByte(DataPacket.Ch_Status) & 0xf0);
//bitstatue +=1;
DataReceivedEvent(ch_tmp);
break;
default:
break;
}
}
/* **********************************************************************************
*
* Function: private bool updateAHRS
* Inputs: int index
* Outputs: None
* Return Value: bool success
* Dependencies: None
* Description:
*
* Causes a packet to be sent to the AHRS to update its state to conform with the internal
* state of the AHRS class.
*
* Returns 'true' on success, 'false' otherwise
*
* *********************************************************************************/
private bool updateAHRS(int index)
{
Packet AHRSPacket = new Packet();
byte_conversion_array ftob = new byte_conversion_array();
if (index == (int)StateName.ACTIVE_CHANNELS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACTIVE_CHANNELS];
AHRSPacket.DataLength = 2;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
int active_channels = 0;
if (m_accelZ_active)
active_channels |= (1 << 1);
if (m_accelY_active)
active_channels |= (1 << 2);
if (m_accelX_active)
active_channels |= (1 << 3);
if (m_gyroZ_active)
active_channels |= (1 << 4);
if (m_gyroY_active)
active_channels |= (1 << 5);
if (m_gyroX_active)
active_channels |= (1 << 6);
if (m_magZ_active)
active_channels |= (1 << 7);
if (m_magY_active)
active_channels |= (1 << 8);
if (m_magX_active)
active_channels |= (1 << 9);
if (m_latitude_active)
active_channels |= (1 << 10);
if (m_longitude_active)
active_channels |= (1 << 11);
if (m_velGPS_active)
active_channels |= (1 << 12);
if (m_altitudine_active)
active_channels |= (1 << 13);
if (m_satellite_number_active)
active_channels |= (1 << 14);
AHRSPacket.Data[0] = (byte)((active_channels >> 8) & 0x0FF);
AHRSPacket.Data[1] = (byte)((active_channels) & 0x0FF);
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_ALIGNMENT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_ALIGNMENT];
AHRSPacket.DataLength = 36;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_accel_alignment[0, 0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_accel_alignment[0, 1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_accel_alignment[0, 2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_accel_alignment[1, 0];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_accel_alignment[1, 1];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_accel_alignment[1, 2];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_accel_alignment[2, 0];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_accel_alignment[2, 1];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_accel_alignment[2, 2];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_COVARIANCE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_COVARIANCE];
AHRSPacket.DataLength = 4;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_accel_covariance;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_BIAS];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_accel_bias;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_accel_bias;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_accel_bias;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ACCEL_REF)
{
AHRSPacket.PacketType = PID[(int)PName.SET_ACCEL_REF_VECTOR];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_accel_ref;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_accel_ref;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_accel_ref;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_BIAS];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_mag_bias;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_mag_bias;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_mag_bias;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_REF)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_REF_VECTOR];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_mag_ref;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_mag_ref;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_mag_ref;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.BROADCAST_MODE)
{
if (m_broadcast_enabled)
{
AHRSPacket.PacketType = PID[(int)PName.SET_BROADCAST_MODE];
AHRSPacket.DataLength = 1;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
AHRSPacket.Data[0] = (byte)(m_broadcast_rate);
}
else
{
AHRSPacket.PacketType = PID[(int)PName.SET_SILENT_MODE];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
}
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.EKF_CONFIG)
{
AHRSPacket.PacketType = PID[(int)PName.SET_EKF_CONFIG];
AHRSPacket.DataLength = 1;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
byte data = 0;
if (m_accel_enabled)
data |= 0x02;
if (m_mag_enabled)
data |= 0x01;
if (m_mag_restriced_to_yaw)
data |= 0x04;
AHRSPacket.Data[0] = data;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GYRO_ALIGNMENT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_GYRO_ALIGNMENT];
AHRSPacket.DataLength = 36;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_gyro_alignment[0, 0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_gyro_alignment[0, 1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_gyro_alignment[0, 2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_gyro_alignment[1, 0];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_gyro_alignment[1, 1];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_gyro_alignment[1, 2];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_gyro_alignment[2, 0];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_gyro_alignment[2, 1];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_gyro_alignment[2, 2];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GYRO_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.SET_GYRO_BIAS];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_x_gyro_bias;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_gyro_bias;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_z_gyro_bias;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GYRO_SCALE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_GYRO_SCALE];
AHRSPacket.DataLength = 12;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_z_gyro_scale;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_y_gyro_scale;
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_x_gyro_scale;
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_COVARIANCE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_COVARIANCE];
AHRSPacket.DataLength = 4;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_mag_covariance;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.PROCESS_COVARIANCE)
{
AHRSPacket.PacketType = PID[(int)PName.SET_PROCESS_COVARIANCE];
AHRSPacket.DataLength = 4;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_process_covariance;
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.START_CAL)
{
AHRSPacket.PacketType = PID[(int)PName.SET_START_CAL];
AHRSPacket.DataLength = 1;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
if (m_zero_gyros_on_startup)
AHRSPacket.Data[0] = 0x01;
else
AHRSPacket.Data[0] = 0x00;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.ZERO_GYROS)
{
AHRSPacket.PacketType = PID[(int)PName.ZERO_RATE_GYROS];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.AUTO_SET_MAG_REF)
{
AHRSPacket.PacketType = PID[(int)PName.AUTO_SET_MAG_REF];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.AUTO_SET_ACCEL_REF)
{
AHRSPacket.PacketType = PID[(int)PName.AUTO_SET_ACCEL_REF];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.SELF_TEST)
{
AHRSPacket.PacketType = PID[(int)PName.SELF_TEST];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.RESET_TO_FACTORY)
{
AHRSPacket.PacketType = PID[(int)PName.RESET_TO_FACTORY];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.EKF_RESET)
{
AHRSPacket.PacketType = PID[(int)PName.EKF_RESET];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.REBOOT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_REBOOT];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.MAG_CAL)
{
AHRSPacket.PacketType = PID[(int)PName.SET_MAG_CAL];
AHRSPacket.DataLength = 36;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_mag_cal[0, 0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_mag_cal[0, 1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_mag_cal[0, 2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_mag_cal[1, 0];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_mag_cal[1, 1];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_mag_cal[1, 2];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_mag_cal[2, 0];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_mag_cal[2, 1];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_mag_cal[2, 2];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.SET_KALMAN_PARM)
{
AHRSPacket.PacketType = PID[(int)PName.SET_KALMAN_PARM];
AHRSPacket.DataLength = 52;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
ftob.float0 = m_init_quat[0];
AHRSPacket.Data[0] = ftob.byte0;
AHRSPacket.Data[1] = ftob.byte1;
AHRSPacket.Data[2] = ftob.byte2;
AHRSPacket.Data[3] = ftob.byte3;
ftob.float0 = m_init_quat[1];
AHRSPacket.Data[4] = ftob.byte0;
AHRSPacket.Data[5] = ftob.byte1;
AHRSPacket.Data[6] = ftob.byte2;
AHRSPacket.Data[7] = ftob.byte3;
ftob.float0 = m_init_quat[2];
AHRSPacket.Data[8] = ftob.byte0;
AHRSPacket.Data[9] = ftob.byte1;
AHRSPacket.Data[10] = ftob.byte2;
AHRSPacket.Data[11] = ftob.byte3;
ftob.float0 = m_init_quat[3];
AHRSPacket.Data[12] = ftob.byte0;
AHRSPacket.Data[13] = ftob.byte1;
AHRSPacket.Data[14] = ftob.byte2;
AHRSPacket.Data[15] = ftob.byte3;
ftob.float0 = m_Q_bias[ 0];
AHRSPacket.Data[16] = ftob.byte0;
AHRSPacket.Data[17] = ftob.byte1;
AHRSPacket.Data[18] = ftob.byte2;
AHRSPacket.Data[19] = ftob.byte3;
ftob.float0 = m_Q_bias[1];
AHRSPacket.Data[20] = ftob.byte0;
AHRSPacket.Data[21] = ftob.byte1;
AHRSPacket.Data[22] = ftob.byte2;
AHRSPacket.Data[23] = ftob.byte3;
ftob.float0 = m_Q_bias[2];
AHRSPacket.Data[24] = ftob.byte0;
AHRSPacket.Data[25] = ftob.byte1;
AHRSPacket.Data[26] = ftob.byte2;
AHRSPacket.Data[27] = ftob.byte3;
ftob.float0 = m_gain[0];
AHRSPacket.Data[28] = ftob.byte0;
AHRSPacket.Data[29] = ftob.byte1;
AHRSPacket.Data[30] = ftob.byte2;
AHRSPacket.Data[31] = ftob.byte3;
ftob.float0 = m_gain[1];
AHRSPacket.Data[32] = ftob.byte0;
AHRSPacket.Data[33] = ftob.byte1;
AHRSPacket.Data[34] = ftob.byte2;
AHRSPacket.Data[35] = ftob.byte3;
ftob.float0 = m_gain[2];
AHRSPacket.Data[36] = ftob.byte0;
AHRSPacket.Data[37] = ftob.byte1;
AHRSPacket.Data[38] = ftob.byte2;
AHRSPacket.Data[39] = ftob.byte3;
ftob.float0 = m_gain[3];
AHRSPacket.Data[40] = ftob.byte0;
AHRSPacket.Data[41] = ftob.byte1;
AHRSPacket.Data[42] = ftob.byte2;
AHRSPacket.Data[43] = ftob.byte3;
ftob.float0 = m_filter_type;
AHRSPacket.Data[44] = ftob.byte0;
AHRSPacket.Data[45] = ftob.byte1;
AHRSPacket.Data[46] = ftob.byte2;
AHRSPacket.Data[47] = ftob.byte3;
ftob.float0 = m_convolution_time;
AHRSPacket.Data[48] = ftob.byte0;
AHRSPacket.Data[49] = ftob.byte1;
AHRSPacket.Data[50] = ftob.byte2;
AHRSPacket.Data[51] = ftob.byte3;
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.SET_OUTPUT)
{
AHRSPacket.PacketType = PID[(int)PName.SET_OUTPUT];
AHRSPacket.DataLength = 24;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
AHRSPacket.Data[0] = (byte)((m_output_enable >> 8) & 0x0FF);
AHRSPacket.Data[1] = (byte)(m_output_enable & 0x0FF);
AHRSPacket.Data[2] = (byte)((m_output_rate >> 8) & 0x0FF);
AHRSPacket.Data[3] = (byte)(output_rate & 0x0FF);
AHRSPacket.Data[4] = (byte)((m_badurate >> 8) & 0x0FF);
AHRSPacket.Data[5] = (byte)(m_badurate & 0x0FF);
AHRSPacket.Data[6] = (byte)((m_port >> 8) & 0x0FF);
AHRSPacket.Data[7] = (byte)(m_port & 0x0FF);
for(int i =0; i<=14; i ++)
{
if (i <= (m_ip.Length-1))
AHRSPacket.Data[i+8] = (byte)m_ip[i];
else
AHRSPacket.Data[i+8]=(byte)' ' ;
}
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.WRITE_TO_FLASH)
{
AHRSPacket.PacketType = PID[(int)PName.WRITE_TO_FLASH];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.CALIBRATE_GYRO_BIAS)
{
AHRSPacket.PacketType = PID[(int)PName.CALIBRATE_GYRO_BIAS];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.UPDATE_FW)
{
AHRSPacket.PacketType = PID[(int)PName.UPDATE_FW];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
else if (index == (int)StateName.GET_CONFIGURATION)
{
AHRSPacket.PacketType = PID[(int)PName.GET_CONFIGURATION];
AHRSPacket.DataLength = 0;
AHRSPacket.Data = new byte[AHRSPacket.DataLength];
sendPacket(AHRSPacket);
}
return true;
}
/* **********************************************************************************
*
* Function: private bool sendPacket
* Inputs: Packet AHRSPacket - the packet to be transmitted
* Outputs: None
* Return Value: bool success
* Dependencies: None
* Description:
*
* Sends the specified packet to the AHRS.
*
* Returns 'true' on success, 'false' otherwise
*
* *********************************************************************************/
private bool sendPacket(Packet AHRSPacket)
{
int i;
UInt16 checksum;
if (!connected)
return false;
byte[] packet = new byte[AHRSPacket.DataLength + 120];
// Build packet header
packet[0] = (byte)'s';
packet[1] = (byte)'n';
packet[2] = (byte)'p';
packet[3] = AHRSPacket.PacketType;
packet[4] = AHRSPacket.DataLength;
// Fill data section
for (i = 0; i < AHRSPacket.DataLength; i++)
{
packet[5 + i] = AHRSPacket.Data[i];
}
// Compute Checksum
checksum = ComputeChecksum( packet, 5 + AHRSPacket.DataLength);
// Add checksum to end of packet
packet[5 + AHRSPacket.DataLength] = (byte)(checksum >> 8);
packet[6 + AHRSPacket.DataLength] = (byte)(checksum & 0x0FF);
packet[7+AHRSPacket.DataLength] = (byte)('\n');
//packet[8 + AHRSPacket.DataLength] = (byte)('\r');
// Now write the packet to the serial port
try
{
serialPort.Write(packet, 0,( AHRSPacket.DataLength + 120));
serialPort.BaseStream.Flush();
PacketSentEvent((PName)getTypeIndex(AHRSPacket.PacketType), 0);
System.Threading.Thread.Sleep(1000);
}
catch
{
return false;
}
return true;
}