mirror of https://github.com/ArduPilot/ardupilot
AP_FETtecOneWire: complete rewrite of the ESC-configuration state machine
Co-authored-by: Dr.-Ing. Amilcar do Carmo Lucas <amilcar.lucas@iav.de> tidy message sending using templates Calculate and enforce the minimum update period. Disable unused features to save flash forced time gaps between all transmits correct ESC reset functionality Avoid re-initialization repeatition Make sure we stop FETtec if safety is on (ignore reverse) this reduces duplicated code Error count calculation changed as the telemetry error count is absolute only the overflow status can be safed and used for the percentage calculation Update the README to add autotests information FETtec needs a time gap between frames This allows running at high fast_loop_rates do not send fast_throttle data if a configuration command just got sent Example parameter configuration file is for a Quadcopter with ESCs connected to Telem2 remove two FIXME fix compilation in master Fix the ESC not re-initializing issue. Now we re-init whenever we loose connection RVMASK parameter changes only take effect when not armed Improve documentation Always use the same wording when referring to fast-throttle commands fix pre-arm check message assure the length of the memmove is positive Set HAL_AP_FETTEC_CONFIGURE_ESCS to 0 when no ESC hardware is available and you want to test the UART send function
This commit is contained in:
Peter Barker
Peter Barker
parent
e263063600
commit
ea7c7f9d19
File diff suppressed because it is too large
Load Diff
|
|
@ -46,10 +46,19 @@
|
|||
|
||||
#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
|
||||
{
|
||||
|
|
@ -63,14 +72,17 @@ public:
|
|||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
bool pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) const;
|
||||
|
||||
/// periodically called from SRV_Channels::push()
|
||||
void update();
|
||||
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
|
||||
|
|
@ -93,168 +105,116 @@ private:
|
|||
static AP_FETtecOneWire *_singleton;
|
||||
AP_HAL::UARTDriver *_uart;
|
||||
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
static constexpr uint8_t MOTOR_COUNT_MAX = ESC_TELEM_MAX_ESCS; ///< OneWire supports up-to 15 ESCs, but Ardupilot only supports 12
|
||||
#else
|
||||
static constexpr uint8_t MOTOR_COUNT_MAX = 12; ///< OneWire supports up-to 15 ESCs, but Ardupilot only supports 12
|
||||
#endif
|
||||
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;
|
||||
static constexpr uint8_t MAX_TRANSMIT_LENGTH = 4;
|
||||
static constexpr uint8_t MAX_RECEIVE_LENGTH = 12;
|
||||
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
|
||||
static constexpr uint8_t SERIAL_NUMBER_LENGTH = 12; ///< ESC serial number length in bytes
|
||||
|
||||
/**
|
||||
initialize the serial port, scan the OneWire bus, setup the found ESCs
|
||||
initialize the device driver: configure serial port, wake-up and configure ESCs
|
||||
*/
|
||||
void init();
|
||||
|
||||
/**
|
||||
check if the current configuration is OK
|
||||
initialize the serial port
|
||||
*/
|
||||
void configuration_check();
|
||||
void init_uart();
|
||||
|
||||
/**
|
||||
transmits a FETtec OneWire frame to an ESC
|
||||
@param esc_id id of the ESC
|
||||
@param bytes 8 bit array of bytes. Where byte 1 contains the command, and all following bytes can be the payload
|
||||
@param length length of the bytes array
|
||||
@return false if length is bigger than MAX_TRANSMIT_LENGTH, true on write success
|
||||
scan the OneWire bus, configure the ESCs requested in the _motor_mask_parameter
|
||||
*/
|
||||
bool transmit(const uint8_t esc_id, const uint8_t *bytes, uint8_t length);
|
||||
void configure_escs();
|
||||
|
||||
enum class return_type : uint8_t
|
||||
{
|
||||
RESPONSE,
|
||||
FULL_FRAME
|
||||
};
|
||||
// states configured ESCs can be in:
|
||||
enum class ESCState : uint8_t {
|
||||
UNINITIALISED = 5, // when we haven't tried to send anything to the ESC
|
||||
|
||||
enum class receive_response : uint8_t
|
||||
{
|
||||
NO_ANSWER_YET,
|
||||
ANSWER_VALID,
|
||||
CRC_MISSMATCH,
|
||||
REQ_OVERLENGTH
|
||||
};
|
||||
WANT_SEND_OK_TO_GET_RUNNING_SW_TYPE = 10,
|
||||
WAITING_OK_FOR_RUNNING_SW_TYPE = 11,
|
||||
|
||||
/**
|
||||
reads the FETtec OneWire answer frame of an ESC
|
||||
@param bytes 8 bit byte array, where the received answer gets stored in
|
||||
@param length the expected answer length
|
||||
@param return_full_frame can be return_type::RESPONSE or return_type::FULL_FRAME
|
||||
@return receive_response enum
|
||||
*/
|
||||
receive_response receive(uint8_t *bytes, uint8_t length, return_type return_full_frame);
|
||||
uint8_t receive_buf[FRAME_OVERHEAD + MAX_RECEIVE_LENGTH];
|
||||
uint8_t receive_buf_used;
|
||||
void move_preamble_in_receive_buffer(uint8_t search_start_pos = 0);
|
||||
void consume_bytes(uint8_t n);
|
||||
WANT_SEND_START_FW = 20,
|
||||
WAITING_OK_FOR_START_FW = 21,
|
||||
|
||||
enum class pull_state : uint8_t {
|
||||
BUSY,
|
||||
COMPLETED,
|
||||
FAILED
|
||||
};
|
||||
|
||||
/**
|
||||
Pulls a complete request between flight controller and ESC
|
||||
@param esc_id id of the ESC
|
||||
@param command 8bit array containing the command that should be send including the possible payload
|
||||
@param response 8bit array where the response will be stored in
|
||||
@param return_full_frame can be return_type::RESPONSE or return_type::FULL_FRAME
|
||||
@param req_len transmit request length
|
||||
@return pull_state enum
|
||||
*/
|
||||
pull_state pull_command(const uint8_t esc_id, const uint8_t *command, uint8_t *response, return_type return_full_frame, const uint8_t req_len);
|
||||
|
||||
/**
|
||||
Scans for all ESCs in bus. Configures fast-throttle and telemetry for the ones found.
|
||||
Should be periodically called until _scan.state == scan_state_t::DONE
|
||||
*/
|
||||
void scan_escs();
|
||||
|
||||
/**
|
||||
configure the fast-throttle command.
|
||||
Should be called once after scan_escs() is completted and before config_escs()
|
||||
*/
|
||||
void config_fast_throttle();
|
||||
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
/**
|
||||
increment message packet count for every ESC
|
||||
*/
|
||||
void inc_sent_msg_count();
|
||||
|
||||
/**
|
||||
calculates tx (outgoing packets) error-rate by converting the CRC error counts reported by the ESCs into percentage
|
||||
@param esc_id id of ESC, that the error is calculated for
|
||||
@param esc_error_count the error count given by the esc
|
||||
@return the error in percent
|
||||
*/
|
||||
float calc_tx_crc_error_perc(const uint8_t esc_id, uint16_t esc_error_count);
|
||||
|
||||
/**
|
||||
if init is complete checks if the requested telemetry is available.
|
||||
@param t telemetry datastructure where the read telemetry will be stored in.
|
||||
@param centi_erpm 16bit centi-eRPM value returned from the ESC
|
||||
@param tx_err_count Ardupilot->ESC communication CRC error counter
|
||||
@param tlm_from_id receives the ID from the ESC that has respond with its telemetry
|
||||
@return receive_response enum
|
||||
*/
|
||||
receive_response decode_single_esc_telemetry(TelemetryData& t, int16_t& centi_erpm, uint16_t& tx_err_count, uint8_t &tlm_from_id);
|
||||
#endif
|
||||
|
||||
/**
|
||||
if init is complete sends a single fast-throttle frame containing the throttle for all found 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 tlm_request the ESC to request telemetry from (-1 for no telemetry, 0 for ESC1, 1 for ESC2, 2 for ESC3, ...)
|
||||
*/
|
||||
void escs_set_values(const uint16_t *motor_values, const int8_t tlm_request);
|
||||
|
||||
static constexpr uint8_t SERIAL_NR_BITWIDTH = 12;
|
||||
|
||||
class FETtecOneWireESC
|
||||
{
|
||||
public:
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
uint16_t error_count; ///< error counter from the ESCs.
|
||||
uint16_t error_count_since_overflow; ///< error counter from the ESCs to pass the overflow.
|
||||
#endif
|
||||
bool active;
|
||||
#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO
|
||||
uint8_t firmware_version;
|
||||
uint8_t firmware_sub_version;
|
||||
uint8_t esc_type;
|
||||
uint8_t serial_number[SERIAL_NR_BITWIDTH];
|
||||
#endif
|
||||
} _found_escs[MOTOR_COUNT_MAX]; ///< Zero-indexed array
|
||||
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
|
||||
|
||||
uint32_t _last_config_check_ms;
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
float _crc_error_rate_factor; ///< multiply factor. Used to avoid division operations
|
||||
uint16_t _sent_msg_count; ///< number of fast-throttle commands sent by the flight controller
|
||||
uint16_t _update_rate_hz;
|
||||
WANT_SEND_SET_TLM_TYPE = 60,
|
||||
WAITING_SET_TLM_TYPE_OK = 61,
|
||||
#endif
|
||||
uint16_t _motor_mask;
|
||||
uint16_t _reverse_mask;
|
||||
uint8_t _nr_escs_in_bitmask; ///< number of ESCs set on the FTW_MASK parameter
|
||||
uint8_t _found_escs_count; ///< number of ESCs auto-scanned in the bus by the scan_escs() function
|
||||
uint8_t _configured_escs; ///< number of ESCs fully configured by the scan_escs() function, might be smaller than _found_escs_count
|
||||
|
||||
int8_t _requested_telemetry_from_esc; ///< the ESC to request telemetry from (-1 for no telemetry, 0 for ESC1, 1 for ESC2, 2 for ESC3, ...)
|
||||
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
|
||||
uint32_t last_reset_us;
|
||||
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
|
||||
#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
|
||||
uint8_t _last_crc; ///< the CRC from the last sent fast-throttle command
|
||||
bool _use_hdplex; ///< use asynchronous half-duplex serial communication
|
||||
#endif
|
||||
bool _initialised; ///< device driver and ESCs are fully initialized
|
||||
bool _pull_busy; ///< request-reply transaction is busy
|
||||
|
||||
enum class msg_type : uint8_t
|
||||
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
|
||||
|
|
@ -271,53 +231,270 @@ private:
|
|||
#endif
|
||||
SET_FAST_COM_LENGTH = 26, ///< configure fast-throttle command
|
||||
SET_TLM_TYPE = 27, ///< telemetry operation mode
|
||||
SIZEOF_RESPONSE_LENGTH, ///< size of the _response_length array used in the pull_command() function, you can move this one around
|
||||
#if HAL_AP_FETTEC_ESC_LIGHT
|
||||
SET_LED_TMP_COLOR = 51, ///< msg_type::SET_LED_TMP_COLOR is ignored here. You must update this if you add new msg_type cases
|
||||
SET_LED_TMP_COLOR = 51, ///< set ESC's LED color
|
||||
#endif
|
||||
};
|
||||
|
||||
enum class scan_state_t : uint8_t {
|
||||
WAIT_FOR_BOOT, ///< initial state, wait for a ESC(s) cold-start
|
||||
IN_BOOTLOADER, ///< in bootloader?
|
||||
START_FW, ///< start the firmware
|
||||
WAIT_START_FW, ///< wait for the firmware to start
|
||||
/**
|
||||
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
|
||||
ESC_TYPE, ///< ask the ESC type
|
||||
SW_VER, ///< ask the software version
|
||||
SN, ///< ask the serial number
|
||||
#endif
|
||||
NEXT_ID, ///< increment ESC ID and jump to IN_BOOTLOADER
|
||||
CONFIG_FAST_THROTTLE, ///< configure fast-throttle command header
|
||||
#if HAL_WITH_ESC_TELEM
|
||||
CONFIG_TLM, ///< configure telemetry mode
|
||||
#endif
|
||||
CONFIG_NEXT_ACTIVE_ESC, ///< increment ESC ID and jump to CONFIG_FAST_THROTTLE
|
||||
DONE ///< configuration done
|
||||
class PACKED REQ_TYPE {
|
||||
public:
|
||||
uint8_t msgid { (uint8_t)MsgType::REQ_TYPE };
|
||||
};
|
||||
|
||||
/// presistent scan state data (only used inside scan_escs() function)
|
||||
struct scan_state
|
||||
{
|
||||
uint32_t last_us; ///< last transaction time in microseconds
|
||||
uint8_t id; ///< Zero-indexed ID of the used ESC
|
||||
scan_state_t state; ///< scan state-machine state
|
||||
uint8_t rx_try_cnt; ///< receive try counter
|
||||
uint8_t trans_try_cnt; ///< transaction (transmit and response) try counter
|
||||
} _scan;
|
||||
class PACKED REQ_SW_VER {
|
||||
public:
|
||||
uint8_t msgid { (uint8_t)MsgType::REQ_SW_VER };
|
||||
};
|
||||
|
||||
/// fast-throttle command configuration
|
||||
struct fast_throttle_config
|
||||
{
|
||||
uint16_t bits_to_add_left; ///< bits to add after the header
|
||||
uint8_t command[4]; ///< fast-throttle command frame header bytes
|
||||
uint8_t byte_count; ///< nr bytes in a fast throttle command
|
||||
uint8_t min_id; ///< Zero-indexed ESC ID
|
||||
uint8_t max_id; ///< Zero-indexed ESC ID
|
||||
} _fast_throttle;
|
||||
class PACKED REQ_SN {
|
||||
public:
|
||||
uint8_t msgid { (uint8_t)MsgType::REQ_SN };
|
||||
};
|
||||
|
||||
/// response length lookup table, saves 104 bytes of flash and speeds up the pull_command() function
|
||||
uint8_t _response_length[uint8_t(msg_type::SIZEOF_RESPONSE_LENGTH)];
|
||||
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;
|
||||
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
|
||||
|
|
|
|||
|
|
@ -10,46 +10,74 @@ For purchase, connection and configuration information please see the [Ardupilot
|
|||
|
||||
- use ArduPilot's coding guidelines and naming conventions
|
||||
- control motor speed
|
||||
- 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
|
||||
- report telemetry communication error rate in the dataflash logs
|
||||
- warn the user if there is a gap in the bitmask parameter.
|
||||
- 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
|
||||
- ~~report telemetry communication error rate in the dataflash logs~~
|
||||
- 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-enumerate all ESCs if not armed (motors not spinning) when
|
||||
- there is a gap in their address space IDs
|
||||
- communication with one of the ESCs is lost
|
||||
- some of the configured ESCs are not found
|
||||
- some of the configured ESCs are not correctly configured
|
||||
- allows the user to configure motor rotation direction per ESC (only gets updated if not armed)
|
||||
- adds a serial simulator of FETtec OneWire ESCs
|
||||
- adds autotest (using the simulator) to fly a copter over a simulated serial link connection
|
||||
- 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 to save memory.
|
||||
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.
|
||||
|
||||
On this device driver the `SERVO_FTW_MASK` parameter must contain a single contiguous block of bits set, and bit0 (zero-indexed) must be a part of that set.
|
||||
The ESC IDs (one-indexed) set on the ESCs must also be a single contiguous block, but it can start at an ID different from 1. The max ID must be lower than 12.
|
||||
There are two types of messages sent to the ESCs configuration and fast-throttle messages:
|
||||
|
||||
There are two types of messages sent to the ESCs:
|
||||
|
||||
### Configuration message
|
||||
### Configuration message frame
|
||||
Consists of six frame bytes + payload bytes.
|
||||
|
||||
```
|
||||
Byte 0 is the transfer direction (e.g. 0x01 Master to Slave)
|
||||
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.
|
||||
```
|
||||
```
|
||||
|
||||
### Fast Throttle Signal
|
||||
#### 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
|
||||
|
|
@ -73,11 +101,14 @@ See *ESC to Ardupilot Protocol* section below and comments in `FETtecOneWire.cpp
|
|||
|
||||
### Timing
|
||||
|
||||
Four ESCs need 90uS for the throttle request and telemetry reception. With four ESCs 11kHz are possible.
|
||||
As each additional ESC adds 11 extra fast-throttle command bits, so the rate is lowered by each ESC.
|
||||
If you use 8 ESCs, it needs 160uS including telemetry response, so 5.8kHz are possible.
|
||||
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:** You need at least a 4Hz motor signal (max 250ms between messages) before the motors disarm.
|
||||
**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
|
||||
|
||||
|
|
@ -98,26 +129,50 @@ This information is used by Ardupilot to:
|
|||
- Optionally measure battery voltage and power consumption
|
||||
|
||||
|
||||
## 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, `msg_type::SET_TLM_TYPE` is send to each ESC while initializing.
|
||||
If this was successful set the ESC response with `msg_type::OK`.
|
||||
|
||||
The answer is packed inside a OneWire package, that can be received with the `FETtecOneWire::receive()` function, that also checks the CRC.
|
||||
|
||||
As the packages are send in an uInt8_t array the values must be restored like as only temp is one byte long:
|
||||
|
||||
```C++
|
||||
Telemetry[0]= telem[0]; // Temperature [°C/10]
|
||||
Telemetry[1]=(telem[1]<<8)|telem[2]; // Voltage [V/10]
|
||||
Telemetry[2]=(telem[3]<<8)|telem[4]; // Current [A/10]
|
||||
Telemetry[3]=(telem[5]<<8)|telem[6]; // ERPM/100 (must be divided by number of motor poles to translate to propeller RPM)
|
||||
Telemetry[4]=(telem[7]<<8)|telem[8]; // Consumption [mA.h]
|
||||
Telemetry[5]=(telem[9]<<8)|telem[10];// CRC error (ArduPilot->ESC) counter
|
||||
```
|
||||
### 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:
|
||||
|
||||
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 is only visible if the `SERVO_FTW_MASK` parameter has at least one bit set.
|
||||
This parameter effects the outputs immediately when changed and the motors are not armed.
|
||||
|
||||
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.
|
||||
|
||||
## Extra features
|
||||
|
||||
|
|
@ -130,7 +185,7 @@ To control this you must activate the code in the header file:
|
|||
#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 `_found_escs[]` datastructure.
|
||||
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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
Loading…
Reference in New Issue