TEMP

ROMFS/px4fmu_common/init.d/rc.interface

@@ -144,6 +144,11 @@ then
 		set OUTPUT_DEV /dev/uavcan/esc
 	fi
 
+	if [ $OUTPUT_MODE = vesc ]
+	then
+		set OUTPUT_DEV /dev/vesc
+	fi
+
 	if mixer load ${OUTPUT_DEV} ${MIXER_FILE}
 	then
 		echo "INFO  [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}"

ROMFS/px4fmu_common/init.d/rcS

@@ -282,6 +282,11 @@ else
 		fi
 	fi
 
+	if param greater -s VESC_PORT 0
+	then
+		set OUTPUT_MODE vesc
+	fi
+
 	#
 	# Check if PX4IO present and update firmware if needed.
 	# Assumption IOFW set to firmware file and IO_PRESENT = no

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

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:
  *

src/drivers/dshot/DShot.cpp

@@ -43,7 +43,6 @@ DShot::DShot() :
 	_mixing_output.setAllDisarmedValues(DSHOT_DISARM_VALUE);
 	_mixing_output.setAllMinValues(DSHOT_MIN_THROTTLE);
 	_mixing_output.setAllMaxValues(DSHOT_MAX_THROTTLE);
-
 }
 
 DShot::~DShot()

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
+	)

src/drivers/vesc/VescDriver/VescDriver.cpp

@@ -0,0 +1,285 @@
+/****************************************************************************
+ *
+ *   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 <maetugr@gmail.com>
+ */
+
+#include "VescDriver.hpp"
+#include <memory.h>
+
+void VescDriver::commandDutyCycle(float duty_cycle, const uint8_t forward_can_id)
+{
+	uint8_t command[5] {VescCommand::SET_DUTY};
+	uint16_t index{1};
+	insertInt32(command, index, static_cast<int32_t>(duty_cycle * 100000.f));
+	sendPayload(command, 5, forward_can_id);
+}
+
+void VescDriver::commandCurrent(float current, const uint8_t forward_can_id)
+{
+	uint8_t command[5] {VescCommand::SET_CURRENT};
+	uint16_t index{1};
+	insertInt32(command, index, static_cast<int32_t>(current * 1000.f));
+	sendPayload(command, 5, forward_can_id);
+}
+
+void VescDriver::commandBrakeCurrent(float current, const uint8_t forward_can_id)
+{
+	uint8_t command[5] {VescCommand::SET_CURRENT_BRAKE};
+	uint16_t index{1};
+	insertInt32(command, index, static_cast<int32_t>(current * 1000.f));
+	sendPayload(command, 5, forward_can_id);
+}
+
+void VescDriver::requestFirmwareVersion()
+{
+	uint8_t command{VescCommand::FW_VERSION};
+	sendPayload(&command, 1);
+}
+
+void VescDriver::requestValues()
+{
+	uint8_t command{VescCommand::GET_VALUES};
+	sendPayload(&command, 1);
+}
+
+size_t VescDriver::sendPayload(const uint8_t *payload, const uint16_t payload_length, const uint8_t forward_can_id)
+{
+	if (forward_can_id == 0) {
+		return sendPacket(payload, payload_length);
+
+	} else {
+		uint8_t command[2 + payload_length] {VescCommand::FORWARD_CAN, forward_can_id};
+		uint16_t index{2};
+		memcpy(&command[index], payload, payload_length);
+		index += payload_length;
+		return sendPacket(command, 2 + payload_length);
+	}
+}
+
+size_t VescDriver::sendPacket(const uint8_t *payload, uint16_t payload_length, const uint8_t can_forwarding_id)
+{
+	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<uint8_t>(payload_length >> 8);
+		packet_buffer[index++] = static_cast<uint8_t>(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<uint8_t>(crc >> 8);
+	packet_buffer[index++] = static_cast<uint8_t>(crc & 0xFF);
+
+	// Stop byte
+	packet_buffer[index++] = 3;
+
+	// Write bytes out through callback interface
+	return _vesc_writable.writeCallback(packet_buffer, index);
+}
+
+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<const char *>(&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<int16_t>(buffer[index - 2] << 8 | buffer[index - 1]);
+}
+
+float VescDriver::extractFloat16(const uint8_t *buffer, uint16_t &index)
+{
+	return static_cast<float>(extractInt16(buffer, index));
+}
+
+int32_t VescDriver::extractInt32(const uint8_t *buffer, uint16_t &index)
+{
+	index += 4;
+	return static_cast<int32_t>(
+		       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<float>(extractInt32(buffer, index));
+}

src/drivers/vesc/VescDriver/VescDriver.hpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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 <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include "VescProtocol.h"
+#include "VescWritableInterface.hpp"
+#include <stdint.h>
+#include <stdio.h>
+
+class VescDriver
+{
+public:
+	VescDriver(VescWritableInterface &vesc_writable) : _vesc_writable(vesc_writable) {};
+	~VescDriver() = default;
+
+	void commandDutyCycle(float duty_cycle, const uint8_t forward_can_id = 0);
+	void commandCurrent(float current, const uint8_t forward_can_id = 0);
+	void commandBrakeCurrent(float current, const uint8_t forward_can_id = 0);
+
+	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 sendPayload(const uint8_t *payload, const uint16_t payload_length, const uint8_t forward_can_id = 0);
+	size_t sendPacket(const uint8_t *payload, uint16_t payload_length, const uint8_t can_forwarding_id = 0);
+	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
+	VescWritableInterface &_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{};
+};

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 <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+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;
+};

src/drivers/vesc/VescDriver/VescWritableInterface.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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 VescWritableInterface.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 <maetugr@gmail.com>
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+class VescWritableInterface
+{
+public:
+	virtual size_t writeCallback(const uint8_t *buffer, const uint16_t length) = 0;
+};

src/drivers/vesc/VescSerialDevice.cpp

@@ -0,0 +1,303 @@
+/****************************************************************************
+ *
+ *   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 <maetugr@gmail.com>
+ */
+
+#include "VescSerialDevice.hpp"
+#include <stdint.h>
+#include <termios.h>
+#include <lib/drivers/device/Device.hpp>
+
+using namespace time_literals;
+
+VescDevice::VescDevice(const char *port) :
+	CDev("/dev/vesc"),
+	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
+	_vesc_driver(*this)
+{
+	// Store port name
+	strncpy(_port, port, sizeof(_port) - 1);
+
+	// Enforce null termination
+	_port[sizeof(_port) - 1] = '\0';
+
+	_mixing_output.setAllDisarmedValues(DISARM_VALUE);
+	_mixing_output.setAllMinValues(MIN_THROTTLE);
+	_mixing_output.setAllMaxValues(MAX_THROTTLE);
+}
+
+VescDevice::~VescDevice()
+{
+	perf_free(_cycle_perf);
+}
+
+int VescDevice::init()
+{
+	// Do regular cdev init
+	int ret = CDev::init();
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	// Set Baudrate and schedule for the first time
+	ret = setBaudrate(115200);
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	ScheduleNow();
+	return OK;
+}
+
+void VescDevice::printStatus()
+{
+	printf("Using port '%s'\n", _port);
+	printf("VESC version: %d.%d\n", _vesc_driver.getVersionMajor(), _vesc_driver.getVersionMinor());
+	_mixing_output.printStatus();
+	perf_print_counter(_cycle_perf);
+}
+
+bool VescDevice::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+			       unsigned num_control_groups_updated)
+{
+	// for (unsigned i = 0; i < num_outputs; i++) {
+	// 	if (!stop_motors && outputs[i] != DISARM_VALUE) {
+	// 		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[i]) / 1e3f);
+	// 	}
+	// }
+
+	if (!stop_motors) {
+		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[0]) / 1e3f);
+		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[1]) / 1e3f, 11);
+		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[2]) / 1e3f, 7);
+		_vesc_driver.commandDutyCycle(static_cast<float>(outputs[3]) / 1e3f, 107);
+
+	} else {
+		_vesc_driver.commandDutyCycle(0.f);
+		_vesc_driver.commandDutyCycle(0.f, 11);
+		_vesc_driver.commandDutyCycle(0.f, 7);
+		_vesc_driver.commandDutyCycle(0.f, 107);
+	}
+
+	return true;
+}
+
+void VescDevice::Run()
+{
+	perf_begin(_cycle_perf);
+
+	if (_serial_fd < 0) {
+		// Reopen fd from the work queue context
+		_serial_fd = ::open(_port, O_RDWR | O_NOCTTY);
+		_vesc_driver.requestFirmwareVersion();
+	}
+
+	_mixing_output.update();
+
+	// Check the number of bytes available in the buffer
+	int bytes_available{0};
+	int ret = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available);
+
+	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;
+	}
+
+	_mixing_output.updateSubscriptions(true);
+
+	perf_end(_cycle_perf);
+}
+
+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;
+}
+
+int VescDevice::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg);
+
+	lock();
+
+	switch (cmd) {
+	case MIXERIOCRESET:
+		_mixing_output.resetMixerThreadSafe();
+		break;
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strlen(buf);
+			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	if (ret == -ENOTTY) {
+		ret = CDev::ioctl(filp, cmd, arg);
+	}
+
+	return ret;
+}

src/drivers/vesc/VescSerialDevice.hpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   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 <maetugr@gmail.com>
+ */
+
+#include "VescDriver/VescDriver.hpp"
+#include <drivers/device/device.h>
+#include <drivers/drv_mixer.h>
+#include <lib/cdev/CDev.hpp>
+#include <lib/mixer_module/mixer_module.hpp>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+class VescDevice : public VescWritableInterface, public cdev::CDev, public OutputModuleInterface
+{
+public:
+	VescDevice(const char *port);
+	~VescDevice();
+	int init();
+	void printStatus();
+	int ioctl(device::file_t *filp, int cmd, unsigned long arg);
+
+private:
+	void Run();
+	size_t writeCallback(const uint8_t *buffer, const uint16_t length) override;
+	int setBaudrate(const unsigned baudrate);
+	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+			   unsigned num_control_groups_updated) override;
+
+	static constexpr size_t READ_BUFFER_SIZE{256};
+	static constexpr int DISARM_VALUE = 0;
+	static constexpr int MIN_THROTTLE = 100;
+	static constexpr int MAX_THROTTLE = 1000;
+
+	char _port[20] {};
+	int _serial_fd{-1};
+	VescDriver _vesc_driver;
+	MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+};

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

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 <px4_platform_common/defines.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+
+// 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->printStatus();
+
+	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();
+}

src/lib/mixer_module/mixer_module.cpp

@@ -439,8 +439,7 @@ bool MixingOutput::update()
 	return true;
 }
 
-void
-MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
+void MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs)
 {
 	actuator_outputs.noutputs = num_outputs;
 
@@ -452,8 +451,7 @@ MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_output
 	_outputs_pub.publish(actuator_outputs);
 }
 
-void
-MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
+void MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
 {
 	MultirotorMixer::saturation_status saturation_status;
 	saturation_status.value = _mixers->get_saturation_status();
@@ -467,8 +465,7 @@ MixingOutput::publishMixerStatus(const actuator_outputs_s &actuator_outputs)
 	}
 }
 
-void
-MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs)
+void MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs)
 {
 	// use first valid timestamp_sample for latency tracking
 	for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
@@ -482,37 +479,27 @@ MixingOutput::updateLatencyPerfCounter(const actuator_outputs_s &actuator_output
 	}
 }
 
-void
-MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
+void MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
 {
 	if (MAX_ACTUATORS < 4) {
 		return;
 	}
 
-	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
-		/*
-		 * Betaflight default motor ordering:
-		 * 4     2
-		 *    ^
-		 * 3     1
-		 */
-		const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3] };
-		values[0] = value_tmp[3];
-		values[1] = value_tmp[0];
-		values[2] = value_tmp[1];
-		values[3] = value_tmp[2];
-	}
-
-	/* else: PX4, no need to reorder
-	 * 3     1
-	 *    ^
-	 * 2     4
-	 */
+	const uint16_t value_tmp[4] = {values[0], values[1], values[2], values[3]};
+	values[reorderedMotorIndex(0)] = value_tmp[0];
+	values[reorderedMotorIndex(1)] = value_tmp[1];
+	values[reorderedMotorIndex(2)] = value_tmp[2];
+	values[reorderedMotorIndex(3)] = value_tmp[3];
 }
 
 int MixingOutput::reorderedMotorIndex(int index) const
 {
 	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
+		/* Betaflight default motor ordering:
+		 * 4     2
+		 *    ^
+		 * 3     1
+		 */
 		switch (index) {
 		case 0: return 1;
 
@@ -524,6 +511,29 @@ int MixingOutput::reorderedMotorIndex(int index) const
 		}
 	}
 
+	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Clockwise) {
+		/* Clockwise motor ordering:
+		 * 4     1
+		 *    ^
+		 * 3     2
+		 */
+		switch (index) {
+		case 0: return 0;
+
+		case 1: return 2;
+
+		case 2: return 3;
+
+		case 3: return 1;
+		}
+	}
+
+	/* else: PX4, no need to reorder
+	 * 3     1
+	 *    ^
+	 * 2     4
+	 */
+
 	return index;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -203,7 +203,8 @@ private:
 
 	enum class MotorOrdering : int32_t {
 		PX4 = 0,
-		Betaflight = 1
+		Betaflight = 1,
+		Clockwise = 2
 	};
 
 	struct Command {
@@ -281,6 +282,5 @@ private:
 		(ParamFloat<px4::params::MOT_SLEW_MAX>) _param_mot_slew_max,
 		(ParamFloat<px4::params::THR_MDL_FAC>) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor
 		(ParamInt<px4::params::MOT_ORDERING>) _param_mot_ordering
-
 	)
 };

src/lib/mixer_module/params.c

@@ -29,6 +29,7 @@ PARAM_DEFINE_INT32(MC_AIRMODE, 0);
  *
  * @value 0 PX4
  * @value 1 Betaflight / Cleanflight
+ * @value 2 Clockwise
  *
  * @group Mixer Output
  */