aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
a3d9a1d463
Ardupilot2/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h
Peter Barker ea7c7f9d19 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
2021-08-13 16:22:37 +10:00

501 lines
16 KiB
C++
Raw Blame History

/*
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
static constexpr uint8_t SERIAL_NUMBER_LENGTH = 12; ///< ESC serial number length in bytes
/**
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
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
#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;
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
Reference in New Issue View Git Blame Copy Permalink