private static void UpdateTelemetry(ArmPacket incomingPacket)
{
JointTelemetry tel = Telemetry[incomingPacket.TargetDeviceID];
switch (incomingPacket.PacketType)
{
case CANPacket.ENCODER_CNT:
tel.Encoder = UtilData.ToInt(incomingPacket.Payload);
ENCODER_CNT_FLG = false;
break;
case CANPacket.MODEL_NUM:
tel.ModelNumber = UtilData.ToInt(incomingPacket.Payload);
break;
case CANPacket.TELEMETRY:
// Break up into Voltage and Current
byte[] voltageData = incomingPacket.Payload.Take(2).ToArray();
byte[] currentData = incomingPacket.Payload.Skip(2).Take(2).ToArray();
tel.Current = UtilData.ToInt(currentData);
tel.Voltage = UtilData.ToInt(voltageData);
break;
case CANPacket.SERVO:
tel.ServoPos = UtilData.ToInt(incomingPacket.Payload);
break;
case CANPacket.ERROR_MSG:
tel.ErrorCode = incomingPacket.Payload[0];
break;
}
Telemetry[incomingPacket.TargetDeviceID] = tel;
}
public static void PacketGroundSensor(Packet Packet)
{
if (Packet == null || Packet.Data == null || Packet.Data.Payload == null || Packet.Data.Payload.Length != 40)
{
Log.Output(Log.Severity.WARNING, Log.Source.NETWORK, "Ground sensor packet invalid. Discarding. Length: " + Packet?.Data?.Payload?.Length);
return;
}
DateTime Timestamp = DateTime.Now;
int UVLight = UtilData.ToInt(UtilMain.SubArray(Packet.Data.Payload, 0, 4));
float AirQuality = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 4, 4));
float SoilMoist = UtilData.ToFloat(UtilMain.SubArray(Packet.Data.Payload, 8, 4));
uint ThermocoupleData = UtilData.ToUInt(UtilMain.SubArray(Packet.Data.Payload, 12, 4));
double AtmoTemp = UtilData.ToDouble(UtilMain.SubArray(Packet.Data.Payload, 16, 8));
double AtmoPres = UtilData.ToDouble(UtilMain.SubArray(Packet.Data.Payload, 24, 8));
double AtmoHumid = UtilData.ToDouble(UtilMain.SubArray(Packet.Data.Payload, 32, 8));
UV.Data.Add(new Datum <int>(Timestamp, UVLight));
AirPollution.Data.Add(new Datum <float>(Timestamp, AirQuality));
SoilMoisture.Data.Add(new Datum <float>(Timestamp, SoilMoist));
ThermoExt.Data.Add(new Datum <float>(Timestamp, MAX31855.ConvertExternalFromRaw(ThermocoupleData)));
ThermoInt.Data.Add(new Datum <float>(Timestamp, MAX31855.ConvertInternalFromRaw(ThermocoupleData)));
AirTemp.Data.Add(new Datum <double>(Timestamp, AtmoTemp));
AirPressure.Data.Add(new Datum <double>(Timestamp, AtmoPres));
AirHumidity.Data.Add(new Datum <double>(Timestamp, AtmoHumid));
}
public static void ParseServoPacket(Packet Packet)
{
if (CheckPacket(Packet, 5, "Servo Set"))
{
if (Packet.Data.Payload[0] == 0x00) // Sample Door
{
RoverMain.IOHandler.DrillController.DoorOpen = (UtilData.ToInt(UtilMain.SubArray(Packet.Data.Payload, 1, 4)) == 1);
}
}
}
public static void PacketTesting(Packet Packet)
{
if (Packet == null || Packet.Data == null || Packet.Data.Payload == null || Packet.Data.Payload.Length != 12)
{
Log.Output(Log.Severity.WARNING, Log.Source.NETWORK, "System sensor packet invalid. Discarding. Length: " + Packet?.Data?.Payload?.Length);
return;
}
int Enc = UtilData.ToInt(UtilMain.SubArray(Packet.Data.Payload, 0, 4));
DateTime Sample = new DateTime(UtilData.ToLong(UtilMain.SubArray(Packet.Data.Payload, 4, 8)));
}
/// <summary> Interperate next data block as int. </summary>
/// <returns> Next data. </returns>
public int NextInt()
{
return(UtilData.ToInt(NextData(sizeof(int))));
}
