These are the relevant functions, I think most of it is self-explanatory, but if anything is unclear just ask.
Note that everything under the odrive namespace is directly from the endpoint list downloaded from the O-drive.
// Top function used to request a required current from the O-drive
void OdriveComms::setCurrent(float currentSetpoint){
this->sendFloat(odrive::AXIS__CONTROLLER__TORQUE_SETPOINT, currentSetpoint);
}
// Wrapper function to send a float unit over UART to abstract usage of the class
void OdriveComms::sendFloat( uint16_t address, float floatToSend) {
uint8_t* ptr; // Let this point to the address of the 4 byte float
ptr = (uint8_t*)&floatToSend; // A little C trick, let the uint8_t pointer point to the address of the integer by typecasting its address to the desired type
// Now ptr[0] has the lowest significant byte
if(CONST_LSB_FIRST == true){ // Note that ARM is generally little endian, so only flip if big endian is desired
// Re-order the bytes into little endian format (MSByte first)
for (int l = 0; l < 2; l++) {
uint8_t buf = ptr[l];
ptr[l] = ptr[3 - l];
ptr[3 - l] = buf;
}
}
// Construct and send the message
this->sendMessage(address, ptr, 4); // Datasize is 4
}
// Sends a message to the O-drive.
// Data provided should be little endian (i.e. data[0] = MSByte))
// Total package size limit is at 30 bytes
void OdriveComms::sendMessage(uint16_t address, uint8_t* data, uint8_t datasize) {
// Size of a message depends on the payload size (data size)
// For a payload of size N, the total message size in bytes is N + 8
// Message structure as follows:
// B0 + B1 = sequence number
// B2 + B3 = Endpoint ID (register of o-drive)
// B4 + B5 = 0 (how many bytes expected in return)
// B6 - BN+5 = payload
// BN+6 + BN+7 = CRC over JSON, which is constant per Odrive version. Defined in parameters.h
// For more details see "Protocal Analysis.odt"
// The structure is little Endian, i.e. MSByte last
// Since dynamic memory cannot be used, static memory with safety check is used
// Note that dynamic memory allocation compromises realtime behavior because it is unpredictable and potentially unacceptable in terms of performance.
// It also lacks limits, and can give rise to memory fragmantation of the heap without proper management
if(datasize + 8 < 31){
uint8_t dataToSend[30]; // max size of 30 bytes, which should be plenty
// Add the sequence number
dataToSend[0] = this->sequenceNumber; // Upper part of the 16 bit variable (this first because little endian)
dataToSend[1] = this->sequenceNumber >> 8; // Lower part of the 16 bit variable
// Add enpoint ID (address)
address &= 0x7FFF;// Ensure that the MSB is set to 0 as no return is expected in response
dataToSend[2] = address; // Upper part of the 16 bit variable
dataToSend[3] = address >> 8; // Lower part of the 16 bit variable
// Add zeros for 'bytes expected in return'
dataToSend[4] = 0x00;
dataToSend[5] = 0x00;
// Add payload
memcpy(dataToSend + (sizeof(uint8_t)*6),data,datasize);
// Add the JSON CRC which is constant and predefined. Defined in parameters.h
dataToSend[datasize + 6] = odrive::json_crc; // Upper part of checksum
dataToSend[datasize + 7] = odrive::json_crc >> 8; // Lower part of checksum
// Send the message
this->transmitPacket(dataToSend, datasize+8);
// Increase sequence number in preperation for next packet that will be sent
this->sequenceNumber++;
}
}
// Wraps the packet into a stream and sends the bytes over UART
// The *data should contain the packet of size datasize
void OdriveComms::transmitPacket(uint8_t* data, uint8_t packetsize) {
// https://docs.odriverobotics.com/protocol
// Message structure as follows:
// B0 = Sync byte, constant at 0xAA
// B1 = Packet length
// B2 = CRC8 of the B0 and B1
// B3 - BN+3 = packet
// BN+4 + BN+5 = CRC over previous bytes? TODO double check
// The structure is little Endian, i.e. MSByte last
if(packetsize + 5 < 41){ // Double check the packet size, ignore if too big
uint8_t dataStream[40]; // The stream that will be send
uint8_t streamsize = packetsize + 5; // There are 5 additional 'wrapper bytes'
// Wrap the original packet into a stream
dataStream[0] = 0xAA; // Sync byte
dataStream[1] = packetsize; // Packet length
// Add the CRC8 of the previous 2 bytes
dataStream[2] = this->calculateCRC8(dataStream, 2);
// Add the packet
memcpy(dataStream + (sizeof(uint8_t)*3),data,packetsize);
// Add CRC16
uint16_t CRC16_checksum = this->calculateCRC16(data, packetsize); // Do the CRC16 over just the data packet
dataStream[packetsize + 3] = CRC16_checksum >> 8; // Upper part of checksum
dataStream[packetsize + 4] = CRC16_checksum; // Lower part of checksum
// Send bytes over UART
// Only do so if serial port is initialized
if(this->odriveserialport != NULL){
this->odriveserialport->write(dataStream, streamsize);
// Below are a few debug statements that request serial number and bus voltage to test working of UART controls
//uint8_t Request_Serial[] = { 0xaa, 0x08, 0x3d, 0x04, 0x00, 0x04, 0x80, 0x08, 0x00, 0x40, 0x9b, 0x9b, 0x94 };
//uint8_t Request_VBUS[] = { 0xaa, 0x08, 0x3d, 0x01, 0x00, 0x01, 0x80, 0x04, 0x00, 0x40, 0x9b, 0xd3, 0x58 };
//uint8_t Request_Current[] = {0xaa, 0x0c, 0xE1, 0x94, 0x00, 0x94, 0x80, 0x00, 0x00, 0x66, 0x66, 0x0e, 0x40, 0x40, 0x9b, 0x17, 0xA7};
//this->odriveserialport->write(Request_Serial, 13);
//this->odriveserialport->write(Request_VBUS, 13);
//this->odriveserialport->write(Request_Current, 17);
}
// Note that when calling this function, the data gets copied to a hardware register, so the original data used to call the function can be modified/deleted etc without worries
// Note that the serial port should be configured as non-blocking to ensure that the program does not hang here (especially if called via interrupts)
}
}
In my case I donât need to match them up, I just need to know what the response contains. If a packet gets dropped, it is OK, because I will be sending another request soon anyway.
In my application (which is a control system, the O-drive is just used as a current controller for the brushless motor) I donât care about what the O-drive sends back regardless. So we donât need to worry about any kind of response.
What might be important to mention, is that this control loop runs at 1khz.
Again, the message this creates is:
For completeness sake, this is the message at this point:
0xAA 0X0C 0XE1 | 0X44 0X6C 0XC7 0X00 0X00 0X00 0X66 0X66 0X0E 0X40 0X40 0X9B | 0X14 0XB0
) I had the bytes for the Sequence Number and Endpoint reversed.