[HAL] Collect Telemetry from PDP

[ncharlton02]

The HAL should collect voltage/current information about each channel from the PDP.

[ncharlton02]

The CAN API is not documented anywhere to my knowledge, but the FRC implementation is here: https://github.com/wpilibsuite/allwpilib/blob/main/hal/src/main/native/athena/CTREPDP.cpp

[WangQiHao-Charlie]

I read through the code, but I found that static analysis of the code may not be enough for us to implement the features mentioned in the issue. Here are some of my findings and why I believe simply static analysis may not be sufficient.

How to extract current information from this data:

Data Structure:

PdpStatus1:

Contains current data for channels 1 to 6.

PdpStatus2:

Contains current data for channels 7 to 12.

PdpStatus3:

Contains current data for channels 13 to 16, and includes additional battery resistance, bus voltage, and temperature data.

union PdpStatus1 {
  uint8_t data[8];
  struct Bits {
    unsigned chan1_h8 : 8;
    unsigned chan2_h6 : 6;
    unsigned chan1_l2 : 2;
    unsigned chan3_h4 : 4;
    unsigned chan2_l4 : 4;
    unsigned chan4_h2 : 2;
    unsigned chan3_l6 : 6;
    unsigned chan4_l8 : 8;
    unsigned chan5_h8 : 8;
    unsigned chan6_h6 : 6;
    unsigned chan5_l2 : 2;
    unsigned reserved4 : 4;
    unsigned chan6_l4 : 4;
  } bits;
};

union PdpStatus2 {
  uint8_t data[8];
  struct Bits {
    unsigned chan7_h8 : 8;
    unsigned chan8_h6 : 6;
    unsigned chan7_l2 : 2;
    unsigned chan9_h4 : 4;
    unsigned chan8_l4 : 4;
    unsigned chan10_h2 : 2;
    unsigned chan9_l6 : 6;
    unsigned chan10_l8 : 8;
    unsigned chan11_h8 : 8;
    unsigned chan12_h6 : 6;
    unsigned chan11_l2 : 2;
    unsigned reserved4 : 4;
    unsigned chan12_l4 : 4;
  } bits;
};

union PdpStatus3 {
  uint8_t data[8];
  struct Bits {
    unsigned chan13_h8 : 8;
    unsigned chan14_h6 : 6;
    unsigned chan13_l2 : 2;
    unsigned chan15_h4 : 4;
    unsigned chan14_l4 : 4;
    unsigned chan16_h2 : 2;
    unsigned chan15_l6 : 6;
    unsigned chan16_l8 : 8;
    unsigned internalResBattery_mOhms : 8;
    unsigned busVoltage : 8;
    unsigned temp : 8;
  } bits;
};

Data Decoding Process:

Read CAN Data:

Receive data from the CAN bus via the HAL_ReadCANPacketTimeout function, and the data is saved in the data array.

Parsing Data:

Extract the current data field from the CAN data using bit fields (e.g. chan1_h8, chan1_l2). Combine the high and low bit fields into a complete current value.

Calculate the actual current:

Multiply the combined raw data by 0.125 to get the actual current value in amperes.

What to do next:

Even though I have already figured out how to decode the data, we still can't tell starting which byte is the PDP voltage and current data. We may not have direct access to the implementation details of the underlying FRC CAN controller, especially in FRC's PDP, the actual data transmission and format may be determined by the CAN controller's provider's implementation, which may not be visible. In this case, static analysis and assumptions may not be sufficient to ensure that we can correctly decode and process the CAN data.

What we can do is:

1. Use a CAN Debugger to get the bytes
2. Add debug output in our existing system

Here's the proof:

Here's the implementation of HAL_ReadCANPacketTimeout

void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
                              uint8_t* data, int32_t* length,
                              uint64_t* receivedTimestamp, int32_t timeoutMs,
                              int32_t* status) {
  auto can = canHandles->Get(handle);
  if (!can) {
    *status = HAL_HANDLE_ERROR;
    return;
  }

  uint32_t messageId = CreateCANId(can.get(), apiId);
  uint8_t dataSize = 0;
  uint32_t ts = 0;
  HAL_CAN_ReceiveMessage(&messageId, 0x1FFFFFFF, data, &dataSize, &ts, status);

  std::scoped_lock lock(can->receivesMutex);
  if (*status == 0) {
    // fresh update
    auto& msg = can->receives[messageId];
    msg.length = dataSize;
    *length = dataSize;
    msg.lastTimeStamp = ts;
    *receivedTimestamp = ts;
    // The NetComm call placed in data, copy into the msg
    std::memcpy(msg.data, data, dataSize);
  } else {
    auto i = can->receives.find(messageId);
    if (i != can->receives.end()) {
      // Found, check if new enough
      uint32_t now = HAL_GetCANPacketBaseTime();
      if (now - i->second.lastTimeStamp > static_cast<uint32_t>(timeoutMs)) {
        // Timeout, return bad status
        *status = HAL_CAN_TIMEOUT;
        return;
      }
      // Read the data from the stored message into the output
      std::memcpy(data, i->second.data, i->second.length);
      *length = i->second.length;
      *receivedTimestamp = i->second.lastTimeStamp;
      *status = 0;
    }
  }
}

In which if we look at HAL_CAN_ReceiveMessage,

void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
                            uint8_t* data, uint8_t* dataSize,
                            uint32_t* timeStamp, int32_t* status) {
  FRC_NetworkCommunication_CANSessionMux_receiveMessage(
      messageID, messageIDMask, data, dataSize, timeStamp, status);
}

FRC_NetworkCommunication_CANSessionMux_receiveMessage is something defined here:

https://github.com/kylestach/frc-bazel/blob/master/third_party/wpilib/hal/lib/Athena/FRC_NetworkCommunication/CANSessionMux.h

which I believe is a standard interface defined by FRC, so that the manufacturer can implement it, and I doubt their implementations are visible on the internet.

[ncharlton02]

@WangQiHao-Charlie Thanks for looking into this!

Based on what you found there appears to be the three status messages (PdpStatus1, PdpStatus2, PdpStatus3). Each of these status messages are probably sent by the PDP with three corresponding IDs.

Each ID is made up of a device type, manufacturer ID, API class, API index, and device ID.

For the PDP, the device type will always be 0x08 and the manufacturer ID will always be 0x04 (Link).

The API class and API index seem to be combined into single fields and are defined here. The device ID is probably the default which I think is zero.

[WangQiHao-Charlie]

I just realized that I had a misunderstanding earlier. I initially thought that the PDP would push the voltage and current values, along with many other values, in each frame in an unknown order, which left me confused about how to identify which byte represents the current or voltage value. However, I’ve since thought it through and realized that these values are more likely pushed periodically. A few bits representing the API class indicate whether the data is the CAN current value or the voltage value.

Now that I understand the logic, I should be able to fix the issue.