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