communicator.hpp

Go to the documentation of this file.

#pragma once
 
#include <FlexCAN_T4.h>
#include <Arduino.h>
#include <string>
 
#include "comm/communicatorSettings.hpp"
#include "model/systemData.hpp"
#include "comm/utils.hpp"
#include "debugUtils.hpp"
 
inline Code fifoCodes[] = {
    {0, C1_ID},
    {1, BAMO_RESPONSE_ID},
    {2, AS_CU_EMERGENCY_SIGNAL},
    {3, MISSION_FINISHED},
    {4, AS_CU_ID},
    {5, RES_STATE},
    {6, RES_READY}
};
 
inline Code fifoExtendedCodes[] = {
    {7, STEERING_ID},
};
 
 
class Communicator {
private:
    // Static FlexCAN_T4 object for CAN2 interface with RX and TX buffer sizes specified
    inline static FlexCAN_T4<CAN2, RX_SIZE_256, TX_SIZE_16> can2;
 
public:
    // Pointer to SystemData instance for storing system-related data
    inline static SystemData *_systemData = nullptr;
 
    Communicator(SystemData* systemdata);
 
    void init();
 
    static void parse_message(const CAN_message_t& msg);
 
    static int send_message(unsigned len, const unsigned char* buffer, unsigned id);
 
 
    static void pcCallback(const uint8_t *buf);
    
    static void c1Callback(const uint8_t *buf);
 
    static void resStateCallback(const uint8_t *buf);
 
    static void resReadyCallback();
 
    static void bamocarCallback(const uint8_t *buf);
 
    static void steeringCallback();
 
 
    static int publish_state(int state_id);
 
    static int publish_mission(int mission_id);
 
    static int publish_left_wheel_rpm(double value);
};
 
inline Communicator::Communicator(SystemData* systemData) {
    _systemData = systemData;
}
 
void Communicator::init() {
    
    // Library initialization
    can2.begin();
    can2.setBaudRate(500000);
 
    // Enable FIFO
    can2.enableFIFO();
    can2.enableFIFOInterrupt();
    
    // Set filters
    can2.setFIFOFilter(REJECT_ALL);
    for (auto &fifoCode: fifoCodes)
        can2.setFIFOFilter(fifoCode.key, fifoCode.code, STD);
 
    // Set filters for extended
    for (auto &fifoExtendedCode: fifoExtendedCodes)
        can2.setFIFOFilter(fifoExtendedCode.key, fifoExtendedCode.code, EXT);
    
    // Set callback
    can2.onReceive(FIFO, parse_message);
 
    DEBUG_PRINT("CAN2 started");
    can2.mailboxStatus(); // Prints CAN mailbox info
}
 
inline void Communicator::c1Callback(const uint8_t *buf) {
  if (buf[0] == HYDRAULIC_LINE) {
    _systemData->sensors._hydraulic_line_pressure = (buf[2] << 8) | buf[1];
    // DEBUG_PRINT_VAR(_systemData->sensors._hydraulic_line_pressure);
  } else if (buf[0] == RIGHT_WHEEL_CODE) {
    double right_wheel_rpm = (buf[4] << 24) | (buf[3] << 16) | (buf[2] << 8) | buf[1];
    right_wheel_rpm *= WHEEL_PRECISION; // convert back adding decimal part
    _systemData->sensors._right_wheel_rpm = right_wheel_rpm;
    // DEBUG_PRINT_VAR(_systemData->sensors._right_wheel_rpm);
  } else if (buf[0] == LEFT_WHEEL_CODE) {
    double left_wheel_rpm = (buf[4] << 24) | (buf[3] << 16) | (buf[2] << 8) | buf[1];
    left_wheel_rpm *= WHEEL_PRECISION; // convert back adding decimal part
    _systemData->sensors._left_wheel_rpm = left_wheel_rpm;
    // DEBUG_PRINT_VAR(_systemData->sensors._left_wheel_rpm);
  }
}
 
inline void Communicator::resStateCallback(const uint8_t *buf) {
    bool emg_stop1 = buf[0] & 0x01;
    bool emg_stop2 = buf[3] >> 7 & 0x01;
    bool go_switch = (buf[0] >> 1) & 0x01;
    bool go_button = (buf[0] >> 2) & 0x01;
  
    // DEBUG_PRINT("Received message from RES");
 
    // DEBUG_PRINT_VAR(emg_stop1);
    // DEBUG_PRINT_VAR(emg_stop2);
    // DEBUG_PRINT_VAR(go_switch);
    // DEBUG_PRINT_VAR(go_button);
 
    if (go_button || go_switch)
        _systemData->r2dLogics.processGoSignal();
    else if (!emg_stop1 && !emg_stop2) {
        DEBUG_PRINT("RES Emergency Signal");
        _systemData->failureDetection.emergencySignal = true;
    }
 
    _systemData->failureDetection.radio_quality = buf[6];
    // DEBUG_PRINT_VAR(_systemData->failureDetection.radio_quality);
    bool signal_loss = (buf[7] >> 6) & 0x01;
    if (!signal_loss) {
        _systemData->failureDetection.resSignalLossTimestamp.reset();
    } else {
        // Too many will violate the disconnection time limit
        DEBUG_PRINT("SIGNAL LOSS"); 
    }
 
    // DEBUG_PRINT_VAR(_systemData->failureDetection.emergencySignal);
    // DEBUG_PRINT_VAR(_systemData->r2dLogics.r2d);
    // DEBUG_PRINT_VAR(_systemData->failureDetection.radio_quality);
}
 
inline void Communicator::resReadyCallback() {
    // If res sends boot message, activate it
    // DEBUG_PRINT("Received RES Ready");
    unsigned id = RES_ACTIVATE;
    uint8_t msg[] = {0x01, NODE_ID}; // 0x00 in byte 2 for all nodes
 
    send_message(2, msg, id);
}
 
inline void Communicator::bamocarCallback(const uint8_t *buf) {
    _systemData->failureDetection.inversorAliveTimestamp.reset();
    // DEBUG_PRINT("Received Bamocar Alive");
 
    if (buf[0] == BTB_READY) {
        if (buf[1] == false) {
            _systemData->failureDetection.ts_on = false;
        }
 
    } else if (buf[0] == VDC_BUS) {
        int dc_voltage = (buf[2] << 8) | buf[1];
 
        // DEBUG_PRINT_VAR(dc_voltage);
 
        if (dc_voltage < DC_THRESHOLD) {
            _systemData->failureDetection.ts_on = false;
        } else {
            _systemData->failureDetection.ts_on = true;       
        }
    }   
    // DEBUG_PRINT_VAR(_systemData->failureDetection.ts_on);
 
}
 
inline void Communicator::pcCallback(const uint8_t *buf) {
    if (buf[0] == PC_ALIVE) {
        _systemData->failureDetection.pcAliveTimestamp.reset();
        // DEBUG_PRINT("Received AS CU Alive");
    } else if (buf[0] == MISSION_FINISHED) {
        _systemData->missionFinished = true;
        // DEBUG_PRINT_VAR(_systemData->missionFinished);
    } else if (buf[0] == AS_CU_EMERGENCY_SIGNAL) {
        _systemData->failureDetection.emergencySignal = true;
        DEBUG_PRINT_VAR(_systemData->failureDetection.emergencySignal);
    }
    
}
 
inline void Communicator::steeringCallback() {
    _systemData->failureDetection.steerAliveTimestamp.reset();
    // DEBUG_PRINT("Received Steering Alive");
}
 
inline void Communicator::parse_message(const CAN_message_t& msg) {
    switch(msg.id) {
        case AS_CU_ID:
            pcCallback(msg.buf);
        case RES_STATE:
            resStateCallback(msg.buf);
            break;
        case RES_READY:
            resReadyCallback();
            break;
        case C1_ID:
            c1Callback(msg.buf); // rwheel, lwheel and hydraulic line
            break;
        case BAMO_RESPONSE_ID:
            bamocarCallback(msg.buf);
            break;
        case STEERING_ID:
            steeringCallback();
            break;
        default:
            break;
    }
}
 
inline int Communicator::publish_state(const int state_id) {
    const uint8_t msg[] = {STATE_MSG, static_cast<uint8_t>(state_id)};
 
    // DEBUG_PRINT_VAR(state_id);
    send_message(2, msg, MASTER_ID);
    return 0;
}
 
inline int Communicator::publish_mission(int mission_id) {
    const uint8_t msg[] = {MISSION_MSG, static_cast<uint8_t>(mission_id)};
 
    send_message(2, msg, MASTER_ID);
    return 0;
}
 
inline int Communicator::publish_left_wheel_rpm(double value) {
    uint8_t msg[5];
    create_left_wheel_msg(msg, value);
    
    send_message(5, msg, MASTER_ID);
    return 0;
}
 
inline int Communicator::send_message(const unsigned len, const unsigned char* buffer, const unsigned id) {
    CAN_message_t can_message;
    can_message.id = id;
    can_message.len = len;
    for (unsigned i = 0; i < len; i++) {
        can_message.buf[i] = buffer[i];
    }
    can2.write(can_message);
 
    return 0;
}