mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-07 23:21:35 +08:00
Initial test
This commit is contained in:
Matthias Grob
@@ -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
|
||||
|
||||
@@ -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:
|
||||
*
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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 <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#include "VescDriver.hpp"
|
||||
#include <memory.h>
|
||||
|
||||
void VescDriver::commandCurrent(float current)
|
||||
{
|
||||
uint8_t command[5] {VescCommand::SET_CURRENT};
|
||||
uint16_t index{1};
|
||||
insertInt32(command, index, static_cast<int32_t>(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<int32_t>(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<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
|
||||
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<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));
|
||||
}
|
||||
@@ -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 <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "VescProtocol.h"
|
||||
#include "VescWritable.hpp"
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
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{};
|
||||
};
|
||||
@@ -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;
|
||||
};
|
||||
@@ -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 <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
|
||||
class VescWritable
|
||||
{
|
||||
public:
|
||||
VescWritable() = default;
|
||||
~VescWritable() = default;
|
||||
virtual size_t writeCallback(const uint8_t *buffer, const uint16_t length) = 0;
|
||||
|
||||
private:
|
||||
};
|
||||
@@ -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 <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) :
|
||||
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;
|
||||
}
|
||||
@@ -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 <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#include "VescDriver/VescDriver.hpp"
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
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;
|
||||
};
|
||||
@@ -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
|
||||
@@ -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->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();
|
||||
}
|
||||
Reference in New Issue
Block a user