forked from Archive/PX4-Autopilot
dshot: add telemetry and publish esc_status message
This commit is contained in:
Beat Küng
parent
d44302c03b
commit
65f3c7f93d
|
|
@ -3,9 +3,10 @@ uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013)
|
|||
|
||||
uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC
|
||||
uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC
|
||||
uint8 ESC_CONNECTION_TYPE_ONESHOOT = 2 # One Shoot PPM
|
||||
uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM
|
||||
uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C
|
||||
uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus
|
||||
uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot
|
||||
|
||||
uint16 counter # incremented by the writing thread everytime new data is stored
|
||||
|
||||
|
|
|
|||
|
|
@ -36,10 +36,13 @@ px4_add_module(
|
|||
STACK_MAIN 1200
|
||||
SRCS
|
||||
dshot.cpp
|
||||
telemetry.cpp
|
||||
DEPENDS
|
||||
arch_io_pins
|
||||
arch_dshot
|
||||
mixer
|
||||
mixer_module
|
||||
output_limit
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
)
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@
|
|||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_arch/dshot.h>
|
||||
#include <px4_atomic.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_getopt.h>
|
||||
#include <px4_log.h>
|
||||
|
|
@ -60,6 +61,9 @@
|
|||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
#include "telemetry.h"
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
|
@ -148,6 +152,8 @@ public:
|
|||
void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) override;
|
||||
|
||||
void mixerChanged() override;
|
||||
|
||||
private:
|
||||
enum class DShotConfig {
|
||||
Disabled = 0,
|
||||
|
|
@ -157,8 +163,22 @@ private:
|
|||
DShot1200 = 1200,
|
||||
};
|
||||
|
||||
struct Telemetry {
|
||||
DShotTelemetry handler;
|
||||
uORB::PublicationData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
|
||||
int last_motor_index{-1};
|
||||
};
|
||||
|
||||
void updateTelemetryNumMotors();
|
||||
void initTelemetry(const char *device);
|
||||
void handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data);
|
||||
|
||||
MixingOutput _mixing_output{*this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
Telemetry *_telemetry{nullptr};
|
||||
static char _telemetry_device[20];
|
||||
static px4::atomic_bool _request_telemetry_init;
|
||||
|
||||
Mode _mode{MODE_NONE};
|
||||
|
||||
uORB::Subscription _param_sub{ORB_ID(parameter_update)};
|
||||
|
|
@ -188,10 +208,14 @@ private:
|
|||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::DSHOT_CONFIG>) _param_dshot_config,
|
||||
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min
|
||||
(ParamFloat<px4::params::DSHOT_MIN>) _param_dshot_min,
|
||||
(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
|
||||
)
|
||||
};
|
||||
|
||||
char DShotOutput::_telemetry_device[] {};
|
||||
px4::atomic_bool DShotOutput::_request_telemetry_init{false};
|
||||
|
||||
DShotOutput::DShotOutput() :
|
||||
CDev("/dev/dshot"),
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
|
|
@ -214,6 +238,7 @@ DShotOutput::~DShotOutput()
|
|||
|
||||
perf_free(_cycle_perf);
|
||||
perf_free(_cycle_interval_perf);
|
||||
delete _telemetry;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -484,6 +509,79 @@ DShotOutput::update_dshot_out_state(bool on)
|
|||
up_dshot_arm(on);
|
||||
}
|
||||
|
||||
void DShotOutput::updateTelemetryNumMotors()
|
||||
{
|
||||
if (!_telemetry) {
|
||||
return;
|
||||
}
|
||||
|
||||
int motor_count = 0;
|
||||
|
||||
if (_mixing_output.mixers()) {
|
||||
motor_count = _mixing_output.mixers()->get_multirotor_count();
|
||||
}
|
||||
|
||||
_telemetry->handler.setNumMotors(motor_count);
|
||||
}
|
||||
|
||||
void DShotOutput::initTelemetry(const char *device)
|
||||
{
|
||||
if (!_telemetry) {
|
||||
_telemetry = new Telemetry{};
|
||||
|
||||
if (!_telemetry) {
|
||||
PX4_ERR("alloc failed");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
int ret = _telemetry->handler.init(device);
|
||||
|
||||
if (ret != 0) {
|
||||
PX4_ERR("telemetry init failed (%i)", ret);
|
||||
}
|
||||
|
||||
updateTelemetryNumMotors();
|
||||
}
|
||||
|
||||
void DShotOutput::handleNewTelemetryData(int motor_index, const DShotTelemetry::EscData &data)
|
||||
{
|
||||
// fill in new motor data
|
||||
esc_status_s &esc_status = _telemetry->esc_status_pub.get();
|
||||
|
||||
if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
|
||||
esc_status.esc_online_flags |= 1 << motor_index;
|
||||
esc_status.esc[motor_index].timestamp = data.time;
|
||||
esc_status.esc[motor_index].esc_rpm = ((int)data.erpm * 100) / (_param_mot_pole_count.get() / 2);
|
||||
esc_status.esc[motor_index].esc_voltage = (float)data.voltage * 0.01f;
|
||||
esc_status.esc[motor_index].esc_current = (float)data.current * 0.01f;
|
||||
esc_status.esc[motor_index].esc_temperature = data.temperature;
|
||||
// TODO: accumulate consumption and use for battery estimation
|
||||
}
|
||||
|
||||
// publish when motor index wraps (which is robust against motor timeouts)
|
||||
if (motor_index < _telemetry->last_motor_index) {
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
|
||||
esc_status.esc_count = _telemetry->handler.numMotors();
|
||||
++esc_status.counter;
|
||||
// FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout
|
||||
esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1;
|
||||
|
||||
_telemetry->esc_status_pub.update();
|
||||
|
||||
// reset esc data (in case a motor times out, so we won't send stale data)
|
||||
memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc));
|
||||
esc_status.esc_online_flags = 0;
|
||||
}
|
||||
|
||||
_telemetry->last_motor_index = motor_index;
|
||||
}
|
||||
|
||||
void DShotOutput::mixerChanged()
|
||||
{
|
||||
updateTelemetryNumMotors();
|
||||
}
|
||||
|
||||
void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated)
|
||||
|
|
@ -494,6 +592,10 @@ void DShotOutput::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
|
|||
|
||||
int requested_telemetry_index = -1;
|
||||
|
||||
if (_telemetry) {
|
||||
requested_telemetry_index = _mixing_output.reorderedMotorIndex(_telemetry->handler.getRequestMotorIndex());
|
||||
}
|
||||
|
||||
if (stop_motors) {
|
||||
|
||||
for (int i = 0; i < (int)num_outputs; i++) {
|
||||
|
|
@ -534,10 +636,24 @@ DShotOutput::Run()
|
|||
update_dshot_out_state(outputs_on);
|
||||
}
|
||||
|
||||
if (_telemetry) {
|
||||
int telem_update = _telemetry->handler.update();
|
||||
|
||||
if (telem_update >= 0) {
|
||||
handleNewTelemetryData(telem_update, _telemetry->handler.latestESCData());
|
||||
}
|
||||
}
|
||||
|
||||
if (_param_sub.updated()) {
|
||||
update_params();
|
||||
}
|
||||
|
||||
// telemetry device update request?
|
||||
if (_request_telemetry_init.load()) {
|
||||
initTelemetry(_telemetry_device);
|
||||
_request_telemetry_init.store(false);
|
||||
}
|
||||
|
||||
// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
|
||||
_mixing_output.updateSubscriptions(true);
|
||||
|
||||
|
|
@ -1108,6 +1224,17 @@ int DShotOutput::custom_command(int argc, char *argv[])
|
|||
PortMode new_mode = PORT_MODE_UNSET;
|
||||
const char *verb = argv[0];
|
||||
|
||||
if (!strcmp(verb, "telemetry")) {
|
||||
if (argc > 1) {
|
||||
// telemetry can be requested before the module is started
|
||||
strncpy(_telemetry_device, argv[1], sizeof(_telemetry_device) - 1);
|
||||
_telemetry_device[sizeof(_telemetry_device) - 1] = '\0';
|
||||
_request_telemetry_init.store(true);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* start the FMU if not running */
|
||||
if (!is_running()) {
|
||||
int ret = DShotOutput::task_spawn(argc, argv);
|
||||
|
|
@ -1234,6 +1361,9 @@ to use DShot as ESC communication protocol instead of PWM.
|
|||
PRINT_MODULE_USAGE_COMMAND("mode_pwm1");
|
||||
#endif
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("telemetry", "Enable Telemetry on a UART");
|
||||
PRINT_MODULE_USAGE_ARG("<device>", "UART device", false);
|
||||
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
|
|
@ -1290,6 +1420,9 @@ int DShotOutput::print_status()
|
|||
perf_print_counter(_cycle_perf);
|
||||
perf_print_counter(_cycle_interval_perf);
|
||||
_mixing_output.printStatus();
|
||||
if (_telemetry) {
|
||||
PX4_INFO("telemetry on: %s", _telemetry_device);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,54 @@
|
|||
module_name: DShot Driver
|
||||
serial_config:
|
||||
- command: dshot telemetry ${SERIAL_DEV}
|
||||
port_config_param:
|
||||
name: DSHOT_TEL_CFG
|
||||
group: DShot
|
||||
|
||||
parameters:
|
||||
- group: DShot
|
||||
definitions:
|
||||
DSHOT_CONFIG:
|
||||
description:
|
||||
short: Configure DShot
|
||||
long: |
|
||||
This enables/disables DShot. The different modes define different
|
||||
speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes.
|
||||
|
||||
Note: this enables DShot on the FMU outputs. For boards with an IO it is the
|
||||
AUX outputs.
|
||||
type: enum
|
||||
values:
|
||||
0: Disable (use PWM/Oneshot)
|
||||
150: DShot150
|
||||
300: DShot300
|
||||
600: DShot600
|
||||
1200: DShot1200
|
||||
reboot_required: true
|
||||
default: 0
|
||||
DSHOT_MIN:
|
||||
description:
|
||||
short: Minimum DShot Motor Output
|
||||
long: |
|
||||
Minimum Output Value for DShot in percent. The value depends on the ESC. Make
|
||||
sure to set this high enough so that the motors are always spinning while
|
||||
armed.
|
||||
type: float
|
||||
unit: '%'
|
||||
min: 0
|
||||
max: 1
|
||||
decimal: 2
|
||||
increment: 0.01
|
||||
default: 0.055
|
||||
MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group
|
||||
description:
|
||||
short: Number of magnetic poles of the motors
|
||||
long: |
|
||||
Specify the number of magnetic poles of the motors.
|
||||
It is required to compute the RPM value from the eRPM returned with the ESC telemetry.
|
||||
|
||||
Either get the number from the motor spec sheet or count the magnets on the bell of the motor (not the stator magnets).
|
||||
Typical motors for 5 inch props have 14 poles.
|
||||
type: int32
|
||||
default: 14
|
||||
|
||||
|
|
@ -0,0 +1,265 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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 "telemetry.h"
|
||||
|
||||
#include <px4_log.h>
|
||||
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <errno.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#define DSHOT_TELEMETRY_UART_BAUDRATE 115200
|
||||
|
||||
DShotTelemetry::~DShotTelemetry()
|
||||
{
|
||||
deinit();
|
||||
}
|
||||
|
||||
int DShotTelemetry::init(const char *uart_device)
|
||||
{
|
||||
deinit();
|
||||
_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("failed to open serial port: %s err: %d", uart_device, errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
_current_motor_index_request = -1;
|
||||
return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
|
||||
}
|
||||
|
||||
void DShotTelemetry::deinit()
|
||||
{
|
||||
if (_uart_fd >= 0) {
|
||||
close(_uart_fd);
|
||||
_uart_fd = -1;
|
||||
}
|
||||
}
|
||||
|
||||
int DShotTelemetry::update()
|
||||
{
|
||||
if (_uart_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (_current_motor_index_request == -1) {
|
||||
// nothing in progress, start a request
|
||||
_current_motor_index_request = 0;
|
||||
_current_request_start = 0;
|
||||
_frame_position = 0;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// read from the uart. This must be non-blocking, so check first if there is data available
|
||||
int bytes_available = 0;
|
||||
int ret = ioctl(_uart_fd, FIONREAD, (unsigned long)&bytes_available);
|
||||
|
||||
if (ret != 0 || bytes_available <= 0) {
|
||||
// no data available. Check for a timeout
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
if (_current_request_start > 0 && now - _current_request_start > 30_ms) {
|
||||
PX4_DEBUG("ESC telemetry timeout for motor %i (frame pos=%i)", _current_motor_index_request, _frame_position);
|
||||
requestNextMotor();
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
const int buf_length = ESC_FRAME_SIZE;
|
||||
uint8_t buf[buf_length];
|
||||
|
||||
int num_read = read(_uart_fd, buf, buf_length);
|
||||
ret = -1;
|
||||
|
||||
for (int i = 0; i < num_read; ++i) {
|
||||
if (decodeByte(buf[i])) {
|
||||
ret = _current_motor_index_request;
|
||||
requestNextMotor();
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool DShotTelemetry::decodeByte(uint8_t byte)
|
||||
{
|
||||
_frame_buffer[_frame_position++] = byte;
|
||||
|
||||
if (_frame_position == ESC_FRAME_SIZE) {
|
||||
PX4_DEBUG("got ESC frame for motor %i", _current_motor_index_request);
|
||||
uint8_t checksum = crc8(_frame_buffer, ESC_FRAME_SIZE - 1);
|
||||
uint8_t checksum_data = _frame_buffer[ESC_FRAME_SIZE - 1];
|
||||
|
||||
if (checksum == checksum_data) {
|
||||
_latest_data.time = hrt_absolute_time();
|
||||
_latest_data.temperature = _frame_buffer[0];
|
||||
_latest_data.voltage = (_frame_buffer[1] << 8) | _frame_buffer[2];
|
||||
_latest_data.current = (_frame_buffer[3] << 8) | _frame_buffer[4];
|
||||
_latest_data.consumption = (_frame_buffer[5]) << 8 | _frame_buffer[6];
|
||||
_latest_data.erpm = (_frame_buffer[7] << 8) | _frame_buffer[8];
|
||||
PX4_DEBUG("Motor %i: temp=%i, V=%i, cur=%i, consumpt=%i, rpm=%i", _current_motor_index_request,
|
||||
_latest_data.temperature, _latest_data.voltage, _latest_data.current, _latest_data.consumption,
|
||||
_latest_data.erpm);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed)
|
||||
{
|
||||
uint8_t crc_u = crc ^ crc_seed;
|
||||
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
|
||||
}
|
||||
|
||||
return crc_u;
|
||||
}
|
||||
|
||||
|
||||
uint8_t DShotTelemetry::crc8(const uint8_t *buf, uint8_t len)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
|
||||
for (int i = 0; i < len; ++i) {
|
||||
crc = updateCrc8(buf[i], crc);
|
||||
}
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
void DShotTelemetry::requestNextMotor()
|
||||
{
|
||||
_current_motor_index_request = (_current_motor_index_request + 1) % _num_motors;
|
||||
_current_request_start = 0;
|
||||
_frame_position = 0;
|
||||
}
|
||||
|
||||
int DShotTelemetry::getRequestMotorIndex()
|
||||
{
|
||||
if (_current_request_start != 0) {
|
||||
// already in progress, do not send another request
|
||||
return -1;
|
||||
}
|
||||
|
||||
_current_request_start = hrt_absolute_time();
|
||||
return _current_motor_index_request;
|
||||
}
|
||||
|
||||
int DShotTelemetry::setBaudrate(unsigned baud)
|
||||
{
|
||||
int speed;
|
||||
|
||||
switch (baud) {
|
||||
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;
|
||||
|
||||
default:
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_uart_fd, &uart_config);
|
||||
|
||||
//
|
||||
// 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);
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -31,39 +31,69 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* Configure DShot
|
||||
*
|
||||
* This enabled/disabled DShot. The different modes define different
|
||||
* speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes.
|
||||
*
|
||||
* Note: this enabled DShot on the FMU outputs. For boards with an IO it is the
|
||||
* AUX outputs.
|
||||
*
|
||||
* @value 0 Disable (use PWM/Oneshot)
|
||||
* @value 150 DShot150
|
||||
* @value 300 DShot300
|
||||
* @value 600 DShot600
|
||||
* @value 1200 DShot1200
|
||||
* @reboot_required true
|
||||
* @group DShot
|
||||
*/
|
||||
PARAM_DEFINE_INT32(DSHOT_CONFIG, 0);
|
||||
#pragma once
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
class DShotTelemetry
|
||||
{
|
||||
public:
|
||||
struct EscData {
|
||||
hrt_abstime time;
|
||||
int8_t temperature; ///< [deg C]
|
||||
int16_t voltage; ///< [0.01V]
|
||||
int16_t current; ///< [0.01A]
|
||||
int16_t consumption; ///< [mAh]
|
||||
int16_t erpm; ///< [100ERPM]
|
||||
};
|
||||
|
||||
~DShotTelemetry();
|
||||
|
||||
int init(const char *uart_device);
|
||||
|
||||
void deinit();
|
||||
|
||||
void setNumMotors(int num_motors) { _num_motors = num_motors; }
|
||||
int numMotors() const { return _num_motors; }
|
||||
|
||||
/**
|
||||
* Minimum DShot Motor Output
|
||||
*
|
||||
* Minimum Output Value for DShot in percent. The value depends on the ESC. Make
|
||||
* sure to set this high enough so that the motors are always spinning while
|
||||
* armed.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0
|
||||
* @max 1
|
||||
* @decimal 2
|
||||
* @increment 0.01
|
||||
* @group DShot
|
||||
* Read telemetry from the UART (non-blocking) and handle timeouts.
|
||||
* @return -1 if no update, >= 0 for the motor index. Use @latestESCData() to get the data.
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(DSHOT_MIN, 0.055f);
|
||||
int update();
|
||||
|
||||
/**
|
||||
* Get the motor index for which telemetry should be requested.
|
||||
* @return -1 if no request should be made, motor index otherwise
|
||||
*/
|
||||
int getRequestMotorIndex();
|
||||
|
||||
const EscData &latestESCData() const { return _latest_data; }
|
||||
|
||||
private:
|
||||
static constexpr int ESC_FRAME_SIZE = 10;
|
||||
|
||||
/**
|
||||
* set the Baudrate
|
||||
* @param baud
|
||||
* @return 0 on success, <0 on error
|
||||
*/
|
||||
int setBaudrate(unsigned baud);
|
||||
|
||||
void requestNextMotor();
|
||||
|
||||
bool decodeByte(uint8_t byte);
|
||||
|
||||
static inline uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed);
|
||||
static uint8_t crc8(const uint8_t *buf, uint8_t len);
|
||||
|
||||
int _uart_fd{-1};
|
||||
int _num_motors{0};
|
||||
uint8_t _frame_buffer[ESC_FRAME_SIZE];
|
||||
int _frame_position{0};
|
||||
|
||||
EscData _latest_data;
|
||||
|
||||
int _current_motor_index_request{-1};
|
||||
hrt_abstime _current_request_start{0};
|
||||
};
|
||||
|
|
@ -374,6 +374,23 @@ MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
|
|||
*/
|
||||
}
|
||||
|
||||
int MixingOutput::reorderedMotorIndex(int index)
|
||||
{
|
||||
if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
|
||||
switch (index) {
|
||||
case 0: return 1;
|
||||
|
||||
case 1: return 2;
|
||||
|
||||
case 2: return 3;
|
||||
|
||||
case 3: return 0;
|
||||
}
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
|
||||
{
|
||||
const MixingOutput *output = (const MixingOutput *)handle;
|
||||
|
|
@ -420,6 +437,8 @@ void MixingOutput::resetMixer()
|
|||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
}
|
||||
|
||||
_interface.mixerChanged();
|
||||
}
|
||||
|
||||
int MixingOutput::loadMixer(const char *buf, unsigned len)
|
||||
|
|
@ -447,6 +466,7 @@ int MixingOutput::loadMixer(const char *buf, unsigned len)
|
|||
PX4_DEBUG("loaded mixers \n%s\n", buf);
|
||||
|
||||
updateParams();
|
||||
_interface.mixerChanged();
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -66,6 +66,9 @@ public:
|
|||
|
||||
virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
unsigned num_outputs, unsigned num_control_groups_updated) = 0;
|
||||
|
||||
/** called whenever the mixer gets updated/reset */
|
||||
virtual void mixerChanged() {};
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -148,11 +151,18 @@ public:
|
|||
|
||||
uint16_t &reverseOutputMask() { return _reverse_output_mask; }
|
||||
uint16_t &failsafeValue(int index) { return _failsafe_value[index]; }
|
||||
/** Disarmed values: if ramp_up is true, then disarmedValue < minValue needs to hold */
|
||||
/** Disarmed values: disarmedValue < minValue needs to hold */
|
||||
uint16_t &disarmedValue(int index) { return _disarmed_value[index]; }
|
||||
uint16_t &minValue(int index) { return _min_value[index]; }
|
||||
uint16_t &maxValue(int index) { return _max_value[index]; }
|
||||
|
||||
/**
|
||||
* Get the motor index that maps from PX4 convention to the configured one
|
||||
* @param index motor index in [0, num_motors-1]
|
||||
* @return reordered motor index. When out of range, the input index is returned
|
||||
*/
|
||||
int reorderedMotorIndex(int index);
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue