#include "xc.h" #include "config.h" #include "extern.h" #include "CalibrationConstants.h" #include "conv.h" #include "imu.h" // TREFOIL CONTROLLER DATA PACKET DEFINITIONS. SRC_ID = 4 /************************************************************************************ * Data_ID = 0 * Byte 0-1 <-- "PowerLoss Timeout" (unsigned int) xxxxx * Byte 2-3 <-- "Tether Voltage" (signed int) 1cnt = 0.01V xx.xx * Byte 4-5 <-- "Batt Voltage" (signed int) 1cnt = 0.01V xx.xx * Byte 6-7 <-- "Pressure (psi)" (signed int) 1cnt = 0.005psi xx.xx * * Data_ID = 1 * Byte 0-1 <-- "qtn[0]" (signed int) 1cnt = 1/32767 x.xxxxx * Byte 2-3 <-- "qtn[1]" (signed int) 1cnt = 1/32767 x.xxxxx * Byte 4-5 <-- "qtn[2]" (signed int) 1cnt = 1/32767 x.xxxxx * Byte 6-7 <-- "qtn[3]" (signed int) 1cnt = 1/32767 x.xxxxx * * Data_ID = 2 * Byte 0-1 <-- "mag[0]" (signed int) 1cnt = 1/32767 Gauss x.xxxxx * Byte 2-3 <-- "mag[1]" (signed int) 1cnt = 1/32767 Gauss x.xxxxx * Byte 4-5 <-- "mag[2]" (signed int) 1cnt = 1/32767 Gauss x.xxxxx * Byte 6-7 <-- "Status" (unsigned int) No Scaling xxxxx * * Data_ID = 3 * Byte 0-1 <-- "ang_rate[0]" (signed int) 1cnt = 1 milli-rad/sec xxxxx * Byte 2-3 <-- "ang_rate[1]" (signed int) 1cnt = 1 milli-rad/sec xxxxx * Byte 4-5 <-- "ang_rate[2]" (signed int) 1cnt = 1 milli-rad/sec xxxxx * Byte 6-7 <-- "VpeStatus" (unsigned int) No Scaling xxxxx * * Data_ID = 4 * Byte 0-1 <-- "accel[0]" (signed int) 1cnt = 1 mm/sec^2 xxxxx * Byte 2-3 <-- "accel[1]" (signed int) 1cnt = 1 mm/sec^2 xxxxx * Byte 4-5 <-- "accel[2]" (signed int) 1cnt = 1 mm/sec^2 xxxxx * Byte 6-7 <-- "CommsLoss Timeout" (usigned int) No Scaling xxxxx * * Data_ID = 5 * Byte 0-1 <-- "Maxon Status" (unsigned int) No Scaling xxxxx * Byte 2-3 <-- "Motor Curr (mA)" (signed int) 1cnt = 1 mA xxxxx * Byte 4-5 <-- "Enc Pos (Low Word)"(signed int) Bytes 4-7 combine to represent one 32 bit * Byte 6-7 <-- "Enc Pos (High Word)"(signed int) signed integer, little endian xxxxxxxxxx * ************************************************************************************/ void SendCANPackets(IMU_Packet *IMU_Data) { int DataID; int arg1, arg2, arg3, arg4; static int seq_num = 0; //Send data over CAN DataID = 0; if (!C1TR01CONbits.TXREQ0) //If previous request for buffer zero isn't still waiting { if (raw == 0) { arg1 = power_loss_timeout; arg2 = (int) 100*ConvertFractToEngUnits(TetherVoltage_Buf, TetherVoltage_cnt, TetherVoltage_eng); //Reports in .01V arg3 = (int) 100*ConvertFractToEngUnits(BattVoltage_Buf, BattVoltage_cnt, BattVoltage_eng); //Reports in .01V arg4 = (int) (200 * ConvertFractToEngUnits(Pressure_Buf, Pressure_cnt, Pressure_eng)); } else { arg1 = power_loss_timeout; arg2 = TetherVoltage_Buf; arg3 = BattVoltage_Buf; arg4 = Pressure_Buf; } AssembleMessageData_FourInts(&message[DataID],arg1,arg2,arg3,arg4); message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num); AssembleCAN_Packet(DataID, &message[DataID]); C1TR01CONbits.TXREQ0 = 1; //Initiates a request } DataID = 1; if (!C1TR01CONbits.TXREQ1) //If previous request for this buffer isn't still waiting { arg1 = Q15(IMU_Data->qtn[0]); arg2 = Q15(IMU_Data->qtn[1]); arg3 = Q15(IMU_Data->qtn[2]); arg4 = Q15(IMU_Data->qtn[3]); AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4); message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num); AssembleCAN_Packet(DataID, &message[DataID]); C1TR01CONbits.TXREQ1 = 1; //Initiates a request } DataID = 2; if (!C1TR23CONbits.TXREQ2) //If previous request for this buffer isn't still waiting { arg1 = Q15(IMU_Data->mag[0]); //Gauss (Q15) arg2 = Q15(IMU_Data->mag[1]); arg3 = Q15(IMU_Data->mag[2]); arg4 = Status.status; AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4); message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num); AssembleCAN_Packet(DataID, &message[DataID]); C1TR23CONbits.TXREQ2 = 1; //Initiates a request } DataID = 3; if (!C1TR23CONbits.TXREQ3) //If previous request for this buffer isn't still waiting { arg1 = (signed int) (1000 * IMU_Data->ang_rate[0]); //milli-rad/sec^2 arg2 = (signed int) (1000 * IMU_Data->ang_rate[1]); arg3 = (signed int) (1000 * IMU_Data->ang_rate[2]); arg4 = IMU_Data->VpeStatus; AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4); message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num); AssembleCAN_Packet(DataID, &message[DataID]); C1TR23CONbits.TXREQ3 = 1; //Initiates a request } DataID = 4; if (!C1TR45CONbits.TXREQ4) //If previous request for this buffer isn't still waiting { arg1 = (signed int) (1000 * IMU_Data->accel[0]); //mm/sec^2 arg2 = (signed int) (1000 * IMU_Data->accel[1]); arg3 = (signed int) (1000 * IMU_Data->accel[2]); arg4 = comms_loss_timeout; AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4); message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num); AssembleCAN_Packet(DataID, &message[DataID]); C1TR45CONbits.TXREQ4 = 1; //Initiates a request } DataID = 5; if (!C1TR45CONbits.TXREQ5) //If previous request for this buffer isn't still waiting { arg1 = StatusWord; //Maxon Status word arg2 = MotorCurrentActualValue; //Maxon Motor Current, Reporting only low word since we won't ever be up in the range of +/-32Amps plus with this drive. arg3 = (unsigned int) (TreFoilPositionActualValue & 0xffff); //Low Word arg4 = (unsigned int) ((TreFoilPositionActualValue & 0xffff0000) >> 16); //High Word AssembleMessageData_FourInts(&message[DataID], arg1, arg2, arg3, arg4); message[DataID].id = Assemble_packetID(CAN_SRCID, DataID, seq_num); AssembleCAN_Packet(DataID, &message[DataID]); C1TR45CONbits.TXREQ5 = 1; //Initiates a request } seq_num++; //Increment sequence number for next time if (seq_num >= (1 << SEQ_WIDTH)) seq_num = 0; //Rollover sequence number when it exceeds specified width }