From 883661fc518234cca57e507f7e33bcc983834ca7 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 12 Mar 2021 21:49:35 +0100 Subject: [PATCH] Initial test --- boards/px4/fmu-v4/default.cmake | 3 +- boards/px4/fmu-v4/src/board_config.h | 2 + src/drivers/vesc/CMakeLists.txt | 46 ++++ src/drivers/vesc/VescDriver/VescDriver.cpp | 269 +++++++++++++++++++ src/drivers/vesc/VescDriver/VescDriver.hpp | 92 +++++++ src/drivers/vesc/VescDriver/VescProtocol.h | 206 ++++++++++++++ src/drivers/vesc/VescDriver/VescWritable.hpp | 54 ++++ src/drivers/vesc/VescSerialDevice.cpp | 220 +++++++++++++++ src/drivers/vesc/VescSerialDevice.hpp | 60 +++++ src/drivers/vesc/module.yaml | 6 + src/drivers/vesc/vesc_main.cpp | 180 +++++++++++++ 11 files changed, 1137 insertions(+), 1 deletion(-) create mode 100644 src/drivers/vesc/CMakeLists.txt create mode 100644 src/drivers/vesc/VescDriver/VescDriver.cpp create mode 100644 src/drivers/vesc/VescDriver/VescDriver.hpp create mode 100644 src/drivers/vesc/VescDriver/VescProtocol.h create mode 100644 src/drivers/vesc/VescDriver/VescWritable.hpp create mode 100644 src/drivers/vesc/VescSerialDevice.cpp create mode 100644 src/drivers/vesc/VescSerialDevice.hpp create mode 100644 src/drivers/vesc/module.yaml create mode 100644 src/drivers/vesc/vesc_main.cpp diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 3c8cc66c35..f850205b60 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -51,6 +51,7 @@ px4_add_board( telemetry # all available telemetry drivers tone_alarm uavcan + vesc MODULES airspeed_selector attitude_estimator_q @@ -89,7 +90,7 @@ px4_add_board( vtol_att_control SYSTEMCMDS bl_update - #dmesg + dmesg dumpfile esc_calib gpio diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h index a822097199..ee7a06d29c 100644 --- a/boards/px4/fmu-v4/src/board_config.h +++ b/boards/px4/fmu-v4/src/board_config.h @@ -98,6 +98,8 @@ #define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) #define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15) +#define BOARD_ENABLE_CONSOLE_BUFFER + /** * PWM: * diff --git a/src/drivers/vesc/CMakeLists.txt b/src/drivers/vesc/CMakeLists.txt new file mode 100644 index 0000000000..0355cfb348 --- /dev/null +++ b/src/drivers/vesc/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__vesc + MAIN vesc + SRCS + vesc_main.cpp + VescSerialDevice.hpp + VescSerialDevice.cpp + VescDriver/VescDriver.hpp + VescDriver/VescDriver.cpp + VescDriver/VescProtocol.h + MODULE_CONFIG + module.yaml + DEPENDS + ) diff --git a/src/drivers/vesc/VescDriver/VescDriver.cpp b/src/drivers/vesc/VescDriver/VescDriver.cpp new file mode 100644 index 0000000000..87c36058f2 --- /dev/null +++ b/src/drivers/vesc/VescDriver/VescDriver.cpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VescDriverUart.cpp + * @author Matthias Grob + */ + +#include "VescDriver.hpp" +#include + +void VescDriver::commandCurrent(float current) +{ + uint8_t command[5] {VescCommand::SET_CURRENT}; + uint16_t index{1}; + insertInt32(command, index, static_cast(current * 1000.f)); + sendPacket(command, 5); +} + +void VescDriver::commandBrakeCurrent(float current) +{ + uint8_t command[5] {VescCommand::SET_CURRENT_BRAKE}; + uint16_t index{1}; + insertInt32(command, index, static_cast(current * 1000.f)); + sendPacket(command, 5); +} + +void VescDriver::requestFirmwareVersion() +{ + uint8_t command{VescCommand::FW_VERSION}; + sendPacket(&command, 1); +} + +void VescDriver::requestValues() +{ + uint8_t command{VescCommand::GET_VALUES}; + sendPacket(&command, 1); +} + +size_t VescDriver::sendPacket(const uint8_t *payload, const uint16_t payload_length) +{ + if (payload_length == 0 || payload_length > MAXIMUM_PAYLOAD_LENGTH) { + return 0; + } + + uint8_t packet_buffer[payload_length + PACKET_OVERHEAD_LENGTH]; + uint16_t index{0}; + + // Start byte and payload size + if (payload_length <= 256) { + packet_buffer[index++] = 2; + packet_buffer[index++] = payload_length; + + } else { + packet_buffer[index++] = 3; + packet_buffer[index++] = static_cast(payload_length >> 8); + packet_buffer[index++] = static_cast(payload_length & 0xFF); + } + + // Payload + memcpy(&packet_buffer[index], payload, payload_length); + index += payload_length; + + // CRC + const uint16_t crc = crc16(payload, payload_length); + packet_buffer[index++] = static_cast(crc >> 8); + packet_buffer[index++] = static_cast(crc & 0xFF); + + // Stop byte + packet_buffer[index++] = 3; + + // Write bytes out through callback interface + size_t ret{0}; + + if (_vesc_writable) { + ret = _vesc_writable->writeCallback(packet_buffer, index); + } + + return ret; +} + +void VescDriver::parseInputByte(uint8_t byte) +{ + if (_input_byte_index == 0) { + // Start byte + if (byte == 2 /*|| byte == 3*/) { + _input_start_byte = byte; + _input_byte_index++; + } + + } else if (_input_byte_index == 1) { + // Payload size + _input_byte_index++; + + if (_input_start_byte == 2) { + // Short packet + _input_payload_length = byte; + + if (_input_payload_length < 1) { + _input_byte_index = 0; + } + + } else { + // Long packet high byte + _input_payload_length = byte << 8; + } + + } else if (_input_byte_index == 2 && _input_start_byte == 3) { + // Payload size long packet low byte + _input_payload_length |= byte; + _input_byte_index++; + + if (_input_payload_length < 255 || _input_payload_length > MAXIMUM_PAYLOAD_LENGTH) { + _input_byte_index = 0; + } + + } else if (_input_byte_index < _input_start_byte + _input_payload_length) { + // Payload + _input_payload[_input_byte_index - _input_start_byte] = byte; + _input_byte_index++; + + } else if (_input_byte_index == _input_start_byte + _input_payload_length) { + // CRC high byte + _input_payload_crc = byte << 8; + _input_byte_index++; + + } else if (_input_byte_index == _input_start_byte + _input_payload_length + 1u) { + // CRC low byte + _input_payload_crc |= byte; + _input_byte_index++; + + if (_input_payload_crc != crc16(_input_payload, _input_payload_length)) { + _input_byte_index = 0; + } + + } else if (_input_byte_index == _input_start_byte + _input_payload_length + 2u) { + // Stop byte + _input_byte_index = 0; + + if (byte == 3) { + parsePayload(_input_payload, _input_payload_length); + } + } +} + +void VescDriver::parsePayload(const uint8_t *payload, const uint16_t payload_length) +{ + uint16_t index{1}; + + switch (payload[0]) { + case VescCommand::FW_VERSION: + if (payload_length >= 9u) { + _vesc_version.version_major = payload[index++]; + _vesc_version.version_minor = payload[index++]; + // strcpy(_vesc_version.hardware_name, reinterpret_cast(&payload[index])); + // index += strlen(_vesc_version.hardware_name) + 1u; + // memcpy(_vesc_version.stm32_uuid_8, &payload[index], sizeof(_vesc_version.stm32_uuid_8)); + // index += 12; + // _vesc_version.pairing_done = payload[index++]; + // _vesc_version.test_version_number = payload[index++]; + // _vesc_version.hardware_type = payload[index++]; + // _vesc_version.custom_configuration = payload[index++]; + } + + break; + + case VescCommand::GET_VALUES: + if (payload_length >= 73u) { + _vesc_values.fet_temperature = extractFloat16(payload, index) / 10.f; + _vesc_values.motor_temperature = extractFloat16(payload, index) / 10.f; + _vesc_values.motor_current = extractFloat32(payload, index) / 100.f; + _vesc_values.input_current = extractFloat32(payload, index) / 100.f; + _vesc_values.reset_average_id = extractFloat32(payload, index) / 100.f; + _vesc_values.reset_average_iq = extractFloat32(payload, index) / 100.f; + _vesc_values.duty_cycle = extractFloat16(payload, index) / 1000.f; + _vesc_values.rpm = extractInt32(payload, index); + _vesc_values.input_voltage = extractFloat16(payload, index) / 10.f; + _vesc_values.used_charge_Ah = extractFloat32(payload, index) / 1e4f; + _vesc_values.charged_charge_Ah = extractFloat32(payload, index) / 1e4f; + _vesc_values.used_energy_Wh = extractFloat32(payload, index) / 1e4f; + _vesc_values.charged_energy_wh = extractFloat32(payload, index) / 10.f; + _vesc_values.tachometer = extractInt32(payload, index); + _vesc_values.tachometer_absolute = extractInt32(payload, index); + _vesc_values.fault = payload[index++]; + _vesc_values.position_pid = extractFloat32(payload, index) / 1e6f; + _vesc_values.controller_id = payload[index++]; + _vesc_values.ntc_temperature_mos1 = extractFloat16(payload, index) / 10.f; + _vesc_values.ntc_temperature_mos2 = extractFloat16(payload, index) / 10.f; + _vesc_values.ntc_temperature_mos3 = extractFloat16(payload, index) / 10.f; + _vesc_values.read_reset_average_vd = extractFloat32(payload, index) / 1000.f; + _vesc_values.read_reset_average_vq = extractFloat32(payload, index) / 1000.f; + } + + break; + } +} + +uint16_t VescDriver::crc16(const uint8_t *buffer, const uint16_t length) +{ + uint16_t checksum{0}; + + for (size_t i = 0; i < length; i++) { + uint8_t table_index = (((checksum >> 8) ^ buffer[i]) & 0xFF); + checksum = CRC_TABLE[table_index] ^ (checksum << 8); + } + + return checksum; +} + +void VescDriver::insertInt32(uint8_t *buffer, uint16_t &index, int32_t value) +{ + buffer[index++] = value >> 24; + buffer[index++] = value >> 16; + buffer[index++] = value >> 8; + buffer[index++] = value; +} + +int16_t VescDriver::extractInt16(const uint8_t *buffer, uint16_t &index) +{ + index += 2; + return static_cast(buffer[index - 2] << 8 | buffer[index - 1]); +} + +float VescDriver::extractFloat16(const uint8_t *buffer, uint16_t &index) +{ + return static_cast(extractInt16(buffer, index)); +} + +int32_t VescDriver::extractInt32(const uint8_t *buffer, uint16_t &index) +{ + index += 4; + return static_cast( + buffer[index - 4] << 24 | buffer[index - 3] << 16 | buffer[index - 2] << 8 | buffer[index - 1]); +} + +float VescDriver::extractFloat32(const uint8_t *buffer, uint16_t &index) +{ + return static_cast(extractInt32(buffer, index)); +} diff --git a/src/drivers/vesc/VescDriver/VescDriver.hpp b/src/drivers/vesc/VescDriver/VescDriver.hpp new file mode 100644 index 0000000000..6025f475b9 --- /dev/null +++ b/src/drivers/vesc/VescDriver/VescDriver.hpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VescDriverUart.hpp + * @brief Driver to communicate with VESC motor contollers + * @details More about VESC BLDC (brushless direct current) electric motor controllers by Benjamin Vedder on https://vesc-project.com/ + * @author Matthias Grob + */ + +#pragma once + +#include "VescProtocol.h" +#include "VescWritable.hpp" +#include +#include + +class VescDriver +{ +public: + VescDriver(VescWritable *vesc_writable) : _vesc_writable(vesc_writable) {}; + ~VescDriver() = default; + + void commandCurrent(float current); + void commandBrakeCurrent(float current); + + void requestFirmwareVersion(); + void requestValues(); + float getRpm() { return _vesc_values.rpm; }; + float getInputVoltage() { return _vesc_values.input_voltage; }; + uint8_t getVersionMajor() { return _vesc_version.version_major; } + uint8_t getVersionMinor() { return _vesc_version.version_minor; } + + void parseInputByte(uint8_t byte); ///< Call when data is ready to read + +private: + // De-/serialize packets + size_t sendPacket(const uint8_t *payload, const uint16_t payload_length); + void parsePayload(const uint8_t *payload, const uint16_t payload_length); + uint16_t crc16(const uint8_t *buffer, const uint16_t length); + + // Big-endian helpers + void insertInt32(uint8_t *buffer, uint16_t &index, int32_t value); + int16_t extractInt16(const uint8_t *buffer, uint16_t &index); + float extractFloat16(const uint8_t *buffer, uint16_t &index); + int32_t extractInt32(const uint8_t *buffer, uint16_t &index); + float extractFloat32(const uint8_t *buffer, uint16_t &index); + + // Write access to device through callback + VescWritable *_vesc_writable; + + // Input packet parsing + size_t _input_byte_index{0}; ///< keeps track of the input packets parsing state + uint8_t _input_start_byte{0}; + uint16_t _input_payload_length{0}; + uint8_t _input_payload[MAXIMUM_PAYLOAD_LENGTH]; + uint16_t _input_payload_crc{0}; + + // Information storage for getters + VescVersion _vesc_version{}; + VescValues _vesc_values{}; +}; diff --git a/src/drivers/vesc/VescDriver/VescProtocol.h b/src/drivers/vesc/VescDriver/VescProtocol.h new file mode 100644 index 0000000000..d5366f2099 --- /dev/null +++ b/src/drivers/vesc/VescDriver/VescProtocol.h @@ -0,0 +1,206 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VescProtocol.h + * @brief VESC protocol definitions + * @author Matthias Grob + */ + +#pragma once + +#include + +static constexpr int MAXIMUM_PAYLOAD_LENGTH{512}; +static constexpr int PACKET_OVERHEAD_LENGTH{6}; // Bytes: 1 start, 2 size, 2 CRC, 1 stop +static constexpr int MAXIMUM_PACKET_LENGTH{MAXIMUM_PAYLOAD_LENGTH + PACKET_OVERHEAD_LENGTH}; + +enum VescCommand : uint8_t { + FW_VERSION = 0, + JUMP_TO_BOOTLOADER, + ERASE_NEW_APP, + WRITE_NEW_APP_DATA, + GET_VALUES, + SET_DUTY, + SET_CURRENT, + SET_CURRENT_BRAKE, + SET_RPM, + SET_POS, + SET_HANDBRAKE, + SET_DETECT, + SET_SERVO_POS, + SET_MCCONF, + GET_MCCONF, + GET_MCCONF_DEFAULT, + SET_APPCONF, + GET_APPCONF, + GET_APPCONF_DEFAULT, + SAMPLE_PRINT, + TERMINAL_CMD, + PRINT, + ROTOR_POSITION, + EXPERIMENT_SAMPLE, + DETECT_MOTOR_PARAM, + DETECT_MOTOR_R_L, + DETECT_MOTOR_FLUX_LINKAGE, + DETECT_ENCODER, + DETECT_HALL_FOC, + REBOOT, + ALIVE, + GET_DECODED_PPM, + GET_DECODED_ADC, + GET_DECODED_CHUK, + FORWARD_CAN, + SET_CHUCK_DATA, + CUSTOM_APP_DATA, + NRF_START_PAIRING, + GPD_SET_FSW, + GPD_BUFFER_NOTIFY, + GPD_BUFFER_SIZE_LEFT, + GPD_FILL_BUFFER, + GPD_OUTPUT_SAMPLE, + GPD_SET_MODE, + GPD_FILL_BUFFER_INT8, + GPD_FILL_BUFFER_INT16, + GPD_SET_BUFFER_INT_SCALE, + GET_VALUES_SETUP, + SET_MCCONF_TEMP, + SET_MCCONF_TEMP_SETUP, + GET_VALUES_SELECTIVE, + GET_VALUES_SETUP_SELECTIVE, + EXT_NRF_PRESENT, + EXT_NRF_ESB_SET_CH_ADDR, + EXT_NRF_ESB_SEND_DATA, + EXT_NRF_ESB_RX_DATA, + EXT_NRF_SET_ENABLED, + DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, + DETECT_APPLY_ALL_FOC, + JUMP_TO_BOOTLOADER_ALL_CAN, + ERASE_NEW_APP_ALL_CAN, + WRITE_NEW_APP_DATA_ALL_CAN, + PING_CAN, + APP_DISABLE_OUTPUT, + TERMINAL_CMD_SYNC, + GET_IMU_DATA, + BM_CONNECT, + BM_ERASE_FLASH_ALL, + BM_WRITE_FLASH, + BM_REBOOT, + BM_DISCONNECT, + BM_MAP_PINS_DEFAULT, + BM_MAP_PINS_NRF5X, + ERASE_BOOTLOADER, + ERASE_BOOTLOADER_ALL_CAN, + PLOT_INIT, + PLOT_DATA, + PLOT_ADD_GRAPH, + PLOT_SET_GRAPH, + GET_DECODED_BALANCE, + BM_MEM_READ, + WRITE_NEW_APP_DATA_LZO, + WRITE_NEW_APP_DATA_ALL_CAN_LZO, + BM_WRITE_FLASH_LZO, + SET_CURRENT_REL, + CAN_FWD_FRAME, + SET_BATTERY_CUT, + SET_BLE_NAME, + SET_BLE_PIN, + SET_CAN_MODE, + GET_IMU_CALIBRATION, + GET_MCCONF_TEMP, + + // Custom configuration for hardware + GET_CUSTOM_CONFIG_XML, + GET_CUSTOM_CONFIG, + GET_CUSTOM_CONFIG_DEFAULT, + SET_CUSTOM_CONFIG, + + // BMS commands + BMS_GET_VALUES, + BMS_SET_CHARGE_ALLOWED, + BMS_SET_BALANCE_OVERRIDE, + BMS_RESET_COUNTERS, + BMS_FORCE_BALANCE, + BMS_ZERO_CURRENT_OFFSET, + + // Firmware update commands for different hardware types + JUMP_TO_BOOTLOADER_HW, + ERASE_NEW_APP_HW, + WRITE_NEW_APP_DATA_HW, + ERASE_BOOTLOADER_HW, + JUMP_TO_BOOTLOADER_ALL_CAN_HW, + ERASE_NEW_APP_ALL_CAN_HW, + WRITE_NEW_APP_DATA_ALL_CAN_HW, + ERASE_BOOTLOADER_ALL_CAN_HW, + + SET_ODOMETER, +}; + +static constexpr uint16_t CRC_TABLE[256] = {0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x02b1, 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0}; + +struct VescVersion { + uint8_t version_major; + uint8_t version_minor; + // char hardware_name[50]; + // uint8_t stm32_uuid_8[12]; + // bool pairing_done; + // uint8_t test_version_number; + // uint8_t hardware_type; + // uint8_t custom_configuration; +}; + +struct VescValues { + float fet_temperature; + float motor_temperature; + float motor_current; + float input_current; + float reset_average_id; + float reset_average_iq; + float duty_cycle; + int32_t rpm; + float input_voltage; + float used_charge_Ah; + float charged_charge_Ah; + float used_energy_Wh; + float charged_energy_wh; + int32_t tachometer; + int32_t tachometer_absolute; + int8_t fault; + float position_pid; + int8_t controller_id; + float ntc_temperature_mos1; + float ntc_temperature_mos2; + float ntc_temperature_mos3; + float read_reset_average_vd; + float read_reset_average_vq; +}; diff --git a/src/drivers/vesc/VescDriver/VescWritable.hpp b/src/drivers/vesc/VescDriver/VescWritable.hpp new file mode 100644 index 0000000000..a42b84e9dd --- /dev/null +++ b/src/drivers/vesc/VescDriver/VescWritable.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VescWritable.hpp + * @brief Interface calls defining the write callback for VESC communication + * @details To keep the driver cross-platform compatible the device owner class + * needs to implement this interface and pass itself to the driver via constructor + * such that writeCallback() can be used by the driver to send out data. + * @author Matthias Grob + */ + +#include +#include + +class VescWritable +{ +public: + VescWritable() = default; + ~VescWritable() = default; + virtual size_t writeCallback(const uint8_t *buffer, const uint16_t length) = 0; + +private: +}; diff --git a/src/drivers/vesc/VescSerialDevice.cpp b/src/drivers/vesc/VescSerialDevice.cpp new file mode 100644 index 0000000000..ea7c1cb2cf --- /dev/null +++ b/src/drivers/vesc/VescSerialDevice.cpp @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VescSerialDevice.cpp + * @author Matthias Grob + */ + +#include "VescSerialDevice.hpp" +#include +#include +#include + +using namespace time_literals; + +VescDevice::VescDevice(const char *port) : + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _vesc_driver(this) +{ + // Store port name + strncpy(_port, port, sizeof(_port) - 1); + + // Enforce null termination + _port[sizeof(_port) - 1] = '\0'; +} + +int VescDevice::init() +{ + int ret = setBaudrate(115200); + + if (ret >= 0) { + ScheduleOnInterval(10_ms); + } + + return OK; +} + +void VescDevice::print_info() +{ + printf("Using port '%s'\n", _port); + printf("VESC version: %d.%d\n", _vesc_driver.getVersionMajor(), _vesc_driver.getVersionMinor()); +} + +void VescDevice::Run() +{ + if (_serial_fd < 0) { + // Reopen fd from the work queue context + _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + } + + _vesc_driver.requestFirmwareVersion(); + _vesc_driver.commandCurrent(.5f); + + // Check the number of bytes available in the buffer + int bytes_available{0}; + int ret = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + printf("ioctl: %d %d\n", ret, _serial_fd); + + while (ret >= 0 && bytes_available > 0) { + printf("read: "); + uint8_t readbuf[READ_BUFFER_SIZE] {}; + + // Read from the uart buffer + ret = ::read(_serial_fd, readbuf, READ_BUFFER_SIZE); + + for (int i = 0; i < ret; i++) { + printf("%02X ", readbuf[i]); + } + + printf("\n"); + + for (int i = 0; i < ret; i++) { + _vesc_driver.parseInputByte(readbuf[i]); + } + + bytes_available -= ret; + } + + // TODO: ::close(_serial_fd); on exit +} + +int VescDevice::setBaudrate(const unsigned baudrate) +{ + // Open fd in main task context + _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + + // Translate baud rate to define + int speed; + + switch (baudrate) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + + default: + PX4_ERR("ERR: unknown baudrate: %d", baudrate); + return -EINVAL; + } + + // Fill the struct for the new configuration + struct termios uart_config; + tcgetattr(_serial_fd, &uart_config); + + // Configure the terminal (see https://en.wikibooks.org/wiki/Serial_Programming/termios) + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + // No parity, one stop bit, disable flow control + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + // Apply configuration and baud rate + int termios_state; + + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return PX4_ERROR; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return PX4_ERROR; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return PX4_ERROR; + } + + // Close fd in main task context + ::close(_serial_fd); + _serial_fd = -1; + + return PX4_OK; +} + +size_t VescDevice::writeCallback(const uint8_t *buffer, const uint16_t length) +{ + printf("write: "); + + for (int i = 0; i < length; i++) { + printf("%02X ", buffer[i]); + } + + printf("\n"); + int ret = ::write(_serial_fd, buffer, length); + printf("write result: %d %d\n", ret, _serial_fd); + return ret; +} diff --git a/src/drivers/vesc/VescSerialDevice.hpp b/src/drivers/vesc/VescSerialDevice.hpp new file mode 100644 index 0000000000..6e8452dbee --- /dev/null +++ b/src/drivers/vesc/VescSerialDevice.hpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file VescSerialDevice.hpp + * @brief PX4 serial motor output device using VescDriver for core protocol + * @author Matthias Grob + */ + +#include "VescDriver/VescDriver.hpp" +#include + +class VescDevice : public px4::ScheduledWorkItem, public VescWritable +{ +public: + VescDevice(const char *port); + ~VescDevice() = default; + int init(); + void print_info(); + +private: + void Run(); + size_t writeCallback(const uint8_t *buffer, const uint16_t length) override; + int setBaudrate(const unsigned baudrate); + + static constexpr size_t READ_BUFFER_SIZE{256}; + char _port[20] {}; + int _serial_fd{-1}; + VescDriver _vesc_driver; +}; diff --git a/src/drivers/vesc/module.yaml b/src/drivers/vesc/module.yaml new file mode 100644 index 0000000000..8b47f434e2 --- /dev/null +++ b/src/drivers/vesc/module.yaml @@ -0,0 +1,6 @@ +module_name: VESC motor contoller +serial_config: + - command: vesc start -d ${SERIAL_DEV} + port_config_param: + name: VESC_PORT + group: VESC diff --git a/src/drivers/vesc/vesc_main.cpp b/src/drivers/vesc/vesc_main.cpp new file mode 100644 index 0000000000..2ceec2c7fe --- /dev/null +++ b/src/drivers/vesc/vesc_main.cpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VescSerialDevice.hpp" +#include +#include +#include +#include + +// Local functions in support of the shell command +namespace vesc +{ + +VescDevice *g_dev{nullptr}; + +int start(const char *port); +int status(); +int stop(); +int usage(); + +int +start(const char *port) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_OK; + } + + // Instantiate the driver + g_dev = new VescDevice(port); + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return PX4_ERROR; + } + + if (OK != g_dev->init()) { + PX4_ERR("driver start failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} + +int +status() +{ + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return 1; + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + return 0; +} + +int stop() +{ + if (g_dev != nullptr) { + PX4_INFO("stopping driver"); + delete g_dev; + g_dev = nullptr; + PX4_INFO("driver stopped"); + + } else { + PX4_ERR("driver not running"); + return 1; + } + + return PX4_OK; +} + +int +usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial driver for VESC motor contollers + +Most boards are configured to enable/start the driver on a specified UART using the VESC_PORT parameter. + +Setup/usage information to be added. + +### Examples + +Attempt to start driver on a specified serial device. +$ vesc start -d /dev/ttyS1 +Stop driver +$ vesc stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vesc", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("status","Driver status"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status"); + return PX4_OK; +} + +} // namespace + +extern "C" __EXPORT int vesc_main(int argc, char *argv[]) +{ + int ch{0}; + const char *device_path{nullptr}; + int myoptind{1}; + const char *myoptarg{nullptr}; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_path = myoptarg; + break; + + default: + PX4_WARN("Unknown option"); + return PX4_ERROR; + } + } + + if (myoptind >= argc) { + PX4_ERR("Unrecognized command"); + return vesc::usage(); + } + + if (!strcmp(argv[myoptind], "start")) { + if (strcmp(device_path, "") != 0) { + return vesc::start(device_path); + + } else { + PX4_WARN("Please specify device path"); + return vesc::usage(); + } + + } else if (!strcmp(argv[myoptind], "stop")) { + return vesc::stop(); + + } else if (!strcmp(argv[myoptind], "status")) { + return vesc::status(); + } + + return vesc::usage(); +}