Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_FETtecOneWire: A serial-based ESC protocol with telemetry

Browse Source 
Co-authored-by: Torsten Z <t.zunker@fettec.net>
Co-authored-by: Dr.-Ing. Amilcar do Carmo Lucas <amilcar.lucas@iav.de>
Co-authored-by: Peter Barker <pb-gh@barker.dropbear.id.au>

- uses ArduPilot's coding guidelines and naming conventions
- control motor speed
- Use the AP_ESC_Telem base class to:
  - copy ESC telemetry data into MAVLink telemetry
  - save ESC telemetry data in dataflash logs
  - use RPM telemetry for dynamic notch filter frequencies
  - sum the current telemetry info from all ESCs and use it as virtual battery current monitor sensor
  - average the voltage telemetry info and use it as virtual battery voltage monitor sensor
  - average the temperature telemetry info and use it as virtual battery temperature monitor sensor
- Obey the safety switch. Keeps motors from turning
- Use `SERVO_FWT_MASK` to define which servo output should be routed to FETtec ESCs
- Use `SERVO_FWT_RVMASK` to define the rotation direction of the motors
  - `SERVO_FWT_RVMASK` changes only take effect when disarmed
- Can be compiled when `HAL_WITH_ESC_TELEM` is disabled. No telemetry data will be available but it saves a lot of memory
- pre-arm checks:
  - Check that UART is available
  - check that the desired motor channels parameter (`SERVO_FWT_MASK`) is valid
  - check that the desired motor poles parameter (`SERVO_FWT_POLES`) is valid
  - check that the all desired ESCs are found and configured
  - check that the ESCs are periodically sending telemetry data
- re-init and configure an ESC(s) if not armed (motors not spinning) when
  - telemetry communication with the ESC(s) is lost
- adds a serial simulator (--uartF=sim:fetteconewireesc) of FETtec OneWire ESCs
- adds autotest (using the simulator) to:
  - simulate telemetry voltage, current, temperature, RPM data using SITL internal variables
  - test the safety switch functionality
  - test ESC power outages
  - test `SERVO_FWT_MASK` parameter validation
  - fly a copter over a simulated serial link connection
This commit is contained in:
Torsten Z 2021-07-06 19:25:34 +02:00 committed by Randy Mackay
parent 617f8e3979
commit b4f4ad7c01
4 changed files with 1645 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

867
libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp Normal file
View File

@ -0,0 +1,867 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Initial protocol implementation was provided by FETtec */
/* Strongly modified by Amilcar Lucas, IAV GmbH */
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Math/AP_Math.h>
#include "AP_FETtecOneWire.h"
#if HAL_AP_FETTEC_ONEWIRE_ENABLED
extern const AP_HAL::HAL& hal;
// Set to 0 when no ESC hardware is available and you want to test the UART send function
#ifndef HAL_AP_FETTEC_CONFIGURE_ESCS
#define HAL_AP_FETTEC_CONFIGURE_ESCS 1
#endif
#if HAL_AP_FETTEC_HALF_DUPLEX
static constexpr uint32_t HALF_DUPLEX_BAUDRATE = 2000000;
#endif
static constexpr uint32_t FULL_DUPLEX_BAUDRATE = 500000;
const AP_Param::GroupInfo AP_FETtecOneWire::var_info[] {
// @Param: MASK
// @DisplayName: Servo channel output bitmask
// @Description: Servo channel mask specifying FETtec ESC output.
// @Bitmask: 0:SERVO1,1:SERVO2,2:SERVO3,3:SERVO4,4:SERVO5,5:SERVO6,6:SERVO7,7:SERVO8,8:SERVO9,9:SERVO10,10:SERVO11,11:SERVO12
// @RebootRequired: True
// @User: Standard
AP_GROUPINFO_FLAGS("MASK", 1, AP_FETtecOneWire, _motor_mask_parameter, 0, AP_PARAM_FLAG_ENABLE),
// @Param: RVMASK
// @DisplayName: Servo channel reverse rotation bitmask
// @Description: Servo channel mask to reverse rotation of FETtec ESC outputs.
// @Bitmask: 0:SERVO1,1:SERVO2,2:SERVO3,3:SERVO4,4:SERVO5,5:SERVO6,6:SERVO7,7:SERVO8,8:SERVO9,9:SERVO10,10:SERVO11,11:SERVO12
// @User: Standard
AP_GROUPINFO("RVMASK", 2, AP_FETtecOneWire, _reverse_mask_parameter, 0),
#if HAL_WITH_ESC_TELEM
// @Param: POLES
// @DisplayName: Nr. electrical poles
// @Description: Number of motor electrical poles
// @Range: 2 50
// @User: Standard
AP_GROUPINFO("POLES", 3, AP_FETtecOneWire, _pole_count_parameter, 14),
#endif
AP_GROUPEND
};
AP_FETtecOneWire *AP_FETtecOneWire::_singleton;
AP_FETtecOneWire::AP_FETtecOneWire()
{
AP_Param::setup_object_defaults(this, var_info);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (_singleton != nullptr) {
AP_HAL::panic("AP_FETtecOneWire must be singleton");
}
#endif
_singleton = this;
}
/**
initialize the serial port
*/
void AP_FETtecOneWire::init_uart()
{
if (_uart != nullptr) {
return;
}
const AP_SerialManager& serial_manager = AP::serialmanager();
_uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FETtecOneWire, 0);
if (_uart == nullptr) {
return; // no serial port available, so nothing to do here
}
_uart->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
_uart->set_unbuffered_writes(true);
_uart->set_blocking_writes(false);
uint32_t uart_baud { FULL_DUPLEX_BAUDRATE };
#if HAL_AP_FETTEC_HALF_DUPLEX
if (_uart->get_options() & _uart->OPTION_HDPLEX) { //Half-Duplex is enabled
_use_hdplex = true;
uart_baud = HALF_DUPLEX_BAUDRATE;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FTW using Half-Duplex");
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FTW using Full-Duplex");
}
#endif
_uart->begin(uart_baud);
}
/// initialize the device driver: configure serial port, wake-up and configure ESCs
void AP_FETtecOneWire::init()
{
init_uart();
if (_uart == nullptr) {
return; // no serial port available, so nothing to do here
}
_motor_mask = _motor_mask_parameter; // take a copy that will not change after we leave this function
_esc_count = __builtin_popcount(_motor_mask);
#if HAL_WITH_ESC_TELEM
// OneWire supports at most 15 ESCs, because of the 4 bit limitation
// on the fast-throttle command. But we are still limited to the
// number of ESCs the telem library will collect data for.
if (_esc_count == 0 || _motor_mask >= (1 << MIN(15, ESC_TELEM_MAX_ESCS))) {
#else
if (_esc_count == 0 || _motor_mask >= (1 << NUM_SERVO_CHANNELS)) {
#endif
_invalid_mask = true;
return;
}
// we have a uart and the desired ESC combination id valid, allocate some memory:
_escs = new ESC[_esc_count];
if (_escs == nullptr) {
return;
}
// initialise ESC ids. This enforces that the FETtec ESC ids
// inside FETtec ESCs need to be contiguous and start at ID 1
// which required by fast-throttle commands.
uint8_t esc_offset = 0; // offset into our device-driver dynamically-allocated array of ESCs
uint8_t esc_id = 1; // ESC ids inside FETtec protocol are one-indexed
uint8_t servo_chan_offset = 0; // offset into _motor_mask_parameter array
for (uint32_t mask = _motor_mask; mask != 0; mask >>= 1, servo_chan_offset++) {
if (mask & 0x1) {
_escs[esc_offset].servo_ofs = servo_chan_offset;
_escs[esc_offset].id = esc_id++;
esc_offset++;
}
}
_invalid_mask = false; // mask is good
gcs().send_text(MAV_SEVERITY_INFO, "FETtec: allocated %u motors", _esc_count);
// We expect to be able to send a fast-throttle command in each loop.
// 8 bits - OneWire Header
// 4 bits - telemetry request
// 11 bits - throttle value per ESC
// 8 bits - frame CRC
const uint16_t net_bit_count = 8 + 4 + (_esc_count * 11) + 8;
// 7 dummy for rounding up the division by 8
const uint16_t fast_throttle_byte_count = (net_bit_count + 7)/8;
uint16_t telemetry_byte_count { 0U };
#if HAL_WITH_ESC_TELEM
// Telemetry is fetched from each loop in turn.
telemetry_byte_count = sizeof(u.packed_tlm) + 1; // assume 9 pause bits between TX and RX
_fast_throttle_byte_count = fast_throttle_byte_count;
#endif
uint32_t uart_baud { FULL_DUPLEX_BAUDRATE };
#if HAL_AP_FETTEC_HALF_DUPLEX
if (_use_hdplex == true) { //Half-Duplex is enabled
uart_baud = HALF_DUPLEX_BAUDRATE;
}
#endif
_min_fast_throttle_period_us = (fast_throttle_byte_count + telemetry_byte_count) * 9 * 1000000 / uart_baud + 300; // 300us extra reserve
// tell SRV_Channels about ESC capabilities
// this is a bit soonish because we have not seen the ESCs on the bus yet,
// but saves us having to use a state variable to ensure doing this latter just once
SRV_Channels::set_digital_outputs(_motor_mask, 0);
_init_done = true;
}
/**
transmits data to ESCs
@param bytes bytes to transmit
@param length number of bytes to transmit
@return false there's no space in the UART for this message
*/
bool AP_FETtecOneWire::transmit(const uint8_t* bytes, const uint8_t length)
{
const uint32_t now = AP_HAL::micros();
if (now - _last_transmit_us < _min_fast_throttle_period_us) {
// in case the SRV_Channels::push() is running at very high rates, limit the period
// this function gets executed because FETtec needs a time gap between frames
// this also prevents one loop to do multiple actions, like reinitialize an ESC and sending a fast-throttle command without a gap.
_period_too_short++;
return false;
}
_last_transmit_us = now;
if (length > _uart->txspace()) {
return false;
}
_uart->write(bytes, length);
#if HAL_AP_FETTEC_HALF_DUPLEX
if (_use_hdplex) {
_ignore_own_bytes += length;
}
#endif
return true;
}
/**
transmits a config request to ESCs
@param bytes bytes to transmit
@param length number of bytes to transmit
@return false if vehicle is armed or if transmit(bytes, length) would return false
*/
bool AP_FETtecOneWire::transmit_config_request(const uint8_t* bytes, const uint8_t length)
{
if (hal.util->get_soft_armed()) {
return false;
}
return transmit(bytes, length);
}
/// shifts data to start of buffer based on magic header bytes
void AP_FETtecOneWire::move_frame_source_in_receive_buffer(const uint8_t search_start_pos)
{
uint8_t i;
for (i=search_start_pos; i<_receive_buf_used; i++) {
// FIXME: full-duplex should add MASTER here as we see our own data
if ((FrameSource)u.receive_buf[i] == FrameSource::BOOTLOADER ||
(FrameSource)u.receive_buf[i] == FrameSource::ESC) {
break;
}
}
consume_bytes(i);
}
/// cut n bytes from start of buffer
void AP_FETtecOneWire::consume_bytes(const uint8_t n)
{
if (n == 0) {
return;
}
// assure the length of the memmove is positive
if (_receive_buf_used < n) {
return;
}
memmove(u.receive_buf, &u.receive_buf[n], _receive_buf_used-n);
_receive_buf_used = _receive_buf_used - n;
}
/// returns true if the first message in the buffer is OK
bool AP_FETtecOneWire::buffer_contains_ok(const uint8_t length)
{
if (length != sizeof(u.packed_ok)) {
_message_invalid_in_state_count++;
return false;
}
if ((MsgType)u.packed_ok.msg.msgid != MsgType::OK) {
return false;
}
return true;
}
void AP_FETtecOneWire::handle_message(ESC &esc, const uint8_t length)
{
// only accept messages from the bootloader when we could
// legitimately get a message from the bootloader. Swipes the OK
// message for convenience
const FrameSource frame_source = (FrameSource)u.packed_ok.frame_source;
if (frame_source != FrameSource::ESC) {
if (esc.state != ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE) {
return;
}
}
switch (esc.state) {
case ESCState::UNINITIALISED:
case ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE:
return;
case ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE:
// "OK" is the only valid response
if (!buffer_contains_ok(length)) {
return;
}
switch (frame_source) {
case FrameSource::MASTER:
// probably half-duplex; should be caught before we get here
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
break;
case FrameSource::BOOTLOADER:
esc.set_state(ESCState::WANT_SEND_START_FW);
esc.is_awake = true;
break;
case FrameSource::ESC:
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
esc.set_state(ESCState::WANT_SEND_REQ_TYPE);
#else
#if HAL_WITH_ESC_TELEM
esc.set_state(ESCState::WANT_SEND_SET_TLM_TYPE);
#else
esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
#endif
#endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
esc.is_awake = true;
break;
}
break;
case ESCState::WANT_SEND_START_FW:
return;
case ESCState::WAITING_OK_FOR_START_FW:
if (buffer_contains_ok(length)) {
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
esc.set_state(ESCState::WANT_SEND_REQ_TYPE);
#else
#if HAL_WITH_ESC_TELEM
esc.set_state(ESCState::WANT_SEND_SET_TLM_TYPE);
#else
esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
#endif
#endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
}
break;
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
case ESCState::WANT_SEND_REQ_TYPE:
return;
case ESCState::WAITING_ESC_TYPE:
if (length != sizeof(u.packed_esc_type)) {
_message_invalid_in_state_count++;
return;
}
esc.type = u.packed_esc_type.msg.type;
esc.set_state(ESCState::WANT_SEND_REQ_SW_VER);
break;
case ESCState::WANT_SEND_REQ_SW_VER:
return;
case ESCState::WAITING_SW_VER:
if (length != sizeof(u.packed_sw_ver)) {
_message_invalid_in_state_count++;
return;
}
esc.firmware_version = u.packed_sw_ver.msg.version;
esc.firmware_subversion = u.packed_sw_ver.msg.subversion;
esc.set_state(ESCState::WANT_SEND_REQ_SN);
break;
case ESCState::WANT_SEND_REQ_SN:
return;
case ESCState::WAITING_SN:
if (length != sizeof(u.packed_sn)) {
_message_invalid_in_state_count++;
return;
}
static_assert(ARRAY_SIZE(u.packed_sn.msg.sn) == ARRAY_SIZE(esc.serial_number), "Serial number array length missmatch");
memcpy(esc.serial_number, u.packed_sn.msg.sn, ARRAY_SIZE(u.packed_sn.msg.sn));
#if HAL_WITH_ESC_TELEM
esc.set_state(ESCState::WANT_SEND_SET_TLM_TYPE);
#else
esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
#endif
break;
#endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
#if HAL_WITH_ESC_TELEM
case ESCState::WANT_SEND_SET_TLM_TYPE:
return;
case ESCState::WAITING_SET_TLM_TYPE_OK:
if (buffer_contains_ok(length)) {
esc.set_state(ESCState::WANT_SEND_SET_FAST_COM_LENGTH);
}
break;
#endif
case ESCState::WANT_SEND_SET_FAST_COM_LENGTH:
return;
case ESCState::WAITING_SET_FAST_COM_LENGTH_OK:
if (buffer_contains_ok(length)) {
esc.set_state(ESCState::RUNNING);
}
break;
case ESCState::RUNNING:
// we only expect telemetry messages in this state
#if HAL_WITH_ESC_TELEM
if (!esc.telem_expected) {
esc.unexpected_telem++;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("unexpected telemetry");
#endif
return;
}
esc.telem_expected = false;
return handle_message_telem(esc);
#else
return;
#endif // HAL_WITH_ESC_TELEM
}
}
#if HAL_WITH_ESC_TELEM
void AP_FETtecOneWire::handle_message_telem(ESC &esc)
{
// the following two methods are coming from AP_ESC_Telem:
const TLM &tlm = u.packed_tlm.msg;
// update rpm and error rate
float error_rate_pct = 0;
if (_fast_throttle_cmd_count > _esc_count) {
error_rate_pct = (tlm.tx_err_count-esc.error_count_at_throttle_count_overflow)*(float)100/(float)_fast_throttle_cmd_count;
} else {
// the telemetry is requested in a round-robin, sequential fashion
// so the in the first _esc_count times all ESCs get to initialize this
esc.error_count_at_throttle_count_overflow = tlm.tx_err_count;
}
update_rpm(esc.servo_ofs,
tlm.rpm*(100*2/_pole_count_parameter),
error_rate_pct);
// update power and temperature telem data
TelemetryData t {};
t.temperature_cdeg = tlm.temp * 100;
t.voltage = tlm.voltage * 0.01f;
t.current = tlm.current * 0.01f;
t.consumption_mah = tlm.consumption_mah;
update_telem_data(
esc.servo_ofs,
t,
TelemetryType::TEMPERATURE|
TelemetryType::VOLTAGE|
TelemetryType::CURRENT|
TelemetryType::CONSUMPTION);
esc.last_telem_us = AP_HAL::micros();
}
#endif // HAL_WITH_ESC_TELEM
// reads data from the UART, calling handle_message on any message found
void AP_FETtecOneWire::read_data_from_uart()
{
/*
a frame looks like:
byte 1 = frame header (0x02 = bootloader, 0x03 = ESC firmware)
byte 2 = sender ID (5bit)
byte 3 & 4 = frame type (always 0x00, 0x00 used for bootloader. here just for compatibility)
byte 5 = frame length over all bytes
byte 6 - X = answer type, followed by the payload
byte X+1 = 8bit CRC
*/
#if HAL_AP_FETTEC_HALF_DUPLEX
//ignore own bytes
if (_use_hdplex) {
while (_ignore_own_bytes > 0 && _uart->available()) {
_ignore_own_bytes--;
_uart->read();
}
}
#endif
uint32_t bytes_to_read = MIN(_uart->available(), 128U);
uint32_t last_bytes_to_read = 0;
while (bytes_to_read &&
bytes_to_read != last_bytes_to_read) {
last_bytes_to_read = bytes_to_read;
// read as much from the uart as we can:
const uint8_t space = ARRAY_SIZE(u.receive_buf) - _receive_buf_used;
const uint32_t nbytes = _uart->read(&u.receive_buf[_receive_buf_used], space);
_receive_buf_used += nbytes;
bytes_to_read -= nbytes;
move_frame_source_in_receive_buffer();
// borrow the "OK" message to retrieve the frame length from the buffer:
const uint8_t frame_length = u.packed_ok.frame_length;
if (_receive_buf_used < frame_length) {
continue;
}
if (crc8_dvb_update(0, u.receive_buf, frame_length-1) != u.receive_buf[frame_length-1]) {
// bad message; shift away this frame_source byte to try to find
// another message
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("bad message");
#endif
crc_rec_err_cnt++;
move_frame_source_in_receive_buffer(1);
continue;
}
// borrow the "OK" message to retrieve the frame_source from the buffer:
const FrameSource frame_source = (FrameSource)u.packed_ok.frame_source;
if (frame_source == FrameSource::MASTER) {
// this is our own message - we'd best be running in
// half-duplex or we're in trouble!
consume_bytes(frame_length);
continue;
}
// borrow the "OK" message to retrieve the esc id from the buffer:
const uint8_t esc_id = u.packed_ok.esc_id;
bool handled = false;
// FIXME: we could scribble down the last ESC we sent a
// message to here and use it rather than doing this linear
// search:
for (uint8_t i=0; i<_esc_count; i++) {
auto &esc = _escs[i];
if (esc.id != esc_id) {
continue;
}
handle_message(esc, frame_length);
handled = true;
break;
}
if (!handled) {
_unknown_esc_message++;
}
consume_bytes(frame_length);
}
}
/**
packs a single fast-throttle command frame containing the throttle for all configured OneWire ESCs.
@param motor_values a 16bit array containing the throttle values that should be sent to the motors. 0-2000 where 1001-2000 is positive rotation and 0-999 reversed rotation
@param esc_id_to_request_telem_from the ESC to request telemetry from
*/
void AP_FETtecOneWire::pack_fast_throttle_command(const uint16_t *motor_values, uint8_t *fast_throttle_command, const uint8_t length, const uint8_t esc_id_to_request_telem_from)
{
// byte 1:
// bit 0,1,2,3 = ESC ID, Bit 4 = MSB bit of first ESC (11bit) throttle value, bit 5,6,7 = frame header
// so AAAABCCC
// A = ID from the ESC telemetry is requested from. ESC ID == 0 means no request.
// B = MSB from first throttle value
// C = frame header
fast_throttle_command[0] = esc_id_to_request_telem_from << 4; // 0 here means no telemetry request
fast_throttle_command[0] |= ((motor_values[0] >> 10) & 0x01) << 3;
fast_throttle_command[0] |= (uint8_t)FrameSource::MASTER;
// byte 2:
// AAABBBBB
// A = next 3 bits from (11bit) throttle value
// B = 5bit target ID
fast_throttle_command[1] = (((motor_values[0] >> 7) & 0x07)) << 5;
fast_throttle_command[1] |= 0x1F; // All IDs
// following bytes are the rest 7 bit of the first (11bit) throttle value,
// and all bits from all other throttle values, followed by the CRC byte
uint8_t mot = 0;
uint8_t bits_remaining_in_this_pwm = 7;
for (uint8_t out_byte_offset = 2; out_byte_offset<length; out_byte_offset++) {
if (bits_remaining_in_this_pwm >= 8) {
// const uint8_t mask = 0xFF << (11-bits_remaining_in_this_pwm);
fast_throttle_command[out_byte_offset] = (motor_values[mot] >> (bits_remaining_in_this_pwm-8)) & 0xFF;
bits_remaining_in_this_pwm -= 8;
} else {
const uint8_t mask = (1U<<bits_remaining_in_this_pwm)-1;
const uint8_t bits_to_copy_from_second_pwm = 8-bits_remaining_in_this_pwm;
fast_throttle_command[out_byte_offset] = (motor_values[mot] & mask) << bits_to_copy_from_second_pwm;
// move on to the next motor output
mot++;
if (mot < _esc_count) {
fast_throttle_command[out_byte_offset] |= motor_values[mot] >> (11-bits_to_copy_from_second_pwm);
}
bits_remaining_in_this_pwm = 11 - bits_to_copy_from_second_pwm;
}
}
fast_throttle_command[length-1] =
crc8_dvb_update(0, fast_throttle_command, length-1);
}
void AP_FETtecOneWire::escs_set_values(const uint16_t* motor_values)
{
uint8_t esc_id_to_request_telem_from = 0;
#if HAL_WITH_ESC_TELEM
ESC &esc_to_req_telem_from = _escs[_esc_ofs_to_request_telem_from++];
if (_esc_ofs_to_request_telem_from >= _esc_count) {
_esc_ofs_to_request_telem_from = 0;
}
esc_id_to_request_telem_from = esc_to_req_telem_from.id;
#endif
uint8_t fast_throttle_command[_fast_throttle_byte_count];
pack_fast_throttle_command(motor_values, fast_throttle_command, sizeof(fast_throttle_command), esc_id_to_request_telem_from);
#if HAL_AP_FETTEC_HALF_DUPLEX
// last byte of signal can be used to make sure the first TLM byte is correct, in case of spike corruption
// FIXME: use this somehow
_last_crc = fast_throttle_command[_fast_throttle_byte_count - 1];
#endif
// No command was yet sent, so no reply is expected and all information
// on the receive buffer is either garbage or noise. Discard it
_uart->discard_input();
// send throttle commands to all configured ESCs in a single packet transfer
if (transmit(fast_throttle_command, sizeof(fast_throttle_command))) {
#if HAL_WITH_ESC_TELEM
esc_to_req_telem_from.telem_expected = true; // used to make sure that the returned telemetry comes from this ESC and not another
esc_to_req_telem_from.telem_requested = true; // used to check if this ESC is periodically sending telemetry
_fast_throttle_cmd_count++;
#endif
}
}
bool AP_FETtecOneWire::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const
{
if (_motor_mask_parameter == 0) {
return true; // No FETtec ESCs are expected, no need to run further pre-arm checks
}
if (_uart == nullptr) {
hal.util->snprintf(failure_msg, failure_msg_len, "No uart");
return false;
}
if (_invalid_mask) {
hal.util->snprintf(failure_msg, failure_msg_len, "Invalid motor mask");
return false;
}
#if HAL_WITH_ESC_TELEM
if (_pole_count_parameter < 2) {
hal.util->snprintf(failure_msg, failure_msg_len, "Invalid pole count %u", uint8_t(_pole_count_parameter));
return false;
}
uint8_t no_telem = 0;
const uint32_t now = AP_HAL::micros();
#endif
uint8_t not_running = 0;
for (uint8_t i=0; i<_esc_count; i++) {
auto &esc = _escs[i];
if (esc.state != ESCState::RUNNING) {
not_running++;
continue;
}
#if HAL_WITH_ESC_TELEM
if (now - esc.last_telem_us > max_telem_interval_us) {
no_telem++;
}
#endif
}
if (not_running != 0) {
hal.util->snprintf(failure_msg, failure_msg_len, "%u of %u ESCs are not running", not_running, _esc_count);
return false;
}
if (!_init_done) {
hal.util->snprintf(failure_msg, failure_msg_len, "Not initialised");
return false;
}
#if HAL_WITH_ESC_TELEM
if (no_telem != 0) {
hal.util->snprintf(failure_msg, failure_msg_len, "%u of %u ESCs are not sending telemetry", no_telem, _esc_count);
return false;
}
#endif
return true;
}
void AP_FETtecOneWire::configure_escs()
{
if (_uart->available()) {
// don't attempt to configure if we've unread data
return;
}
// note that we return as soon as we've transmitted anything in
// case we're in one-wire mode
for (uint8_t i=0; i<_esc_count; i++) {
auto &esc = _escs[i];
switch (esc.state) {
case ESCState::UNINITIALISED:
esc.state = ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE;
FALLTHROUGH;
case ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE:
// probe for bootloader or running firmware
if (transmit_config_request(PackedMessage<OK>{esc.id, OK{}})) {
esc.is_awake = false; // Assume the ESC is asleep or unavailable
esc.set_state(ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE);
}
return;
case ESCState::WAITING_OK_FOR_RUNNING_SW_TYPE:
if (!esc.is_awake) {
esc.set_state(ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE); // go back to try to wake up the ESC
}
return;
case ESCState::WANT_SEND_START_FW:
if (transmit_config_request(PackedMessage<START_FW>{esc.id, START_FW{}})) {
esc.set_state(ESCState::WAITING_OK_FOR_START_FW);
}
return;
case ESCState::WAITING_OK_FOR_START_FW:
return;
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
case ESCState::WANT_SEND_REQ_TYPE:
if (transmit_config_request(PackedMessage<REQ_TYPE>{esc.id, REQ_TYPE{}})) {
esc.set_state(ESCState::WAITING_ESC_TYPE);
}
return;
case ESCState::WAITING_ESC_TYPE:
return;
case ESCState::WANT_SEND_REQ_SW_VER:
if (transmit_config_request(PackedMessage<REQ_SW_VER>{esc.id, REQ_SW_VER{}})) {
esc.set_state(ESCState::WAITING_SW_VER);
}
return;
case ESCState::WAITING_SW_VER:
return;
case ESCState::WANT_SEND_REQ_SN:
if (transmit_config_request(PackedMessage<REQ_SN>{esc.id, REQ_SN{}})) {
esc.set_state(ESCState::WAITING_SN);
}
return;
case ESCState::WAITING_SN:
return;
#endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
#if HAL_WITH_ESC_TELEM
case ESCState::WANT_SEND_SET_TLM_TYPE:
if (transmit_config_request(PackedMessage<SET_TLM_TYPE>{esc.id, SET_TLM_TYPE{1}})) {
esc.set_state(ESCState::WAITING_SET_TLM_TYPE_OK);
}
return;
case ESCState::WAITING_SET_TLM_TYPE_OK:
return;
#endif
case ESCState::WANT_SEND_SET_FAST_COM_LENGTH:
if (transmit_config_request(PackedMessage<SET_FAST_COM_LENGTH>{esc.id,
SET_FAST_COM_LENGTH{
_fast_throttle_byte_count,
_escs[0].id,
_esc_count
}})) {
esc.set_state(ESCState::WAITING_SET_FAST_COM_LENGTH_OK);
}
return;
case ESCState::WAITING_SET_FAST_COM_LENGTH_OK:
return;
case ESCState::RUNNING:
_running_mask |= (1 << esc.servo_ofs);
break;
}
}
}
/// periodically called from SRV_Channels::push()
void AP_FETtecOneWire::update()
{
if (!_init_done) {
init();
return; // the rest of this function can only run after fully initted
}
// read all data from incoming serial:
read_data_from_uart();
const uint32_t now = AP_HAL::micros();
#if HAL_WITH_ESC_TELEM
if (!hal.util->get_soft_armed()) {
_reverse_mask = _reverse_mask_parameter; // update this only when disarmed
// if we haven't seen an ESC in a while, the user might
// have power-cycled them. Try re-initialising.
for (uint8_t i=0; i<_esc_count; i++) {
auto &esc = _escs[i];
if (!esc.telem_requested || now - esc.last_telem_us < 1000000U ) {
// telem OK
continue;
}
_running_mask &= ~(1 << esc.servo_ofs);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "No telem from esc id=%u. Resetting it.", esc.id);
//GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "unknown %u, invalid %u, too short %u, unexpected: %u, crc_err %u", _unknown_esc_message, _message_invalid_in_state_count, _period_too_short, esc.unexpected_telem, crc_rec_err_cnt);
esc.set_state(ESCState::WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE);
esc.telem_requested = false;
}
}
#endif // HAL_WITH_ESC_TELEM
if (now - _last_transmit_us < 700U) {
// in case the SRV_Channels::push() is running at very high rates, limit the period
// this function gets executed because FETtec needs a time gap between frames
_period_too_short++;
return;
}
#if HAL_AP_FETTEC_CONFIGURE_ESCS
// run ESC configuration state machines if needed
if (_running_mask != _motor_mask) {
configure_escs();
return; // do not send fast-throttle command if a configuration command just got sent
}
#endif
// get ESC set points
uint16_t motor_pwm[_esc_count];
for (uint8_t i = 0; i < _esc_count; i++) {
const ESC &esc = _escs[i];
const SRV_Channel* c = SRV_Channels::srv_channel(esc.servo_ofs);
// check if safety switch has been pushed
if ( (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED)
|| (c == nullptr)) { // this should never ever happen, but just in case ...
motor_pwm[i] = 1000; // stop motor
continue;
}
motor_pwm[i] = constrain_int16(c->get_output_pwm(), 1000, 2000);
fet_debug("esc=%u in: %u", esc.id, motor_pwm[i]);
if (_reverse_mask & (1U << i)) {
motor_pwm[i] = 2000-motor_pwm[i];
}
}
// send motor setpoints to ESCs, and request for telemetry data
escs_set_values(motor_pwm);
}
#if HAL_AP_FETTEC_ESC_BEEP
/**
makes all connected ESCs beep
@param beep_frequency a 8 bit value from 0-255. higher make a higher beep
*/
void AP_FETtecOneWire::beep(const uint8_t beep_frequency)
{
for (uint8_t i=0; i<_esc_count; i++) {
auto &esc = _escs[i];
if (esc.state != ESCState::RUNNING) {
continue;
}
transmit_config_request(PackedMessage<Beep>{esc.id, Beep{beep_frequency}});
}
}
#endif // HAL_AP_FETTEC_ESC_BEEP
#if HAL_AP_FETTEC_ESC_LIGHT
/**
sets the racewire color for all ESCs
@param r red brightness
@param g green brightness
@param b blue brightness
*/
void AP_FETtecOneWire::led_color(const uint8_t r, const uint8_t g, const uint8_t b)
{
for (uint8_t i=0; i<_esc_count; i++) {
auto &esc = _escs[i];
if (esc.state != ESCState::RUNNING) {
continue;
}
transmit_config_request(PackedMessage<LEDColour>{esc.id, LEDColour{r, g, b}});
}
}
#endif // HAL_AP_FETTEC_ESC_LIGHT
#endif // HAL_AP_FETTEC_ONEWIRE_ENABLED

504
libraries/AP_FETtecOneWire/AP_FETtecOneWire.h Normal file
View File

@ -0,0 +1,504 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Initial protocol implementation was provided by FETtec */
/* Strongly modified by Amilcar Lucas, IAV GmbH */
#pragma once
#include <AP_HAL/AP_HAL.h>
#ifndef HAL_AP_FETTEC_ONEWIRE_ENABLED
#define HAL_AP_FETTEC_ONEWIRE_ENABLED !HAL_MINIMIZE_FEATURES && !defined(HAL_BUILD_AP_PERIPH) && BOARD_FLASH_SIZE > 1024
#endif
// Support both full-duplex at 500Kbit/s as well as half-duplex at 2Mbit/s (optional feature)
#ifndef HAL_AP_FETTEC_HALF_DUPLEX
#define HAL_AP_FETTEC_HALF_DUPLEX 0
#endif
// Get static info from the ESCs (optional feature)
#ifndef HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
#define HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO 0
#endif
// provide beep support (optional feature)
#ifndef HAL_AP_FETTEC_ESC_BEEP
#define HAL_AP_FETTEC_ESC_BEEP 0
#endif
// provide light support (optional feature)
#ifndef HAL_AP_FETTEC_ESC_LIGHT
#define HAL_AP_FETTEC_ESC_LIGHT 0
#endif
#if HAL_AP_FETTEC_ONEWIRE_ENABLED
#define FTW_DEBUGGING 0
#if FTW_DEBUGGING
#include <stdio.h>
#define fet_debug(fmt, args ...) do {::fprintf(stderr,"FETtec: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define fet_debug(fmt, args ...)
#endif
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_Math/crc.h>
class AP_FETtecOneWire : public AP_ESC_Telem_Backend
{
public:
AP_FETtecOneWire();
/// Do not allow copies
AP_FETtecOneWire(const AP_FETtecOneWire &other) = delete;
AP_FETtecOneWire &operator=(const AP_FETtecOneWire&) = delete;
static const struct AP_Param::GroupInfo var_info[];
static AP_FETtecOneWire *get_singleton() {
return _singleton;
}
/// periodically called from SRV_Channels::push()
void update();
/// called from AP_Arming; should return false if arming should be
/// disallowed
bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const;
#if HAL_AP_FETTEC_ESC_BEEP
/**
makes all connected ESCs beep
@param beep_frequency a 8 bit value from 0-255. higher make a higher beep
*/
void beep(const uint8_t beep_frequency);
#endif
#if HAL_AP_FETTEC_ESC_LIGHT
/**
sets the racewire color for all ESCs
@param r red brightness
@param g green brightness
@param b blue brightness
*/
void led_color(const uint8_t r, const uint8_t g, const uint8_t b);
#endif
private:
static AP_FETtecOneWire *_singleton;
AP_HAL::UARTDriver *_uart;
AP_Int32 _motor_mask_parameter;
AP_Int32 _reverse_mask_parameter;
#if HAL_WITH_ESC_TELEM
AP_Int8 _pole_count_parameter;
#endif
static constexpr uint8_t FRAME_OVERHEAD = 6; ///< OneWire message frame overhead (header+tail bytes)
static constexpr uint8_t MAX_RECEIVE_LENGTH = 12; ///< OneWire max receive message payload length in bytes
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
static constexpr uint8_t SERIAL_NUMBER_LENGTH = 12; ///< ESC serial number length in bytes
#endif
/**
initialize the device driver: configure serial port, wake-up and configure ESCs
*/
void init();
/**
initialize the serial port
*/
void init_uart();
/**
scan the OneWire bus, configure the ESCs requested in the _motor_mask_parameter
*/
void configure_escs();
// states configured ESCs can be in:
enum class ESCState : uint8_t {
UNINITIALISED = 5, // when we haven't tried to send anything to the ESC
WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE = 10,
WAITING_OK_FOR_RUNNING_SW_TYPE = 11,
WANT_SEND_START_FW = 20,
WAITING_OK_FOR_START_FW = 21,
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
WANT_SEND_REQ_TYPE = 30,
WAITING_ESC_TYPE = 31,
WANT_SEND_REQ_SW_VER = 40,
WAITING_SW_VER = 41,
WANT_SEND_REQ_SN = 50,
WAITING_SN = 51,
#endif
#if HAL_WITH_ESC_TELEM
WANT_SEND_SET_TLM_TYPE = 60,
WAITING_SET_TLM_TYPE_OK = 61,
#endif
WANT_SEND_SET_FAST_COM_LENGTH = 70,
WAITING_SET_FAST_COM_LENGTH_OK = 71,
RUNNING = 100,
};
class ESC {
public:
#if HAL_WITH_ESC_TELEM
uint32_t last_telem_us; ///< last time we got telemetry from this ESC
uint16_t unexpected_telem;
uint16_t error_count_at_throttle_count_overflow; ///< overflow counter for error counter from the ESCs.
bool telem_expected; ///< this ESC is fully configured and is now expected to send us telemetry
bool telem_requested; ///< this ESC is fully configured and at some point was requested to send us telemetry
#endif
uint8_t id; ///< FETtec ESC ID
uint8_t servo_ofs; ///< offset into ArduPilot servo array
bool is_awake;
void set_state(ESCState _state) {
fet_debug("Moving ESC.id=%u from state=%u to state=%u", (unsigned)id, (unsigned)state, (unsigned)_state);
state = _state;
};
ESCState state = ESCState::UNINITIALISED;
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
uint8_t serial_number[SERIAL_NUMBER_LENGTH];
uint8_t firmware_version;
uint8_t firmware_subversion;
uint8_t type;
#endif
};
uint32_t _min_fast_throttle_period_us; ///< minimum allowed fast-throttle command transmit period
uint32_t _last_not_running_warning_ms; ///< last time we warned the user their ESCs are stuffed
int32_t _motor_mask; ///< an un-mutable copy of the _motor_mask_parameter taken before _init_done goes true
int32_t _reverse_mask; ///< a copy of the _reverse_mask_parameter taken while not armed
int32_t _running_mask; ///< a bitmask of the actively running ESCs
uint32_t _last_transmit_us; ///< last time the transmit() function sent data
ESC *_escs;
uint8_t _esc_count; ///< number of allocated ESCs
uint8_t _fast_throttle_byte_count; ///< pre-calculated number of bytes required to send an entire packed throttle message
#if HAL_AP_FETTEC_HALF_DUPLEX
uint8_t _ignore_own_bytes; ///< bytes to ignore while receiving, because we have transmitted them ourselves
uint8_t _last_crc; ///< the CRC from the last sent fast-throttle command
bool _use_hdplex; ///< use asynchronous half-duplex serial communication
#endif
bool _init_done; ///< device driver is initialized; ESCs may still need to be configured
bool _invalid_mask; ///< true if the mask parameter is invalid
enum class FrameSource : uint8_t {
MASTER = 0x01, ///< master is always 0x01
BOOTLOADER = 0x02,
ESC = 0x03,
};
enum class MsgType : uint8_t
{
OK = 0,
BL_PAGE_CORRECT = 1, ///< Bootloader only
NOT_OK = 2,
BL_START_FW = 3, ///< Bootloader only - exit the boot loader and start the standard firmware
BL_PAGES_TO_FLASH = 4, ///< Bootloader only
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
REQ_TYPE = 5, ///< ESC type
REQ_SN = 6, ///< serial number
REQ_SW_VER = 7, ///< software version
#endif
#if HAL_AP_FETTEC_ESC_BEEP
BEEP = 13, ///< make noise
#endif
SET_FAST_COM_LENGTH = 26, ///< configure fast-throttle command
SET_TLM_TYPE = 27, ///< telemetry operation mode
#if HAL_AP_FETTEC_ESC_LIGHT
SET_LED_TMP_COLOR = 51, ///< set ESC's LED color
#endif
};
/**
a frame looks like:
byte 1 = frame header (master is always 0x01)
byte 2 = target ID (5bit)
byte 3 & 4 = frame type (always 0x00, 0x00 used for bootloader. here just for compatibility)
byte 5 = frame length over all bytes
byte 6 - X = request type, followed by the payload
byte X+1 = 8bit CRC
*/
template <typename T>
class PACKED PackedMessage {
public:
PackedMessage(uint8_t _esc_id, T _msg) :
esc_id(_esc_id),
msg(_msg)
{
update_checksum();
}
uint8_t frame_source { (uint8_t)FrameSource::MASTER };
uint8_t esc_id;
uint16_t frame_type { 0 }; // bootloader only, always zero
uint8_t frame_length {sizeof(T) + FRAME_OVERHEAD}; // all bytes including frame_source and checksum
T msg;
uint8_t checksum;
void update_checksum() {
checksum = crc8_dvb_update(0, (const uint8_t*)this, frame_length-1);
}
};
class PACKED OK {
public:
uint8_t msgid { (uint8_t)MsgType::OK };
};
class PACKED START_FW {
public:
uint8_t msgid { (uint8_t)MsgType::BL_START_FW };
};
class PACKED SET_FAST_COM_LENGTH {
public:
SET_FAST_COM_LENGTH(uint8_t _byte_count, uint8_t _min_esc_id, uint8_t _esc_count) :
byte_count{_byte_count},
min_esc_id{_min_esc_id},
esc_count{_esc_count}
{ }
uint8_t msgid { (uint8_t)MsgType::SET_FAST_COM_LENGTH };
uint8_t byte_count;
uint8_t min_esc_id;
uint8_t esc_count;
};
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
class PACKED REQ_TYPE {
public:
uint8_t msgid { (uint8_t)MsgType::REQ_TYPE };
};
class PACKED REQ_SW_VER {
public:
uint8_t msgid { (uint8_t)MsgType::REQ_SW_VER };
};
class PACKED REQ_SN {
public:
uint8_t msgid { (uint8_t)MsgType::REQ_SN };
};
class PACKED ESC_TYPE {
public:
ESC_TYPE(uint8_t _type) :
type{_type} { }
uint8_t type;
};
class PACKED SW_VER {
public:
SW_VER(uint8_t _version, uint8_t _subversion) :
version{_version},
subversion{_subversion}
{ }
uint8_t version;
uint8_t subversion;
};
class PACKED SN {
public:
SN(uint8_t *_sn, uint8_t snlen) {
memcpy(sn, _sn, ARRAY_SIZE(sn));
}
uint8_t sn[SERIAL_NUMBER_LENGTH];
};
#endif // HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
void pack_fast_throttle_command(const uint16_t *motor_values, uint8_t *buffer, const uint8_t length, const uint8_t esc_id_to_request_telem_from);
/*
* Messages, methods and states for dealing with ESC telemetry
*/
#if HAL_WITH_ESC_TELEM
void handle_message_telem(ESC &esc);
uint16_t _fast_throttle_cmd_count; ///< number of fast-throttle commands sent by the flight controller
/// the ESC at this offset into _escs should be the next to send a
/// telemetry request for:
uint8_t _esc_ofs_to_request_telem_from;
class PACKED SET_TLM_TYPE {
public:
SET_TLM_TYPE(uint8_t _tlm_type) :
tlm_type{_tlm_type}
{ }
uint8_t msgid { (uint8_t)MsgType::SET_TLM_TYPE };
uint8_t tlm_type;
};
class PACKED TLM {
public:
TLM(int8_t _temp, uint16_t _voltage, uint16_t _current, int16_t _rpm, uint16_t _consumption_mah, uint16_t _tx_err_count) :
temp{_temp},
voltage{_voltage},
current{_current},
rpm{_rpm},
consumption_mah{_consumption_mah},
tx_err_count{_tx_err_count}
{ }
int8_t temp; // centi-degrees
uint16_t voltage; // centi-Volt
uint16_t current; // centi-Ampere (signed?)
int16_t rpm; // centi-rpm
uint16_t consumption_mah; // mili-Ampere.hour
uint16_t tx_err_count; // CRC error count, as perceived from the ESC receiving side
};
#endif // HAL_WITH_ESC_TELEM
#if HAL_AP_FETTEC_ESC_BEEP
class PACKED Beep {
public:
Beep(uint8_t _beep_frequency) :
beep_frequency{_beep_frequency}
{ }
uint8_t msgid { (uint8_t)MsgType::BEEP };
uint8_t beep_frequency;
// add two zeros to make sure all ESCs can catch their command as we don't wait for a response here (don't blame me --pb)
uint16_t spacer = 0;
};
#endif // HAL_AP_FETTEC_ESC_BEEP
#if HAL_AP_FETTEC_ESC_LIGHT
class PACKED LEDColour {
public:
LEDColour(uint8_t _r, uint8_t _g, uint8_t _b) :
r{_r},
g{_g},
b{_b}
{ }
uint8_t msgid { (uint8_t)MsgType::SET_LED_TMP_COLOR };
uint8_t r;
uint8_t g;
uint8_t b;
// add two zeros to make sure all ESCs can catch their command as we don't wait for a response here (don't blame me --pb)
uint16_t spacer = 0;
};
#endif // HAL_AP_FETTEC_ESC_LIGHT
/*
* Methods and data for transmitting data to the ESCSs:
*/
/**
transmits data to ESCs
@param bytes bytes to transmit
@param length number of bytes to transmit
@return false there's no space in the UART for this message
*/
bool transmit(const uint8_t* bytes, const uint8_t length);
template <typename T>
bool transmit(const PackedMessage<T> &msg) {
return transmit((const uint8_t*)&msg, sizeof(msg));
}
/**
transmits configuration request data to ESCs
@param bytes bytes to transmit
@param length number of bytes to transmit
@return false if vehicle armed or there's no space in the UART for this message
*/
bool transmit_config_request(const uint8_t* bytes, const uint8_t length);
template <typename T>
bool transmit_config_request(const PackedMessage<T> &msg) {
return transmit_config_request((const uint8_t*)&msg, sizeof(msg));
}
/**
sends a single fast-throttle frame containing the throttle for all configured OneWire ESCs.
@param motor_values a 16bit array containing the throttle values that should be sent to the motors. 0-2000 where 1001-2000 is positive rotation and 999-0 reversed rotation
*/
void escs_set_values(const uint16_t *motor_values);
/*
* Methods and data for receiving data from the ESCs:
*/
// FIXME: this should be tighter - and probably calculated. Note
// that we can't request telemetry faster than the loop interval,
// which is 20ms on Plane, so that puts a constraint here. When
// using fast-throttle with 12 ESCs on Plane you could expect
// 240ms between telem updates. Why you have a Plane with 12 ESCs
// is a bit of a puzzle.
static const uint32_t max_telem_interval_us = 100000;
void handle_message(ESC &esc, const uint8_t length);
/**
reads data from the UART, calling handle_message on any message found
*/
void read_data_from_uart();
union MessageUnion {
MessageUnion() { }
PackedMessage<OK> packed_ok;
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
PackedMessage<ESC_TYPE> packed_esc_type;
PackedMessage<SW_VER> packed_sw_ver;
PackedMessage<SN> packed_sn;
#endif
#if HAL_WITH_ESC_TELEM
PackedMessage<TLM> packed_tlm;
#endif
uint8_t receive_buf[FRAME_OVERHEAD + MAX_RECEIVE_LENGTH];
} u;
static_assert(sizeof(u.packed_ok) <= sizeof(u.receive_buf),"packed_ok does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
static_assert(sizeof(u.packed_esc_type) <= sizeof(u.receive_buf),"packed_esc_type does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
static_assert(sizeof(u.packed_sw_ver) <= sizeof(u.receive_buf),"packed_sw_ver does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
static_assert(sizeof(u.packed_sn) <= sizeof(u.receive_buf),"packed_sn does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
#endif
#if HAL_WITH_ESC_TELEM
static_assert(sizeof(u.packed_tlm) <= sizeof(u.receive_buf),"packed_tlm does not fit in receive_buf. MAX_RECEIVE_LENGTH too small?");
#endif
uint16_t _unknown_esc_message;
uint16_t _message_invalid_in_state_count;
uint16_t _period_too_short;
uint16_t crc_rec_err_cnt;
uint8_t _receive_buf_used;
/// shifts data to start of buffer based on magic header bytes
void move_frame_source_in_receive_buffer(const uint8_t search_start_pos = 0);
/// cut n bytes from start of buffer
void consume_bytes(const uint8_t n);
/// returns true if the first message in the buffer is OK
bool buffer_contains_ok(const uint8_t length);
};
#endif // HAL_AP_FETTEC_ONEWIRE_ENABLED

243
libraries/AP_FETtecOneWire/README.md Normal file
View File

@ -0,0 +1,243 @@
# FETtec OneWire
FETtec OneWire is an [ESC](https://en.wikipedia.org/wiki/Electronic_speed_control) communication protocol created by Felix Niessen (former Flyduino KISS developer) from [FETtec](https://fettec.net).
It is a (bidirectional) [digital full-duplex asynchronous serial communication protocol](https://en.wikipedia.org/wiki/Asynchronous_serial_communication) running at 500Kbit/s Baudrate. It requires three wire (RX, TX and GND) connection (albeit the name OneWire) regardless of the number of ESCs connected.
Unlike bidirectional-Dshot, the FETtec OneWire protocol does not need one DMA channel per ESC for bidirectional communication.
For purchase, connection and configuration information please see the [Ardupilot FETtec OneWire wiki page](https://ardupilot.org/copter/docs/common-fettec-onewire.html).
## Features of this device driver
- use ArduPilot's coding guidelines and naming conventions
- control motor speed
- Use the AP_ESC_Telem base class to:
- copy ESC telemetry data into MAVLink telemetry
- save ESC telemetry data in dataflash logs
- use RPM telemetry for dynamic notch filter frequencies
- sum the current telemetry info from all ESCs and use it as virtual battery current monitor sensor
- average the voltage telemetry info and use it as virtual battery voltage monitor sensor
- average the temperature telemetry info and use it as virtual battery temperature monitor sensor
- Obey the safety switch. Keeps motors from turning
- Use `SERVO_FWT_MASK` to define which servo output should be routed to FETtec ESCs
- Use `SERVO_FWT_RVMASK` to define the rotation direction of the motors
- `SERVO_FWT_RVMASK` changes only take effect when disarmed
- Can be compiled when `HAL_WITH_ESC_TELEM` is disabled. No telemetry data will be available but it saves a lot of memory
- pre-arm checks:
- Check that UART is available
- check that the desired motor channels parameter (`SERVO_FWT_MASK`) is valid
- check that the desired motor poles parameter (`SERVO_FWT_POLES`) is valid
- check that the all desired ESCs are found and configured
- check that the ESCs are periodically sending telemetry data
- re-init and configure an ESC(s) if not armed (motors not spinning) when
- telemetry communication with the ESC(s) is lost
- adds a serial simulator (--uartF=sim:fetteconewireesc) of FETtec OneWire ESCs
- adds autotest (using the simulator) to:
- simulate telemetry voltage, current, temperature, RPM data using SITL internal variables
- test the safety switch functionality
- test ESC power outages
- test `SERVO_FWT_MASK` parameter validation
- fly a copter over a simulated serial link connection
## Ardupilot to ESC protocol
The FETtec OneWire protocol supports up to 24 ESCs. As most copters only use at most 12 motors, Ardupilot's implementation supports only 12 (`ESC_TELEM_MAX_ESCS`)to save memory.
There are two types of messages sent to the ESCs configuration and fast-throttle messages:
### Configuration message frame
Consists of six frame bytes + payload bytes.
```
Byte 0 is the source type
Byte 1 is the ID
Byte 2 is the frame type (Low byte)
Byte 3 is the frame type (High byte)
Byte 4 is the frame length
Byte 5-254 is the payload
Byte 6-255 is CRC (last Byte after the Payload). It uses the same CRC algorithm as Dshot.
```
#### Check if bootloader or ESC firmware is running
To check which firmware is running on the FETtec an ESCs `PackedMessage<OK>` is sent to each ESC.
If the answer frame `frame_source` is FrameSource::BOOTLOADER we are in bootloader and need to send an `PackedMessage<START_FW>` to start the ESC firmware.
If the answer frame `frame_source` is FrameSource::ESC we are already running the correct firmware and can directly configure the telemetry.
#### Start the ESC firmware
If the ESC is running on bootloader firmware we need to send an `PackedMessage<START_FW>` to start the ESC firmware.
The answer must be a `PackedMessage<OK>` with `frame_source` equal to FrameSource::ESC. If not, we need to repeat the command.
#### Configure Full/Alternative Telemetry
The telemetry can be switched to "per ESC" Mode, where one ESC answers with it's full telemetry as oneWire package including CRC and additionally the CRC Errors counted by the ESC.
To use this mode, `PackedMessage<SET_TLM_TYPE>` is send to each ESC while initializing.
If this was successful the ESC responds with `PackedMessage<OK>`.
#### Configure Fast throttle messages
To configure the fast-throttle frame structure a `PackedMessage<SET_FAST_COM_LENGTH>` is send to each ESC while initializing.
If this was successful the ESC responds with `PackedMessage<OK>`.
### Fast-throttle message frame
```
Byte 0 is the frame header
Byte 1 is the telemetry request and part of fast throttle signal
Byte N is CRC (last Byte after the Payload). It uses the same CRC algorithm as Dshot.
```
The first two bytes are frame header and telemetry request as well as the first parts of the throttle signal.
The following bytes are transmitting the throttle signals for the ESCs (11bit per ESC) followed by the CRC.
The signal is used to transfer the eleven bit throttle signals with as few bytes as possible:
```
[990 .. 0] - negative throttle, rotation in one direction (depends on the motor wiring connection). 980 minimum throttle, 00 maximum throttle
[991 .. 1009] - no rotation, dead-band
[1010 .. 2000] - positive throttle, rotation in the other direction. 1020 minimum throttle, 2000 maximum throttle
```
All motors wait for the complete message with all throttle signals before changing their output.
If telemetry is requested the ESCs will answer them in the ESC-ID order.
See *ESC to Ardupilot Protocol* section below and comments in `AP_FETtecOneWire.cpp` for details.
### Timing
Four ESCs need 90us for the fast-throttle request and telemetry reception. With four ESCs 11kHz update would be possible.
Each additional ESC adds 11 extra fast-throttle command bits, so the update rate is lowered by each additional ESC.
If you use 8 ESCs, it needs 160us including telemetry response, so 5.8kHz update rate would be possible.
The FETtec Ardupilot device driver limits the message transmit period to `_min_fast_throttle_period_us` according to the number of ESCs used.
The update() function has an extra invocation period limit so that even at very high loop rates the the ESCs will still operate correctly albeit doing some decimation.
The current update rate for Copter is 400Hz (~2500us) and for other vehicles is 50Hz (~20000us) so we are bellow device driver limit.
**Note:** The FETtec ESCs firmware requires at least a 4Hz fast-throttle update rate (max. 250ms between messages) otherwise the FETtec ESC disarm (stop) the motors.
## ESC to Ardupilot protocol
OneWire ESC telemetry information is sent back to the autopilot:
- Electronic rotations per minute (eRPM/100) (must be divided by number of motor poles to translate to propeller RPM)
- Input voltage (V/10)
- Current draw (A/10)
- Power consumption (mAh)
- Temperature (°C/10)
- CRC errors (ArduPilot->ESC) counter
This information is used by Ardupilot to:
- log the status of each ESC to the SDCard or internal Flash, for post flight analysis
- send the status of each ESC to the ground station or companion computer for real-time monitoring
- Optionally dynamically change the center frequency of the notch filters used to reduce frame vibration noise in the gyros
- Optionally measure battery voltage and power consumption
### Full/Alternative Telemetry
The answer to a fast-throttle command frame is a `PackedMessage<TLM>` message frame, received and decoded in the `FETtecOneWire::handle_message_telem()` function.
The data is forwarded to the `AP_ESC_Telem` class that distributes it to other parts of the ArduPilot code.
## Function structure
There are two public top level functions `update()` and `pre_arm_check()`.
And these two call all other private internal functions.
A single (per ESC) state variable (`_escs[i]._state`) is used in both the RX and TX state machnines.
Here is the callgraph:
```
update()
init()
init_uart()
read_data_from_uart()
move_frame_source_in_receive_buffer()
consume_bytes()
consume_bytes()
handle_message() <-- RX state machine
buffer_contains_ok()
handle_message_telem()
configure_escs() <-- TX state machine
transmit_config_request()
transmit()
escs_set_values()
pack_fast_throttle_command()
transmit()
pre_arm_check()
```
## Device driver parameters
The `SERVO_FTW_MASK` parameter selects which servo outputs, if any, will be routed to FETtec ESCs.
You need to reboot after changing this parameter.
When `HAL_WITH_ESC_TELEM` is active (default) only the first 12 (`ESC_TELEM_MAX_ESCS`) can be used.
The FETtec ESC IDs set inside the FETtec firmware by the FETtec configuration tool are one-indexed.
These IDs must start at ID 1 and be in a single contiguous block.
You do not need to change these FETtec IDs if you change the servo output assignments inside ArduPilot the using the `SERVO_FTW_MASK` parameter.
The `SERVO_FTW_RVMASK` parameter selects which servo outputs, if any, will reverse their rotation.
This parameter effects the outputs immediately when changed and the motors are not armed.
This parameter is only visible if the `SERVO_FTW_MASK` parameter has at least one bit set.
The `SERVO_FTW_POLES` parameter selects Number of motor electrical poles.
It is used to calculate the motors RPM
This parameter effects the RPM calculation immediately when changed.
This parameter is only visible if the `SERVO_FTW_MASK` parameter has at least one bit set.
## Extra features
### Read type, firmware version and serial number
To control this you must activate the code in the header file:
```C++
// Get static info from the ESCs (optional feature)
#ifndef HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
#define HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO 1
#endif
```
Or just set the `HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO` macro in the compiler toolchain.
After that you will be able to access this information in the `_escs[]` datastructure.
### Beep
To control this you must activate the code in the header file:
```C++
// provide beep support (optional feature)
#ifndef HAL_AP_FETTEC_ESC_BEEP
#define HAL_AP_FETTEC_ESC_BEEP 1
#endif
```
Or just set the `HAL_AP_FETTEC_ESC_BEEP` macro in the compiler toolchain.
After that you will be able to call the public function:
```C++
/**
makes all connected ESCs beep
@param beep_frequency a 8 bit value from 0-255. higher make a higher beep
*/
void beep(const uint8_t beep_frequency);
```
### Multicolor RGB Led light
To control this you must activate the code in the header file:
```C++
// provide light support (optional feature)
#ifndef HAL_AP_FETTEC_ESC_LIGHT
#define HAL_AP_FETTEC_ESC_LIGHT 1
#endif
```
Or just set the `HAL_AP_FETTEC_ESC_LIGHT` macro in the compiler toolchain.
After that you will be able to call the public function:
```C++
/**
makes all connected ESCs beep
@param beep_frequency a 8 bit value from 0-255. higher make a higher beep
*/
void beep(const uint8_t beep_frequency);
/**
sets the racewire color for all ESCs
@param r red brightness
@param g green brightness
@param b blue brightness
*/
void led_color(const uint8_t r, const uint8_t g, const uint8_t b);
```
You need to call these functions on your own code according to your requirements.

31
libraries/AP_FETtecOneWire/quad_on_serial2.parm Normal file
View File

@ -0,0 +1,31 @@
# Example parameter configuration file is for a Quadcopter with ESCs connected to Telem2
# FETtec OneWire ESCs are connected on Telem2 port
SERIAL2_PROTOCOL 38
SERIAL2_OPTIONS 0
# Use the full digital range supported by the ESCs
MOT_PWM_MAX 2000
MOT_PWM_MIN 1000
# Make sure the correct functions get routed out
SERVO1_FUNCTION 33
SERVO2_FUNCTION 34
SERVO3_FUNCTION 35
SERVO4_FUNCTION 36
# Activate FETtec OneWire support
SERVO_FTW_MASK 15
SERVO_FTW_RVMASK 0
SERVO_FTW_POLES 14
# Use RPM telemetry data to dynamically configure one notch filter per motor
INS_HNTCH_ENABLE 1
INS_HNTCH_MODE 3
INS_HNTCH_OPTS 2
INS_HNTCH_REF 1
INS_HNTCH_FREQ 60
INS_HNTCH_BW 30
# Use ESC telemetry data as a "virtual" battery monitor
BATT2_MONITOR 9