VescCAN.cpp

Go to the documentation of this file.

/*
 * VescCAN.cpp
 *
 *  Created on: Aug 08, 2021
 *      Author: Vincent Manoukian (manoukianv@gmail.com)
 */
#include "target_constants.h"
#ifdef VESC
#include <VescCAN.h>
#include "ClassIDs.h"
#include "CRC.h"
 
bool VescCAN::crcTableInitialized = false;
std::array<uint16_t,256> VescCAN::crc16_tab __attribute__((section (CCRAM_SEC))); // Generate in ram to save some flash (512B)
 
// *****    static initializer for the VESC_1 instance (extend VESC_CAN) *****
 
bool VESC_1::inUse = false;
 
ClassIdentifier VESC_1::info = {
     .name = "VESC 1" ,
     .id=CLSID_MOT_VESC0
};
 
bool VESC_1::isCreatable() {
    return !VESC_1::inUse;
}
 
const ClassIdentifier VESC_1::getInfo() {
    return info;
}
 
// *****    static initializer for the VESC_2 instance (extend VESC_CAN) *****
 
bool VESC_2::inUse = false;
 
ClassIdentifier VESC_2::info = {
     .name = "VESC 2" ,
     .id=CLSID_MOT_VESC1
};
 
bool VESC_2::isCreatable() {
    return !VESC_2::inUse;
}
 
const ClassIdentifier VESC_2::getInfo() {
    return info;
}
 
// *****                     VESC_CAN                            *****
 
VescCAN::VescCAN(uint8_t address) :
        CommandHandler("vesc", CLSID_MOT_VESC0, address), Thread("VESC", VESC_THREAD_MEM,
                VESC_THREAD_PRIO) {
 
    if(!crcTableInitialized){ // Generate a CRC16 table the first time a vesc instance is created
        makeCrcTable(crc16_tab, crcpoly,16);
        crcTableInitialized = true;
    }
 
    setAddress(address);
    restoreFlash();
 
    setCanFilter();
 
    if(port->getSpeedPreset() < 3){
        port->setSpeedPreset(3); // Minimum 250k
    }
    this->port->takePort();
 
    registerCommands();
    this->Start();
 
}
 
VescCAN::~VescCAN() {
    this->stopMotor();
    this->port->freePort();
    this->state = VescState::VESC_STATE_UNKNOWN;
    this->port->removeCanFilter(filterId);
    this->port->removeCanFilter(filterIdVesc);
}
 
void VescCAN::setAddress(uint8_t address) {
    if (address == 0) {
        this->flashAddrs = VescFlashAddrs( { ADR_VESC1_CANID, ADR_VESC1_DATA, ADR_VESC1_OFFSET });
    } else if (address == 1) {
        this->flashAddrs = VescFlashAddrs( { ADR_VESC2_CANID, ADR_VESC2_DATA, ADR_VESC2_OFFSET });
    } else if (address == 2) {
        this->flashAddrs = VescFlashAddrs( { ADR_VESC3_CANID, ADR_VESC3_DATA, ADR_VESC3_OFFSET });
    }
}
 
bool VescCAN::setCanFilter(){
    if(filterId != -1){
        port->removeCanFilter(filterId);
        filterId = -1;
    }
    // Set up a filter to receive vesc commands
    CAN_filter filterConf;
    filterConf.extid = true;
    filterConf.buffer = 0;
    filterConf.filter_id = this->OFFB_can_Id;
    filterConf.filter_mask =  0xff;
    this->filterId = this->port->addCanFilter(filterConf); // Receive messages for this id
 
    if(filterIdVesc != -1){
        filterIdVesc = -1;
        port->removeCanFilter(filterIdVesc);
    }
 
    if(this->filterIdVesc == -1){
        filterConf.extid = true;
        filterConf.buffer = 0;
        filterConf.filter_id = VESC_can_Id == 0xff ? ((uint8_t)VescCANMsg::CAN_PACKET_POLL_ROTOR_POS) << 8 : VESC_can_Id;
        filterConf.filter_mask =  VESC_can_Id == 0xff ? 0xff00 : 0xff; // Filter pos update or vesc id
        this->filterIdVesc = this->port->addCanFilter(filterConf);
    }
 
    return this->filterId != -1 && this->filterIdVesc != -1;
}
 
void VescCAN::turn(int16_t power) {
    float torque = ((float) power / (float) 0x7fff);
    if(fabs(torque-lastTorque) > 0.01){
        pulseErrLed();
    }
    lastTorque = torque;
    this->setTorque(torque);
}
 
void VescCAN::stopMotor() {
    this->setTorque(0.0);
    activeMotor = false;
}
 
void VescCAN::startMotor() {
    activeMotor = true;
}
 
Encoder* VescCAN::getEncoder() {
    if (useEncoder)
        return static_cast<Encoder*>(this);
    else
        return MotorDriver::getEncoder();
}
 
bool VescCAN::hasIntegratedEncoder() {
    return useEncoder;
}
 
EncoderType VescCAN::getEncoderType(){
    if (useEncoder)
        return EncoderType::absolute;
    else
        return MotorDriver::getEncoder()->getEncoderType();
}
 
bool VescCAN::motorReady() {
    return state == VescState::VESC_STATE_READY;
}
 
void VescCAN::restoreFlash() {
    uint16_t dataFlash = 0;
 
    if (Flash_Read(flashAddrs.canId, &dataFlash)) {
        this->OFFB_can_Id = dataFlash & 0xFF;
        this->VESC_can_Id = (dataFlash >> 8) & 0xFF;
    }
 
    if (Flash_Read(flashAddrs.data, &dataFlash)) {
 
        //uint8_t canspd = dataFlash & 0b111;
        //this->baudrate = canspd;
 
        this->useEncoder = (dataFlash >> 3) & 0x1;
    }
 
    if (Flash_Read(flashAddrs.offset, &dataFlash)) {
        this->posOffset = (float) ((int16_t) dataFlash / 10000.0);
 
        this->posOffset = this->posOffset - (int) this->posOffset; // Remove the multi-turn value
        bool moreThanHalfTurn = fabs(this->posOffset) > 0.5 ? 1 : 0;
        if (moreThanHalfTurn) {
            // if delta is neg, turn is CCW, decrement multi turn pos... else increment it
            this->posOffset += (this->posOffset > 0) ? -1.0 : 1.0;
        }
    }
 
}
 
void VescCAN::saveFlash() {
    uint16_t dataFlash = 0;
 
    // save the OpenFFBoard Can Id in 0..7 and the Vesc_can_id in the 8..15
    dataFlash = (this->VESC_can_Id << 8) | this->OFFB_can_Id;
    Flash_Write(flashAddrs.canId, dataFlash);
 
    dataFlash = 0;
    //dataFlash |= (this->baudrate & 0b111); // set the baudrate in 0..2 bit
    dataFlash |= (this->useEncoder & 0x1) << 3;  // set the encoder use in bit 3
    Flash_Write(flashAddrs.data, dataFlash);
 
    saveFlashOffset();
}
 
void VescCAN::saveFlashOffset() {
 
    // store the -180°/180°offset
    float storedOffset = this->posOffset - (int) this->posOffset; // Remove the multi-turn value
    bool moreThanHalfTurn = fabs(storedOffset) > 0.5 ? 1 : 0;
    if (moreThanHalfTurn) {
        // if delta is neg, turn is CCW, decrement multi turn pos... else increment it
        storedOffset += (storedOffset > 0) ? -1.0 : 1.0;
    }
 
    uint16_t dataFlash = ((int16_t) (storedOffset * 10000) & 0xFFFF);
    Flash_Write(flashAddrs.offset, dataFlash);
 
}

void VescCAN::setPos(int32_t pos) {
    // Only change encoder count internally as offset
    posOffset = lastPos - ((float) pos / (float) getCpr());
 
    saveFlashOffset(); // save the new offset for next restart
}
 
float VescCAN::getPos_f() {
    if (state == VescState::VESC_STATE_READY) {
        this->askPositionEncoder();
    }
    return lastPos - posOffset;
}
 
int32_t VescCAN::getPos() {
    return getCpr() * getPos_f();
}
 
uint32_t VescCAN::getCpr() {
    return 0xFFFF;
}
 
void VescCAN::registerCommands() {
    CommandHandler::registerCommands();
    registerCommand("offbcanid", VescCAN_commands::offbcanid, "CAN id of OpenFFBoard Axis", CMDFLAG_GET | CMDFLAG_SET);
    registerCommand("vesccanid", VescCAN_commands::vesccanid, "CAN id of VESC", CMDFLAG_GET | CMDFLAG_SET);
    registerCommand("canspd", VescCAN_commands::canspd, "CAN baud ! for info", CMDFLAG_GET | CMDFLAG_SET); // Kept for backwards compatibility. Remove in the future
    registerCommand("errorflags", VescCAN_commands::errorflags, "VESC error state", CMDFLAG_GET);
    registerCommand("vescstate", VescCAN_commands::vescstate, "VESC state", CMDFLAG_GET);
    registerCommand("voltage", VescCAN_commands::voltage, "VESC supply voltage (mV)", CMDFLAG_GET);
    registerCommand("encrate", VescCAN_commands::encrate, "Encoder update rate", CMDFLAG_GET);
    registerCommand("pos", VescCAN_commands::pos, "VESC position", CMDFLAG_GET);
    registerCommand("torque", VescCAN_commands::torque, "Current VESC torque", CMDFLAG_GET);
    registerCommand("forceposread", VescCAN_commands::forcePosRead, "Force a position update", CMDFLAG_GET);
    registerCommand("useencoder", VescCAN_commands::useEncoder, "Enable VESC encoder", CMDFLAG_GET | CMDFLAG_SET);
    registerCommand("offset", VescCAN_commands::offset, "Get or set encoder offset", CMDFLAG_GET | CMDFLAG_SET);
}
 
CommandStatus VescCAN::command(const ParsedCommand &cmd,
        std::vector<CommandReply> &replies) {
 
    switch (static_cast<VescCAN_commands>(cmd.cmdId)) {
 
    case VescCAN_commands::offbcanid:
    {
        CommandStatus status = handleGetSet(cmd, replies, this->OFFB_can_Id);
        if(!setCanFilter()) status = CommandStatus::ERR;
        return status;
    }
    case VescCAN_commands::vesccanid:
    {
        CommandStatus status = handleGetSet(cmd, replies, this->VESC_can_Id);
        if(!setCanFilter()) status = CommandStatus::ERR;
        return status;
    }
    case VescCAN_commands::errorflags:
        if (cmd.type == CMDtype::get)
            replies.emplace_back(vescErrorFlag);
        break;
    case VescCAN_commands::canspd:
        if (cmd.type == CMDtype::get) {
            replies.emplace_back(port->getSpeedPreset());
        } else if (cmd.type == CMDtype::set) {
            port->setSpeedPreset(std::max<uint8_t>(3,cmd.val));
        }
        break;
    case VescCAN_commands::vescstate:
        if (cmd.type == CMDtype::get)
            replies.emplace_back((uint32_t) this->state);
        break;
    case VescCAN_commands::encrate:
        if (cmd.type == CMDtype::get)
            replies.emplace_back((uint32_t) this->encRate);
        break;
    case VescCAN_commands::pos:
        if (cmd.type == CMDtype::get) {
            replies.push_back(
                    CommandReply(
                            (int32_t) ((lastPos - posOffset) * 1000000000)));
        }
        break;
    case VescCAN_commands::torque:
        if (cmd.type == CMDtype::get) {
            replies.emplace_back((int32_t) (lastTorque * 10000));
        }
        break;
    case VescCAN_commands::forcePosRead:
        this->askPositionEncoder();
        break;
    case VescCAN_commands::useEncoder:
        if (cmd.type == CMDtype::get) {
            replies.emplace_back(useEncoder ? 1 : 0);
        } else if (cmd.type == CMDtype::set) {
            useEncoder = cmd.val != 0;
        }
        break;
    case VescCAN_commands::offset: {
        if (cmd.type == CMDtype::get) {
            replies.emplace_back((int32_t) (posOffset * 10000));
        } else if (cmd.type == CMDtype::set) {
            posOffset = (float) cmd.val / 10000.0;
            this->saveFlashOffset(); // Really save to flash?
        }
    }
        break;
    case VescCAN_commands::voltage:
        if (cmd.type == CMDtype::get)
            replies.emplace_back(voltage * 1000);
        break;
 
    default:
        return CommandStatus::NOT_FOUND;
    }
 
    return CommandStatus::OK;
}
 
/*
 * state    = VESC_UNKOWN   => ping until the state go to PONG
 *          = VESC_PONG     => call a status message
 *          = VESC_ERROR    => call a status message
 *
 *  if error => state = VESC_ERROR
 *     else=> state = VESC_RESPOND
 *
 *  if RespondVescTime > 100 => VESC_NOT_RESPOND
 *
 */
void VescCAN::Run() {
 
    while (true) {
        Delay(500);
        // if status is UNKNOW, ask firmware info and wait next 500ms
        if (state == VescState::VESC_STATE_UNKNOWN) {
            this->getFirmwareInfo();
            // the return of getFirmwareInfo change the state to COMPATIBLE/INCOMPATIBLE
            // skip the follow control if we don't change from unknown state
            continue;
        }
 
        // if vesc is compatible, ready or in error => check the status
        if (state >= VescState::VESC_STATE_COMPATIBLE && !port->isWaiting()) {
            this->askGetValue();
            // the return of askGetValue put the state in READY or ERROR is vesc respond to status
        }
 
        // test is a ready/error vesc is always respond
        uint32_t period = HAL_GetTick() - lastVescResponse;
        if ((state == VescState::VESC_STATE_READY
                || state == VescState::VESC_STATE_ERROR) && period > 1000) {
            state = VescState::VESC_STATE_UNKNOWN;
        }
 
        // when motor is active we call vesc each 500ms, to disable the vesc watchdog
        // (if vesc not received message in 1sec, it cut off motor)
        if (lastTorque != 0.0 && activeMotor
                && state == VescState::VESC_STATE_READY && !port->isWaiting()) {
            this->setTorque(lastTorque);
        }
 
        // compute encoderRate each second
        period = HAL_GetTick() - encStartPeriod;
        if (period > 1000) {
            encRate = encCount / (period / 1000.0);
            encCount = 0;
            encStartPeriod = HAL_GetTick();
        }
 
    }
}
 
/************************************************************************************************
 *
 *                                      CAN SECTION
 */
 

void VescCAN::getFirmwareInfo() {
    uint8_t msg[3];
    msg[0] = this->OFFB_can_Id;
    msg[1] = 0x00;                              // ask vesc to process the msg
    msg[2] = (uint8_t) VescCmd::COMM_FW_VERSION;// sub action is to get FW version
 
    this->sendMsg((uint8_t) VescCANMsg::CAN_PACKET_PROCESS_SHORT_BUFFER, msg,
            sizeof(msg));
}

void VescCAN::sendPing() {
    uint8_t buffer[1];
    buffer[0] = this->OFFB_can_Id;
 
    this->sendMsg((uint8_t) VescCANMsg::CAN_PACKET_PING, buffer,
            sizeof(buffer));
}

void VescCAN::setTorque(float torque) {
 
    // Check if vesc CAN is ok, if it is, send a message
    if (!this->motorReady() || !activeMotor)
        return;
 
    uint8_t buffer[4];
    int32_t send_index = 0;
    this->buffer_append_float32(buffer, torque, 1e5, &send_index);
 
    this->sendMsg((uint8_t) VescCANMsg::CAN_PACKET_SET_CURRENT_REL, buffer,
            sizeof(buffer));
 
}
 
/*
 * Get the telemetry : Status & Value
 */
void VescCAN::askGetValue() {
    uint8_t msg[8];
    msg[0] = this->OFFB_can_Id;
    msg[1] = 0x00;                  // ask vesc to process the msg
    msg[2] = (uint8_t) VescCmd::COMM_GET_VALUES_SELECTIVE;// sub action is COMM_GET_VALUES_SELECTIVE : ask wanted data
    uint32_t request = ((uint32_t) 1 << 15); // bit 15 = ask vesc status (1byte)
    request |= ((uint32_t) 1 << 8);              // bit 8  = ask voltage (2byte)
 
    int32_t index = 3;
    this->buffer_append_uint32(msg, request, &index);
 
    this->sendMsg((uint8_t) VescCANMsg::CAN_PACKET_PROCESS_SHORT_BUFFER, msg,
            sizeof(msg));
}

void VescCAN::askPositionEncoder() {
    this->sendMsg((uint8_t) VescCANMsg::CAN_PACKET_POLL_ROTOR_POS, nullptr, 0);
}
 
/*
 * Compute the new position (laspos) value from data received by vesc:
 *      newPos is a 0-360° angle position
 *      if delta with previous and new position is > 180° then we count 1 turn
 *      at 1kz, more than 180° mean 30000rpm, not admissible in simracing
 */
void VescCAN::decodeEncoderPosition(const float newPos) {
    float delta = newPos - prevPos360; // Compute the delta position
 
    bool moreThanHalfTurn = fabs(delta) > 180.0 ? 1 : 0;
    if (moreThanHalfTurn) {
        // if delta is neg, turn is CCW, decrement multi turn pos... else increment it
        mtPos += (delta > 0) ? -1.0 : 1.0;
    }
 
    lastPos = (newPos / 360.0) + mtPos; // normalize the position
    prevPos360 = newPos;
 
    encCount++;
}

void VescCAN::sendMsg(uint8_t cmd, uint8_t *buffer, uint8_t len) {
    CAN_tx_msg msg;
    memcpy(&msg.data, buffer, len);
    msg.header.rtr = false;
    msg.header.length = len;
    msg.header.extId = true;
    msg.header.id = this->VESC_can_Id | (cmd << 8);
    port->sendMessage(msg);
}
 
void VescCAN::decode_buffer(uint8_t *buffer, uint8_t len) {
 
    if (!len) {
        return;
    }
 
    VescCmd command = (VescCmd) (buffer[0] & 0xFF);
    buffer++;
    len--;
 
    switch (command) {
 
    case VescCmd::COMM_FW_VERSION: {
        int32_t ind = 0;
        uint8_t fw_major = buffer[ind++];
        uint8_t fw_min = buffer[ind++];
 
        std::string test((char*) buffer + ind);
        ind += test.length() + 1;
 
        uint8_t uuid[12] = { 0 };
        memcpy(buffer + ind, uuid, 12);
        ind += 12;
 
        //bool isPaired = buffer[ind++];
        uint8_t isTestFw = buffer[ind++];
 
        //uint8_t hwType = buffer[ind++];           // Comment because unused
        //uint8_t customConfig = buffer[ind++];     // Comment because unused
 
        // bool hasPhaseFilters = buffer[ind++];    // Comment because unused
 
        bool compatible = false;
        if (!isTestFw)
            compatible = ((fw_major << 8) | fw_min) >= (FW_MIN_RELEASE >> 8);
        else
            compatible = ((fw_major << 16) | (fw_min << 8) | isTestFw)
                    >= FW_MIN_RELEASE;
 
        if (compatible) {
            this->state = VescState::VESC_STATE_COMPATIBLE;
        } else {
            this->state = VescState::VESC_STATE_INCOMPATIBLE;
        }
 
        break;
    }
 
    case VescCmd::COMM_GET_VALUES_SELECTIVE: {
        int32_t ind = 0;
        uint32_t mask = this->buffer_get_uint32(buffer, &ind);
 
        if (mask & ((uint32_t) 1 << 8)) {
            voltage = this->buffer_get_float16(buffer, 1e1, &ind);
        }
        if (mask & ((uint32_t) 1 << 15)) {
            vescErrorFlag = buffer[ind++];
 
            // if the vesc respond but with an error, we put the vesc in error flag
            if (vescErrorFlag) {
                state = VescState::VESC_STATE_ERROR;
            } else {
                state = VescState::VESC_STATE_READY;
            }
 
        }
        if (mask & ((uint32_t) 1 << 16)) {
            float pos = this->buffer_get_float32(buffer, 1e6, &ind);
            this->decodeEncoderPosition(pos);
        }
        break;
 
    }
 
    default:
        break;
 
    }
 
}
 
/*
 *  Process the received message on the CAN bus.
 *  Message have several struct, depends on message type, check the comm_can.c and
 *  command.c in the vesc source project to identify the struct format.
 *
 *  Quick tips :
 *  Msg struct for CAN_PACKET_PROCESS_RX_BUFFER and CAN_PACKET_PROCESS_SHORT_BUFFER :
 *  uint8_t[0] => can_emitter_id (vesc id)
 *  uint8_t[1] => send bool
 *  uint8_t[2] => COMM_PACKET_ID command
 *  uint8_t[3] -> uint8_t[7] => DATA (5 bytes)
 *
 *  Msg struct for CAN_PACKET_POLL_ROTOR_POS
 *  uint8_t[0]..uint8_t[3] : uint32 f_pos / 10000
 *
 */
void VescCAN::canRxPendCallback(CANPort* port,CAN_rx_msg& msg) {
 
    // we record the last time respond to check if vesc is OK
    lastVescResponse = HAL_GetTick();
 
    // Check if message is for the openFFBoard
    uint16_t destCanID = msg.header.id & 0xFF;
    uint8_t *rxBuf = msg.data;
 
    // Extract the command encoded in the ExtId
    VescCANMsg cmd = (VescCANMsg) (msg.header.id >> 8);
 
    bool messageIsForThisVescAxis = destCanID == this->OFFB_can_Id;
    messageIsForThisVescAxis |= (cmd == VescCANMsg::CAN_PACKET_POLL_ROTOR_POS) &&  // if it's a encoder position message
                (this->VESC_can_Id == 0xFF); // and the vescCanId is not the default one
    messageIsForThisVescAxis |= (cmd == VescCANMsg::CAN_PACKET_POLL_ROTOR_POS)
            &&  // if it's a encoder position message
            (this->VESC_can_Id != 0xFF) && // and the vescCanId is not the default one
            (destCanID == this->VESC_can_Id); // we check that emiterId is the vescId for this axis
 
    if (!messageIsForThisVescAxis){
        return;
    }
 
    // Process the CAN message received
    switch (cmd) {
 
    case VescCANMsg::CAN_PACKET_FILL_RX_BUFFER: {
        memcpy(buffer_rx + rxBuf[0], rxBuf + 1, msg.header.length - 1);
        break;
    }
 
    case VescCANMsg::CAN_PACKET_FILL_RX_BUFFER_LONG: {
        uint32_t rxbuf_ind = (unsigned int) rxBuf[0] << 8;
        rxbuf_ind |= rxBuf[1];
        if (rxbuf_ind < BUFFER_RX_SIZE) {
            memcpy(buffer_rx + rxbuf_ind, rxBuf + 2, msg.header.length - 2);
        }
        break;
    }
 
    case VescCANMsg::CAN_PACKET_PROCESS_RX_BUFFER: {
 
        // remove 2 first byte to exclude "can emitter id" and the "send bool" flag from buffer
        int32_t index = 2;
 
        uint32_t rxbuf_length = (uint32_t) rxBuf[index++] << 8;
        rxbuf_length |= (uint32_t) rxBuf[index++];
 
        if (rxbuf_length > BUFFER_RX_SIZE) {
            break;
        }
 
        uint8_t crc_high = rxBuf[index++];
        uint8_t crc_low = rxBuf[index++];
 
        if(calculateCrc16_8(crc16_tab,buffer_rx,rxbuf_length)
        //if (crc16(buffer_rx, rxbuf_length)
                == ((uint8_t) crc_high << 8 | (uint8_t) crc_low)) {
 
            this->decode_buffer(buffer_rx, rxbuf_length);
 
        }
 
        break;
    }
    case VescCANMsg::CAN_PACKET_PROCESS_SHORT_BUFFER: {
 
        // remove 2 first byte to exclude "can emitter id" and the "send bool" flag from buffer
        this->decode_buffer(rxBuf + 2, msg.header.length - 2);
 
        break;
    }
 
    case VescCANMsg::CAN_PACKET_PONG:
        state = VescState::VESC_STATE_PONG;
        break;
 
    case VescCANMsg::CAN_PACKET_POLL_ROTOR_POS: { // decode encoder data
        int32_t index = 0;
        float pos = this->buffer_get_int32(rxBuf, &index) / 100000.0; // extract the 0-360 float position
        this->decodeEncoderPosition(pos);
        break;
    }
 
    default:
        break;
    }
 
}
 
void VescCAN::buffer_append_float32(uint8_t *buffer, float number, float scale,
        int32_t *index) {
    this->buffer_append_int32(buffer, (int32_t) (number * scale), index);
}
 
void VescCAN::buffer_append_int32(uint8_t *buffer, int32_t number,
        int32_t *index) {
    buffer[(*index)++] = number >> 24;
    buffer[(*index)++] = number >> 16;
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
}
 
void VescCAN::buffer_append_uint32(uint8_t *buffer, uint32_t number,
        int32_t *index) {
    buffer[(*index)++] = number >> 24;
    buffer[(*index)++] = number >> 16;
    buffer[(*index)++] = number >> 8;
    buffer[(*index)++] = number;
}
 
uint32_t VescCAN::buffer_get_uint32(const uint8_t *buffer, int32_t *index) {
    uint32_t res = ((uint32_t) buffer[*index]) << 24
            | ((uint32_t) buffer[*index + 1]) << 16
            | ((uint32_t) buffer[*index + 2]) << 8
            | ((uint32_t) buffer[*index + 3]);
    *index += 4;
    return res;
}
 
int32_t VescCAN::buffer_get_int32(const uint8_t *buffer, int32_t *index) {
    int32_t res = ((uint32_t) buffer[*index]) << 24
            | ((uint32_t) buffer[*index + 1]) << 16
            | ((uint32_t) buffer[*index + 2]) << 8
            | ((uint32_t) buffer[*index + 3]);
    *index += 4;
    return res;
}
 
int16_t VescCAN::buffer_get_int16(const uint8_t *buffer, int32_t *index) {
    int16_t res = ((uint16_t) buffer[*index]) << 8
            | ((uint16_t) buffer[*index + 1]);
    *index += 2;
    return res;
}
 
float VescCAN::buffer_get_float32(const uint8_t *buffer, float scale,
        int32_t *index) {
    return (float) buffer_get_int32(buffer, index) / scale;
}
 
float VescCAN::buffer_get_float16(const uint8_t *buffer, float scale,
        int32_t *index) {
    return (float) buffer_get_int16(buffer, index) / scale;
}
 
#endif