ECUHandler.cpp

Go to the documentation of this file.

#include "ECUHandler.hpp"
#include "rclcpp/rclcpp.hpp"
#include <cstring>
#include <sstream>
#include <string>
#include <iostream>
#include <galaxis/servoSettings.hpp>
#include <galaxis/motorSettings.hpp>
#include <galaxis/Errors.hpp>
 
static const uint8_t gcu8_header[8] = { 0xDE, 0xAD, 0xF0, 0x0D, 0xDE, 0xAD, 0xBE, 0xEF };
 
uint32_t ECUHandler::calculateChecksum(const ComputerPacket& packet) {
  CRCType crc;
  crc.reset();
  /* compute crc without crc of packet */
  crc.process_bytes(&packet, sizeof(ComputerPacket) - sizeof(packet.crc));
 
  return crc.checksum();
}
 
uint32_t ECUHandler::calculateChecksum(const SettingsPacket& packet) {
  CRCType crc;
  crc.reset();
  /* compute crc without crc of packet */
  crc.process_bytes(&packet, sizeof(SettingsPacket) - sizeof(packet.crc));
 
  return crc.checksum();
}
 
void ECUHandler::init(PublishMeasurementsCallback cb) {
  cb_ = cb;
  InitializeCommunication();
  xlog("ECUHandler", "Successfully Initialised...");
}
 
void ECUHandler::TransmitPacket(ComputerPacket& packet) {
  memcpy(&(packet.preamble), gcu8_header, sizeof(packet.preamble));
  packet.packet_id = PACKET_ID_COMPUTER;
 
  packet.crc = calculateChecksum(packet);
 
  m_lastEcuPacket = packet;
 
  size_t written = m_pCommunication->VSendData(&packet, sizeof(ComputerPacket));
  if(written != sizeof(ComputerPacket)) {
    xlog("ECUHandler", "Could not write the whole computer packet");
  }
}
 
void ECUHandler::TransmitPacket(SettingsPacket& packet) {
  memcpy(&(packet.preamble), gcu8_header, sizeof(packet.preamble));
  packet.packet_id = PACKET_ID_SETTINGS;
 
  packet.crc = calculateChecksum(packet);
 
  size_t written = m_pCommunication->VSendData(&packet, sizeof(SettingsPacket));
  if(written != sizeof(SettingsPacket)) {
    xlog("ECUHandler", "Could not write the whole settings packet");
    /* TODO: ... */
    throw galaxis::Errors::UNDEFINED_ERROR();
  }
}
 
void ECUHandler::onNewPacketReceived(const EcuPacket* const packet) {
  if(this->cb_ == nullptr || packet == nullptr) {
    xlog("ECUHandler Error", "callback is a nullptr");
    /* TODO: go in failsafe mode */
    throw galaxis::Errors::POINTER_ERROR();
  }
 
  this->cb_(packet, packet_differences);
}
 
void ECUHandler::Stop() {
  receiverThreadRunning = false;
  if(receiverThread != nullptr)
    delete receiverThread;
}
 
void ECUHandler::InitializeCommunication() {
  xlog("ECUHandler", "INIT UART");
  m_pCommunication = std::shared_ptr<ICommunication>(new UART());
 
  if(m_pCommunication != nullptr) {
    m_pCommunication->VConnect();
    if(!m_pCommunication->IsConnected()) {
      xlog("ECUHandler", "Failed to open serial port to breakout");
      return;
    }
 
    receiverThreadRunning = true;
    receiverThread        = new std::thread(&ECUHandler::ReceiveHandler, this);
 
    while(!firstPacketReceived) {
      /* TODO: why do we sleep here for 1 ms? */
      std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
  }
}
 
void ECUHandler::ReceiveHandler() {
  CRCType crc;
  uint8_t buf[4096];
  uint8_t packet[sizeof(EcuPacket)];
 
  int packetIndex = 0;
 
  enum {
    RECEIVING_PREAMBLE,
    RECEIVING_DATA,
    CRC_UPDATE,
  } state = RECEIVING_PREAMBLE;
 
  while(receiverThreadRunning) {
    uint8_t byte;
    size_t bytesRead = m_pCommunication->VReceiveData(buf, sizeof(buf));
 
    for(size_t i = 0; i < bytesRead; i++) {
 
      byte = buf[i];
 
      switch(state) {
        case RECEIVING_PREAMBLE:
 
          packet[packetIndex] = byte;
          if(gcu8_header[packetIndex++] != byte) {
            packetIndex = 0;
          }
          if(packetIndex == 8)
            state = RECEIVING_DATA;
 
          break;
 
        case RECEIVING_DATA:
 
          packet[packetIndex++] = byte;
 
          if(packetIndex == sizeof(packet))
            state = CRC_UPDATE;
          break;
 
        default:
          /* TODO: errorhandler */
          return;
          break;
      }
 
      if(state == CRC_UPDATE) {
        const size_t begin_of_crc_sum = sizeof(packet) - 4;
        crc.reset();
        crc.process_bytes(packet, begin_of_crc_sum);
        const uint32_t computed_crc = crc.checksum();
 
        uint32_t packetCrc = packet[begin_of_crc_sum];
        packetCrc |= packet[begin_of_crc_sum + 1] << 8;
        packetCrc |= packet[begin_of_crc_sum + 2] << 16;
        packetCrc |= packet[begin_of_crc_sum + 3] << 24;
 
        if(packetCrc == computed_crc) {
          std::stringstream ss;
          //xlog("ECUHandler","Received new Packet");
 
          current_timestamp = std::chrono::steady_clock::now();
          if(firstPacketReceived) {
            std::chrono::duration<double, std::milli> diff_ms =
                (current_timestamp - last_timestamp);
            if(diff_ms.count() < 9.5 || diff_ms.count() > 10.5 /*ms*/) {
              //ss << "=====>Current difference is " << diff_ms.count() << "ms";
 
              //std::cout << ss.str() << std::endl;
            }
            packet_differences.at(difference_index) = diff_ms;
          }
          last_timestamp = current_timestamp;
 
          difference_index++;
          if(difference_index == packet_differences.size()) {
            difference_index = 0;
          }
 
          firstPacketReceived = true;
          memcpy(&currentPacket, packet, sizeof(currentPacket));
          onNewPacketReceived(&currentPacket);
          //std::thread(&ECUHandler::onNewPacketReceived, this, &currentPacket);
        } else {
          xlog("ECUHandler", "Wrong CRC checksum");
        }
 
        packetIndex = 0;
        state       = RECEIVING_PREAMBLE;
      }
    }
  }
}