File content as of revision 2:d61b4a989cab:

#include "RoboteqController.h"

RoboteqController::RoboteqController(){
}

RoboteqController::RoboteqController(ControllerCAN* controller, uint8_t node_id, uint8_t mot_num) : 
                    PeripheralCAN(controller),
                    node_id(node_id), mot_num(mot_num),
                    Id_CSS_REQ(SDO_REQUEST+node_id), Id_CSS_ANS(SDO_ANSWER+node_id) {
    PeripheralCAN::addIdRead(&Id_CSS_REQ);
    PeripheralCAN::addIdRead(&Id_CSS_ANS);
}

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.index){
            case qBS: // Brushless speed in rpm
                rotor_speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
            break;
            //case 
            //..
        }
    }
}

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 buffer[8];
    
    buffer[0] = (CSS_CMD << 4) + (n << 2);
    buffer[1] = idx;
    buffer[2] = (idx >> 8);
    buffer[3] = sub_idx;
    
    // Load bytes 4-7 from data array
    for(int i = 0 ; i < 4-n ; i++){
        buffer[4+i] = *(data+i);
    }
    
    writeOnCAN(Id_CSS_REQ, buffer, 8);
}

void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
    SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
    SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
    SDOmsg.index = 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];
}