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