diff --git a/README.md b/README.md index b8280df..ef82090 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # VescUart -Arduino library for interfacing with a VESC over UART. This library is based upon the works of many. The library is tested on a Teensy 4, and is updated for VESC firmware FW5+. The library is not nessecary backwards compatible with older versions of VESC firmware, so please update to the newest firmware available to your VESC. +Arduino library for interfacing with a VESC over UART. This library is based upon the works of many. The library is tested on a Teensy 4, and is updated for VESC firmware FW5+. The library is not necessarily backwards compatible with older versions of VESC firmware, so please update to the newest firmware available to your VESC. The library supports only a small amount of features available to the VESC. You are welcome to make a pull request if you integrate new functionality and I will do my best to merge. diff --git a/keywords.txt b/keywords.txt index 5ec1bcb..534d309 100644 --- a/keywords.txt +++ b/keywords.txt @@ -20,4 +20,5 @@ printVescValues KEYWORD2 setCurrent KEYWORD2 setBrakeCurrent KEYWORD2 setRPM KEYWORD2 -setDuty KEYWORD2 \ No newline at end of file +setDuty KEYWORD2 +getImuData KEYWORD2 \ No newline at end of file diff --git a/library.properties b/library.properties index 38fdc19..8cd309a 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=VescUart -version=1.0.1 +version=1.0.2 author=SolidGeek maintainer=SolidGeek sentence=Library offering UART communication for VESC diff --git a/src/VescUart.cpp b/src/VescUart.cpp index 4d25e18..2c72553 100644 --- a/src/VescUart.cpp +++ b/src/VescUart.cpp @@ -59,7 +59,7 @@ int VescUart::receiveUartMessage(uint8_t * payloadReceived) { default: if( debugPort != NULL ){ - debugPort->println("Unvalid start bit"); + debugPort->println("Invalid start bit"); } break; } @@ -188,10 +188,11 @@ bool VescUart::processReadPacket(uint8_t * message) { switch (packetId){ case COMM_FW_VERSION: // Structure defined here: https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164 - fw_version.major = message[index++]; fw_version.minor = message[index++]; - return true; + + return true; + break; case COMM_GET_VALUES: // Structure defined here: https://github.com/vedderb/bldc/blob/43c3bbaf91f5052a35b75c2ff17b5fe99fad94d1/commands.c#L164 data.tempMosfet = buffer_get_float16(message, 10.0, &index); // 2 bytes - mc_interface_temp_fet_filtered() @@ -214,17 +215,38 @@ bool VescUart::processReadPacket(uint8_t * message) { data.id = message[index++]; // 1 byte - app_get_configuration()->controller_id return true; - break; - /* case COMM_GET_VALUES_SELECTIVE: - - uint32_t mask = 0xFFFFFFFF; */ + case COMM_GET_IMU_DATA: // Structure defined here: https://github.com/vedderb/bldc/blob/c8be115bb5be5a5558e3a50ba82e55931e3a45c4/comm/commands.c#L1111 + + data.imuMask = buffer_get_uint16(message, &index); // Skip 2 bytes for mask + data.imuRoll = buffer_get_float32_auto(message, &index); + data.imuPitch = buffer_get_float32_auto(message, &index); + data.imuYaw = buffer_get_float32_auto(message, &index); + data.accX = buffer_get_float32_auto(message, &index); + data.accY = buffer_get_float32_auto(message, &index); + data.accZ = buffer_get_float32_auto(message, &index); + data.gyroX = buffer_get_float32_auto(message, &index); + data.gyroY = buffer_get_float32_auto(message, &index); + data.gyroZ = buffer_get_float32_auto(message, &index); + data.magX = buffer_get_float32_auto(message, &index); + data.magY = buffer_get_float32_auto(message, &index); + data.magZ = buffer_get_float32_auto(message, &index); + data.q0 = buffer_get_float32_auto(message, &index); + data.q1 = buffer_get_float32_auto(message, &index); + data.q2 = buffer_get_float32_auto(message, &index); + data.q3 = buffer_get_float32_auto(message, &index); + //data.imuID = message[index] // Last byte should have the ID of the VESC but it seems like its + + return true; + break; default: return false; break; } + + return false; } bool VescUart::getFWversion(void){ @@ -282,6 +304,39 @@ bool VescUart::getVescValues(uint8_t canId) { } return false; } + +bool VescUart::getImuData(void) { + return getImuData(0); +} + +bool VescUart::getImuData(uint8_t canId) { + + if (debugPort!=NULL){ + debugPort->println("Command: COMM_GET_IMU_DATA "+String(canId)); + } + + int32_t index = 0; + int payloadSize = (canId == 0 ? 3: 5); + uint8_t payload[payloadSize]; + if (canId != 0) { + payload[index++] = { COMM_FORWARD_CAN }; + payload[index++] = canId; + } + payload[index++] = { COMM_GET_IMU_DATA}; + payload[index++] = 0xFF; // Add Mask + payload[index++] = 0xFF; // Add Mask + + packSendPayload(payload, payloadSize); + + uint8_t message[256]; + int messageLength = receiveUartMessage(message); + + if (messageLength > 65) { // Message length should be at least 67 (set to 65) + return processReadPacket(message); + } + return false; +} + void VescUart::setNunchuckValues() { return setNunchuckValues(0); } @@ -436,5 +491,14 @@ void VescUart::printVescValues() { debugPort->print("tempMosfet: "); debugPort->println(data.tempMosfet); debugPort->print("tempMotor: "); debugPort->println(data.tempMotor); debugPort->print("error: "); debugPort->println(data.error); + debugPort->print("imuRoll: "); debugPort->println(data.imuRoll); + debugPort->print("imuPitch: "); debugPort->println(data.imuPitch); + debugPort->print("imuYaw: "); debugPort->println(data.imuYaw); + debugPort->print("accX: "); debugPort->println(data.accX); + debugPort->print("accY: "); debugPort->println(data.accY); + debugPort->print("accZ: "); debugPort->println(data.accZ); + debugPort->print("gyroX: "); debugPort->println(data.gyroX); + debugPort->print("gyroY: "); debugPort->println(data.gyroY); + debugPort->print("gyroZ: "); debugPort->println(data.gyroZ); } } diff --git a/src/VescUart.h b/src/VescUart.h index dd8492b..9f9f35a 100644 --- a/src/VescUart.h +++ b/src/VescUart.h @@ -10,8 +10,8 @@ class VescUart { /** Struct to store the telemetry data returned by the VESC */ - struct dataPackage { - float avgMotorCurrent; + typedef struct { + float avgMotorCurrent; float avgInputCurrent; float dutyCycleNow; float rpm; @@ -27,20 +27,38 @@ class VescUart float pidPos; uint8_t id; mc_fault_code error; - }; + uint16_t imuMask; + float imuRoll; + float imuPitch; + float imuYaw; + float accX; + float accY; + float accZ; + float gyroX; + float gyroY; + float gyroZ; + float magX; + float magY; + float magZ; + float q0; + float q1; + float q2; + float q3; + // uint8_t imuID; + } vesc_telemetry_t; /** Struct to hold the nunchuck values to send over UART */ - struct nunchuckPackage { + typedef struct { int valueX; int valueY; bool upperButton; // valUpperButton bool lowerButton; // valLowerButton - }; + } vesc_nunchuck_t; - struct FWversionPackage { + typedef struct { uint8_t major; uint8_t minor; - }; + } vesc_firmware_t ; //Timeout - specifies how long the function will wait for the vesc to respond const uint32_t _TIMEOUT; @@ -52,141 +70,156 @@ class VescUart VescUart(uint32_t timeout_ms = 100); /** Variabel to hold measurements returned from VESC */ - dataPackage data; + vesc_telemetry_t data; /** Variabel to hold nunchuck values */ - nunchuckPackage nunchuck; + vesc_nunchuck_t nunchuck; /** Variable to hold firmware version */ - FWversionPackage fw_version; - - /** - * @brief Set the serial port for uart communication - * @param port - Reference to Serial port (pointer) - */ - void setSerialPort(Stream* port); - - /** - * @brief Set the serial port for debugging - * @param port - Reference to Serial port (pointer) - */ - void setDebugPort(Stream* port); - - /** - * @brief Populate the firmware version variables - * - * @return True if successfull otherwise false - */ - bool getFWversion(void); - - /** - * @brief Populate the firmware version variables - * - * @param canId - The CAN ID of the VESC - * @return True if successfull otherwise false - */ - bool getFWversion(uint8_t canId); - - /** - * @brief Sends a command to VESC and stores the returned data - * - * @return True if successfull otherwise false - */ - bool getVescValues(void); - - /** - * @brief Sends a command to VESC and stores the returned data - * @param canId - The CAN ID of the VESC - * - * @return True if successfull otherwise false - */ - bool getVescValues(uint8_t canId); - - /** - * @brief Sends values for joystick and buttons to the nunchuck app - */ - void setNunchuckValues(void); - /** - * @brief Sends values for joystick and buttons to the nunchuck app - * @param canId - The CAN ID of the VESC - */ - void setNunchuckValues(uint8_t canId); - - /** - * @brief Set the current to drive the motor - * @param current - The current to apply - */ - void setCurrent(float current); - - /** - * @brief Set the current to drive the motor - * @param current - The current to apply - * @param canId - The CAN ID of the VESC - */ - void setCurrent(float current, uint8_t canId); - - /** - * @brief Set the current to brake the motor - * @param brakeCurrent - The current to apply - */ - void setBrakeCurrent(float brakeCurrent); - - /** - * @brief Set the current to brake the motor - * @param brakeCurrent - The current to apply - * @param canId - The CAN ID of the VESC - */ - void setBrakeCurrent(float brakeCurrent, uint8_t canId); - - - /** - * @brief Set the rpm of the motor - * @param rpm - The desired RPM (actually eRPM = RPM * poles) - */ - void setRPM(float rpm); - - /** - * @brief Set the rpm of the motor - * @param rpm - The desired RPM (actually eRPM = RPM * poles) - * @param canId - The CAN ID of the VESC - */ - void setRPM(float rpm, uint8_t canId); - - /** - * @brief Set the duty of the motor - * @param duty - The desired duty (0.0-1.0) - */ - void setDuty(float duty); - - /** - * @brief Set the duty of the motor - * @param duty - The desired duty (0.0-1.0) - * @param canId - The CAN ID of the VESC - */ - void setDuty(float duty, uint8_t canId); - - /** - * @brief Send a keepalive message - */ - void sendKeepalive(void); - - /** - * @brief Send a keepalive message - * @param canId - The CAN ID of the VESC - */ - void sendKeepalive(uint8_t canId); - - /** - * @brief Help Function to print struct dataPackage over Serial for Debug - */ - void printVescValues(void); + vesc_firmware_t fw_version; + + /** + * @brief Set the serial port for uart communication + * @param port - Reference to Serial port (pointer) + */ + void setSerialPort(Stream* port); + + /** + * @brief Set the serial port for debugging + * @param port - Reference to Serial port (pointer) + */ + void setDebugPort(Stream* port); + + /** + * @brief Populate the firmware version variables + * + * @return True if successfull otherwise false + */ + bool getFWversion(void); + + /** + * @brief Populate the firmware version variables + * + * @param canId - The CAN ID of the VESC + * @return True if successfull otherwise false + */ + bool getFWversion(uint8_t canId); + + /** + * @brief Sends a command to VESC and stores the returned data + * + * @return True if successfull otherwise false + */ + bool getVescValues(void); + + /** + * @brief Sends a command to VESC and stores the returned data + * @param canId - The CAN ID of the VESC + * + * @return True if successfull otherwise false + */ + bool getVescValues(uint8_t canId); + + /** + * @brief Sends a command to VESC and stores the returned IMU data + * + * @return True if successfull otherwise false + */ + bool getImuData(void); + + /** + * @brief Sends a command to VESC and stores the returned IMU data + * @param canId - The CAN ID of the VESC + * + * @return True if successfull otherwise false + */ + bool getImuData(uint8_t canId); + + /** + * @brief Sends values for joystick and buttons to the nunchuck app + */ + void setNunchuckValues(void); + /** + * @brief Sends values for joystick and buttons to the nunchuck app + * @param canId - The CAN ID of the VESC + */ + void setNunchuckValues(uint8_t canId); + + /** + * @brief Set the current to drive the motor + * @param current - The current to apply + */ + void setCurrent(float current); + + /** + * @brief Set the current to drive the motor + * @param current - The current to apply + * @param canId - The CAN ID of the VESC + */ + void setCurrent(float current, uint8_t canId); + + /** + * @brief Set the current to brake the motor + * @param brakeCurrent - The current to apply + */ + void setBrakeCurrent(float brakeCurrent); + + /** + * @brief Set the current to brake the motor + * @param brakeCurrent - The current to apply + * @param canId - The CAN ID of the VESC + */ + void setBrakeCurrent(float brakeCurrent, uint8_t canId); + + + /** + * @brief Set the rpm of the motor + * @param rpm - The desired RPM (actually eRPM = RPM * poles) + */ + void setRPM(float rpm); + + /** + * @brief Set the rpm of the motor + * @param rpm - The desired RPM (actually eRPM = RPM * poles) + * @param canId - The CAN ID of the VESC + */ + void setRPM(float rpm, uint8_t canId); + + /** + * @brief Set the duty of the motor + * @param duty - The desired duty (0.0-1.0) + */ + void setDuty(float duty); + + /** + * @brief Set the duty of the motor + * @param duty - The desired duty (0.0-1.0) + * @param canId - The CAN ID of the VESC + */ + void setDuty(float duty, uint8_t canId); + + /** + * @brief Send a keepalive message + */ + void sendKeepalive(void); + + /** + * @brief Send a keepalive message + * @param canId - The CAN ID of the VESC + */ + void sendKeepalive(uint8_t canId); + + /** + * @brief Help Function to print struct dataPackage over Serial for Debug + */ + void printVescValues(void); private: - /** Variabel to hold the reference to the Serial object to use for UART */ + /** Variable to hold the reference to the Serial object to use for UART */ Stream* serialPort = NULL; - /** Variabel to hold the reference to the Serial object to use for debugging. + /** Variable to hold the reference to the Serial object to use for debugging. * Uses the class Stream instead of HarwareSerial */ Stream* debugPort = NULL; @@ -203,7 +236,7 @@ class VescUart * @brief Receives the message over Serial * * @param payloadReceived - The received payload as a unit8_t Array - * @return The number of bytes receeived within the payload + * @return The number of bytes received within the payload */ int receiveUartMessage(uint8_t * payloadReceived); @@ -211,7 +244,7 @@ class VescUart * @brief Verifies the message (CRC-16) and extracts the payload * * @param message - The received UART message - * @param lenMes - The lenght of the message + * @param lenMes - The length of the message * @param payload - The final payload ready to extract data from * @return True if the process was a success */ @@ -229,7 +262,7 @@ class VescUart * @brief Help Function to print uint8_t array over Serial for Debug * * @param data - Data array to print - * @param len - Lenght of the array to print + * @param len - Length of the array to print */ void serialPrint(uint8_t * data, int len);