File content as of revision 6:f9c4795448c1:

 #include "RoboteqController.h"

const uint16_t RoboteqController::RPDO_ID[4] = {0x200, 0x300, 0x400, 0x500};   // RPDO msg IDs to send data to the controller

RoboteqController::RoboteqController(){
}

RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, bool TPDO_enabled) : 
                    PeripheralCAN(controller),
                    node_id(node_id),
                    Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id),
                    Id_TPDO1(ID_TPDO1+node_id),Id_TPDO2(ID_TPDO2+node_id), Id_TPDO3(ID_TPDO3+node_id), Id_TPDO4(ID_TPDO4+node_id) {
    PeripheralCAN::addIdRead(&Id_CSS_REQ);
    PeripheralCAN::addIdRead(&Id_CSS_ANS);
    
    // If TPDO is enabled, make TPDO message readable
    if(TPDO_enabled){
        PeripheralCAN::addIdRead(&Id_TPDO1);
        PeripheralCAN::addIdRead(&Id_TPDO2);
        PeripheralCAN::addIdRead(&Id_TPDO3);
        PeripheralCAN::addIdRead(&Id_TPDO4);
    }
}

RoboteqController::~RoboteqController(){}
    

void RoboteqController::update(const unsigned short& Id, const CANMessage& msg){
    SDOMessage SDOmsg;
    
    /* Treatment of the controller answer */
    if(Id == Id_CSS_ANS){
        CANMsg2SDO(msg, SDOmsg);
        
        switch(SDOmsg.idx){
            case qBS: // Brushless speed in rpm
                if(SDOmsg.subindex == 1) speed1 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
                else if(SDOmsg.subindex == 2) speed2 = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
            break;
            //case 
            //..
        }
    }
    else if (Id == Id_TPDO1){
        TPDO_receive(1, msg);
    }
    else if (Id == Id_TPDO2){
        TPDO_receive(2, msg);
    }
    else if (Id == Id_TPDO3){
        TPDO_receive(3, msg);
    }
    else if (Id == Id_TPDO4){
        TPDO_receive(4, msg);
    }
}

void RoboteqController::SDO_query(uint8_t n, uint16_t idx, uint8_t sub_idx){
    char buffer[8];
    
    buffer[0] = (CSS_QUERY << 4) + (n << 2);
    buffer[1] = idx;
    buffer[2] = (idx >> 8);
    buffer[3] = sub_idx;
    
    // Clear bytes 4-7 because no data are  uploaded (it's a query)
    for(int i = 4 ; i < 7 ; i++){
        buffer[i] = 0;
    }
    
    writeOnCAN(Id_CSS_REQ, buffer, 8);
}

void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
    char msg[8];
    
    msg[0] = (CSS_CMD << 4) + (n << 2);
    msg[1] = idx;
    msg[2] = (idx >> 8);
    msg[3] = sub_idx;
    
    // Load bytes 4-7 from data array
    for(int i = 0 ; i < 4-n ; i++){
        msg[4+i] = *(data+i);
    }
    
    writeOnCAN(Id_CSS_REQ, msg, 8);
}

void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
    SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
    SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
    SDOmsg.idx = CANmsg.data[1] + (CANmsg.data[2] << 8);
    SDOmsg.subindex = CANmsg.data[3];
    SDOmsg.data[0] = CANmsg.data[4];
    SDOmsg.data[1] = CANmsg.data[5];
    SDOmsg.data[2] = CANmsg.data[6];
    SDOmsg.data[3] = CANmsg.data[7];
}

void RoboteqController::RPDO_send(uint8_t n, int32_t user_var1, int32_t user_var2){
    char buffer[8];
    
    buffer[0] = user_var1;
    buffer[1] = (user_var1 >> 8);
    buffer[2] = (user_var1 >> 16);
    buffer[3] = (user_var1 >> 24);
    
    buffer[4] = user_var2;
    buffer[5] = (user_var2 >> 8);
    buffer[6] = (user_var2 >> 16);
    buffer[7] = (user_var2 >> 24);
    
    writeOnCAN(RPDO_ID[n-1] + node_id, buffer, 8);
}

void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2){
    *p_user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
    *p_user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
}

void RoboteqController::TPDO_receive(uint8_t n, const CANMessage& CANmsg){
    /* Nothing to do, use the following structure in a redefinition :
    if (n == 1){
        // User defined section
    }
    else if (n == 2){
        // User defined section
    }
    else if (n == 3){
        // User defined section
    }
    else if (n == 4){
        // User defined section
    }
    */
}


void RoboteqController::sendCommand(uint8_t mot_num, uint32_t cmd){
    uint8_t data[4];
    
    data[0] = cmd;
    data[1] = (cmd>>8);
    data[2] = (cmd>>16);
    data[3] = (cmd>>24);
    
    SDO_command(0, cCANGO, mot_num, data);
}