diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp index 235c34777f..5615d51237 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp @@ -14,257 +14,286 @@ */ /* Initial protocol implementation was provided by FETtec */ -#include -#include -#include "AP_FETtecOneWire.h" -#include +/* Strongly modified by Amilcar Lucas, IAV GmbH */ + +#include +#include +#include +#include +#include + +#include "AP_FETtecOneWire.h" +#if HAL_AP_FETTEC_ONEWIRE_ENABLED + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_FETtecOneWire::var_info[] { + + // @Param: MASK + // @DisplayName: Servo channel output bitmask + // @Description: Servo channel mask specifying FETtec ESC output. Set bits must be contiguous. + // @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 + // @RebootRequired: False + // @User: Standard + AP_GROUPINFO("POLES", 3, AP_FETtecOneWire, _pole_count_parameter, 14), +#endif + + AP_GROUPEND +}; + +AP_FETtecOneWire *AP_FETtecOneWire::_singleton; -// constructor AP_FETtecOneWire::AP_FETtecOneWire() { - FETtecOneWire_ResponseLength[OW_OK] = 1; - FETtecOneWire_ResponseLength[OW_BL_PAGE_CORRECT] = 1; // BL only - FETtecOneWire_ResponseLength[OW_NOT_OK] = 1; - FETtecOneWire_ResponseLength[OW_BL_START_FW] = 0; // BL only - FETtecOneWire_ResponseLength[OW_BL_PAGES_TO_FLASH] = 1; // BL only - FETtecOneWire_ResponseLength[OW_REQ_TYPE] = 1; - FETtecOneWire_ResponseLength[OW_REQ_SN] = 12; - FETtecOneWire_ResponseLength[OW_REQ_SW_VER] = 2; - FETtecOneWire_ResponseLength[OW_RESET_TO_BL] = 0; - FETtecOneWire_ResponseLength[OW_THROTTLE] = 1; - FETtecOneWire_ResponseLength[OW_REQ_TLM] = 2; - FETtecOneWire_ResponseLength[OW_BEEP] = 0; + AP_Param::setup_object_defaults(this, var_info); - FETtecOneWire_ResponseLength[OW_SET_FAST_COM_LENGTH] = 1; - - FETtecOneWire_ResponseLength[OW_GET_ROTATION_DIRECTION] = 1; - FETtecOneWire_ResponseLength[OW_SET_ROTATION_DIRECTION] = 1; - - FETtecOneWire_ResponseLength[OW_GET_USE_SIN_START] = 1; - FETtecOneWire_ResponseLength[OW_SET_USE_SIN_START] = 1; - - FETtecOneWire_ResponseLength[OW_GET_3D_MODE] = 1; - FETtecOneWire_ResponseLength[OW_SET_3D_MODE] = 1; - - FETtecOneWire_ResponseLength[OW_GET_ID] = 1; - FETtecOneWire_ResponseLength[OW_SET_ID] = 1; - -/* - FETtecOneWire_ResponseLength[OW_GET_LINEAR_THRUST] = 1; - FETtecOneWire_ResponseLength[OW_SET_LINEAR_THRUST] = 1; -*/ - - FETtecOneWire_ResponseLength[OW_GET_EEVER] = 1; - - FETtecOneWire_ResponseLength[OW_GET_PWM_MIN] = 2; - FETtecOneWire_ResponseLength[OW_SET_PWM_MIN] = 1; - - FETtecOneWire_ResponseLength[OW_GET_PWM_MAX] = 2; - FETtecOneWire_ResponseLength[OW_SET_PWM_MAX] = 1; - - FETtecOneWire_ResponseLength[OW_GET_ESC_BEEP] = 1; - FETtecOneWire_ResponseLength[OW_SET_ESC_BEEP] = 1; - - FETtecOneWire_ResponseLength[OW_GET_CURRENT_CALIB] = 1; - FETtecOneWire_ResponseLength[OW_SET_CURRENT_CALIB] = 1; - - FETtecOneWire_ResponseLength[OW_SET_LED_TMP_COLOR] = 0; - FETtecOneWire_ResponseLength[OW_GET_LED_COLOR] = 5; - FETtecOneWire_ResponseLength[OW_SET_LED_COLOR] = 1; - - FETtecOneWire_RequestLength[OW_OK] = 1; - FETtecOneWire_RequestLength[OW_BL_PAGE_CORRECT] = 1; // BL only - FETtecOneWire_RequestLength[OW_NOT_OK] = 1; - FETtecOneWire_RequestLength[OW_BL_START_FW] = 1; // BL only - FETtecOneWire_RequestLength[OW_BL_PAGES_TO_FLASH] = 1; // BL only - FETtecOneWire_RequestLength[OW_REQ_TYPE] = 1; - FETtecOneWire_RequestLength[OW_REQ_SN] = 1; - FETtecOneWire_RequestLength[OW_REQ_SW_VER] = 1; - FETtecOneWire_RequestLength[OW_RESET_TO_BL] = 1; - FETtecOneWire_RequestLength[OW_THROTTLE] = 1; - FETtecOneWire_RequestLength[OW_REQ_TLM] = 1; - FETtecOneWire_RequestLength[OW_BEEP] = 2; - - FETtecOneWire_RequestLength[OW_SET_FAST_COM_LENGTH] = 4; - - FETtecOneWire_RequestLength[OW_GET_ROTATION_DIRECTION] = 1; - FETtecOneWire_RequestLength[OW_SET_ROTATION_DIRECTION] = 1; - - FETtecOneWire_RequestLength[OW_GET_USE_SIN_START] = 1; - FETtecOneWire_RequestLength[OW_SET_USE_SIN_START] = 1; - - FETtecOneWire_RequestLength[OW_GET_3D_MODE] = 1; - FETtecOneWire_RequestLength[OW_SET_3D_MODE] = 1; - - FETtecOneWire_RequestLength[OW_GET_ID] = 1; - FETtecOneWire_RequestLength[OW_SET_ID] = 1; - -/* - FETtecOneWire_RequestLength[OW_GET_LINEAR_THRUST] = 1; - FETtecOneWire_RequestLength[OW_SET_LINEAR_THRUST] = 1; -*/ - - FETtecOneWire_RequestLength[OW_GET_EEVER] = 1; - - FETtecOneWire_RequestLength[OW_GET_PWM_MIN] = 1; - FETtecOneWire_RequestLength[OW_SET_PWM_MIN] = 2; - - FETtecOneWire_RequestLength[OW_GET_PWM_MAX] = 1; - FETtecOneWire_RequestLength[OW_SET_PWM_MAX] = 2; - - FETtecOneWire_RequestLength[OW_GET_ESC_BEEP] = 1; - FETtecOneWire_RequestLength[OW_SET_ESC_BEEP] = 1; - - FETtecOneWire_RequestLength[OW_GET_CURRENT_CALIB] = 1; - FETtecOneWire_RequestLength[OW_SET_CURRENT_CALIB] = 1; - - FETtecOneWire_RequestLength[OW_SET_LED_TMP_COLOR] = 4; - FETtecOneWire_RequestLength[OW_GET_LED_COLOR] = 1; - FETtecOneWire_RequestLength[OW_SET_LED_COLOR] = 5; +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (_singleton != nullptr) { + AP_HAL::panic("AP_FETtecOneWire must be singleton"); + } +#endif + _singleton = this; + _response_length[uint8_t(msg_type::OK)] = 1; + _response_length[uint8_t(msg_type::BL_START_FW)] = 0; // Bootloader only +#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO + _response_length[uint8_t(msg_type::REQ_TYPE)] = 1; + _response_length[uint8_t(msg_type::REQ_SN)] = 12; + _response_length[uint8_t(msg_type::REQ_SW_VER)] = 2; +#endif + _response_length[uint8_t(msg_type::SET_FAST_COM_LENGTH)] = 1; + _response_length[uint8_t(msg_type::SET_TLM_TYPE)] = 1; } +/** + initialize the serial port, scan the bus, setup the found ESCs + +*/ void AP_FETtecOneWire::init() { - AP_SerialManager& serial_manager = AP::serialmanager(); - _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FETtechOneWire, 0); - if (_uart) { - _uart->begin(2000000); - } - FETtecOneWire_Init(); -} - -void AP_FETtecOneWire::update() -{ - if (!initialised) { - initialised = true; - init(); - last_send_us = AP_HAL::micros(); - return; - } - if (_uart == nullptr) { + 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); +#if HAL_AP_FETTEC_HALF_DUPLEX + if (_uart->get_options() & _uart->OPTION_HDPLEX) { //Half-Duplex is enabled + _use_hdplex = true; + _uart->begin(2000000U); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FTW using Half-Duplex"); + } else { + _uart->begin(500000U); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FTW using Full-Duplex"); + } +#else + _uart->begin(500000U); +#endif + } + + if (_scan.state != scan_state_t::DONE) { + scan_escs(); return; } - uint16_t requestedTelemetry[MOTOR_COUNT] = {0}; - int8_t TelemetryAvailable = FETtecOneWire_ESCsSetValues(motorpwm, requestedTelemetry, MOTOR_COUNT, TLM_request); - if (TelemetryAvailable != -1) { - for (uint8_t i = 0; i < MOTOR_COUNT; i++) { - completeTelemetry[i][TelemetryAvailable] = requestedTelemetry[i]; - } - } - if (++TLM_request == 5) { - TLM_request = 0; - } - if (TelemetryAvailable != -1) { - for (uint8_t i = 0; i < MOTOR_COUNT; i++) { - printf(" esc: %d", i + 1); - printf(" Temperature: %d", completeTelemetry[i][0]); - printf(", Voltage: %d", completeTelemetry[i][1]); - printf(", Current: %d", completeTelemetry[i][2]); - printf(", E-rpm: %d", completeTelemetry[i][3]); - printf(", consumption: %d", completeTelemetry[i][4]); - printf("\n"); +#if HAL_WITH_ESC_TELEM + _update_rate_hz = AP::scheduler().get_loop_rate_hz(); + _crc_error_rate_factor = 100.0f/(float)_update_rate_hz; //to save the division in loop, precalculate by the motor loops 100%/400Hz +#endif + + // do not read telemetry information until a fast-throttle command is send + // and if HAL_WITH_ESC_TELEM is disabled this also ensures correct operation + _requested_telemetry_from_esc = -1; + + // get the user-configured FETtec ESCs bitmask parameter + // if the user changes this parameter, he will have to reboot + _motor_mask = uint16_t(_motor_mask_parameter.get()); + uint16_t smask = _motor_mask; // shifted version of the _motor_mask user parameter + uint16_t mmask = 0; // will be a copy of _motor_mask with only the contiguous LSBs set + + static_assert(MOTOR_COUNT_MAX <= sizeof(_motor_mask)*8, "_motor_mask is too narrow for MOTOR_COUNT_MAX ESCs"); + static_assert(MOTOR_COUNT_MAX <= sizeof(_reverse_mask)*8, "_reverse_mask is too narrow for MOTOR_COUNT_MAX ESCs"); + static_assert(MOTOR_COUNT_MAX <= sizeof(smask)*8, "smask is too narrow for MOTOR_COUNT_MAX ESCs"); + static_assert(MOTOR_COUNT_MAX <= sizeof(mmask)*8, "mmask is too narrow for MOTOR_COUNT_MAX ESCs"); + + _nr_escs_in_bitmask = 0; + // count the number of contiguous user-configured FETtec ESCs in the bitmask parameter + for (uint8_t i = 0; i < MOTOR_COUNT_MAX; i++) { + if ((smask & 0x01) == 0x00) { + break; } + smask >>= 1; + _nr_escs_in_bitmask++; + + // build a copy of _motor_mask_parameter with only the contiguous LSBs set + mmask |= 0x1; + mmask <<= 1; } + + // tell SRV_Channels about ESC capabilities + SRV_Channels::set_digital_outputs(mmask, 0); + + _initialised = true; } -/* - initialize FETtecOneWire protocol +/** + check if the current configuration is OK */ -void AP_FETtecOneWire::FETtecOneWire_Init() +void AP_FETtecOneWire::configuration_check() { - if (FETtecOneWire_firstInitDone == 0) { - FETtecOneWire_FoundESCs = 0; - FETtecOneWire_ScanActive = 0; - FETtecOneWire_SetupActive = 0; - FETtecOneWire_minID = 25; - FETtecOneWire_maxID = 0; - FETtecOneWire_IDcount = 0; - FETtecOneWire_FastThrottleByteCount = 0; - for (uint8_t i = 0; i < 25; i++) { - FETtecOneWire_activeESC_IDs[i] = 0; - } + if (hal.util->get_soft_armed()) { + return; // checks are only done when vehicle is disarmed, because the GCS_SEND_TEXT() function calls use lots of resources + } + + // for safety, only update the reversed motors bitmask when motors are disarmed + _reverse_mask= _reverse_mask_parameter; + + const uint32_t now = AP_HAL::millis(); + if ((now - _last_config_check_ms < 3000) && _last_config_check_ms != 0) { // only runs once every 3 seconds + return; + } + _last_config_check_ms = now; + +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL + if (!_uart->is_dma_enabled()) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FTW UART needs DMA"); + return; + } +#endif + + const bool all_escs_found = _found_escs_count >= _nr_escs_in_bitmask; + const bool all_escs_configured = _found_escs_count == _configured_escs; + const bool all_escs_contiguous = _fast_throttle.max_id - _fast_throttle.min_id < _found_escs_count; + bool telem_rx_missing = false; +#if HAL_WITH_ESC_TELEM + // TLM recovery, if e.g. a power loss occurred but FC is still powered by USB. + const uint16_t active_esc_mask = AP::esc_telem().get_active_esc_mask(); + const uint8_t num_active_escs = __builtin_popcount(active_esc_mask & _motor_mask); + + telem_rx_missing = (num_active_escs < _nr_escs_in_bitmask) && (_sent_msg_count > 2 * MOTOR_COUNT_MAX); +#endif + + if (__builtin_popcount(_motor_mask_parameter.get()) != _nr_escs_in_bitmask) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FTW: gap in SERVO_FTW_MASK parameter bits"); + } + + if (!all_escs_contiguous){ + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FTW: gap in IDs found"); + } + + if (!all_escs_found) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FTW: found only %u of %u ESCs", _found_escs_count, _nr_escs_in_bitmask); + } + + if (!all_escs_configured) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FTW: configured only %u of %u ESCs", _configured_escs, _found_escs_count); + } + +#if HAL_WITH_ESC_TELEM + if (telem_rx_missing) { + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FTW: got TLM from only %u of %u ESCs", num_active_escs, _nr_escs_in_bitmask); + } +#endif + + if (!all_escs_contiguous || !all_escs_found || !all_escs_configured || telem_rx_missing) { + // re-init the entire device driver +#if HAL_WITH_ESC_TELEM + _sent_msg_count = 0; +#endif + _scan.state = scan_state_t::WAIT_FOR_BOOT; + _initialised = false; } - FETtecOneWire_IgnoreOwnBytes = 0; - FETtecOneWire_PullSuccess = 0; - FETtecOneWire_PullBusy = 0; - FETtecOneWire_firstInitDone = 1; } -/* - generates used 8 bit CRC - crc = byte to be added to CRC - crc_seed = CRC where it gets added too - returns 8 bit CRC +/** + 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 (max 4) + @return false if length is bigger than MAX_TRANSMIT_LENGTH, true on write success */ -uint8_t AP_FETtecOneWire::FETtecOneWire_UpdateCrc8(uint8_t crc, uint8_t crc_seed) -{ - uint8_t crc_u, i; - crc_u = crc; - crc_u ^= crc_seed; - for (i = 0; i < 8; i++) { - crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1); - } - return (crc_u); -} - -/* - generates used 8 bit CRC for arrays - Buf = 8 bit byte array - BufLen = count of bytes that should be used for CRC calculation - returns 8 bit CRC -*/ -uint8_t AP_FETtecOneWire::FETtecOneWire_GetCrc8(uint8_t* Buf, uint16_t BufLen) -{ - uint8_t crc = 0; - for (uint16_t i = 0; i < BufLen; i++) { - crc = FETtecOneWire_UpdateCrc8(Buf[i], crc); - } - return (crc); -} - -/* - transmitts a FETtecOneWire frame to a ESC - ESC_id = id of the ESC - Bytes = 8 bit array of bytes. Where byte 1 contains the command, and all following bytes can be the payload - Length = length of the Bytes array - returns nothing -*/ -void AP_FETtecOneWire::FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t* Bytes, uint8_t Length) +bool AP_FETtecOneWire::transmit(const uint8_t esc_id, const uint8_t* bytes, uint8_t length) { /* - a frame lookes like: - byte 1 = fremae header (master is always 0x01) + 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 */ - uint8_t transmitArr[256] = {0x01, ESC_id, 0x00, 0x00}; - transmitArr[4] = Length + 6; - for (uint8_t i = 0; i < Length; i++) { - transmitArr[i + 5] = Bytes[i]; + uint8_t transmit_arr[FRAME_OVERHEAD+MAX_TRANSMIT_LENGTH] = {0x01}; + transmit_arr[1] = esc_id+uint8_t(1); // one-indexed ESC ID + if (length > _uart->txspace() || length > MAX_TRANSMIT_LENGTH) { + return false; // no, do not send at all } - transmitArr[Length + 5] = FETtecOneWire_GetCrc8(transmitArr, Length + 5); // crc - _uart->write(transmitArr, Length + 6); - FETtecOneWire_IgnoreOwnBytes += Length + 6; + transmit_arr[4] = length + FRAME_OVERHEAD; + for (uint8_t i = 0; i < length; i++) { + transmit_arr[i + 5] = bytes[i]; + } + transmit_arr[length + 5] = crc8_dvb_update(0, transmit_arr, length + 5); // crc + _uart->write(transmit_arr, length + FRAME_OVERHEAD); +#if HAL_AP_FETTEC_HALF_DUPLEX + if (_use_hdplex) { + _ignore_own_bytes += length + 6; + } +#endif + return true; } -/* - reads the answer frame of a ESC - Bytes = 8 bit byte array, where the received answer gets stored in - Length = the expected answer length - returnFullFrame can be OW_RETURN_RESPONSE or OW_RETURN_FULL_FRAME - returns 1 if the expected answer frame was there, 0 if dont +void AP_FETtecOneWire::move_preamble_in_receive_buffer(uint8_t search_start_pos) +{ + uint8_t i; + for (i=search_start_pos; i MAX_RECEIVE_LENGTH) { + return receive_response::REQ_OVERLENGTH; + } + +#if HAL_AP_FETTEC_HALF_DUPLEX //ignore own bytes - while (FETtecOneWire_IgnoreOwnBytes > 0 && _uart->available()) { - FETtecOneWire_IgnoreOwnBytes--; - _uart->read(); - } - // look for the real answer - if (_uart->available() >= Length + 6u) { - // sync to frame starte byte - uint8_t testFrameStart = 0; - do { - testFrameStart = _uart->read(); - } - while (testFrameStart != 0x02 && testFrameStart != 0x03 && _uart->available()); - // copy message - if (_uart->available() >= Length + 5u) { - uint8_t ReceiveBuf[20] = {0}; - ReceiveBuf[0] = testFrameStart; - for (uint8_t i = 1; i < Length + 6; i++) { - ReceiveBuf[i] = _uart->read(); - } - // check CRC - if (FETtecOneWire_GetCrc8(ReceiveBuf, Length + 5) == ReceiveBuf[Length + 5]) { - if (!returnFullFrame) { - for (uint8_t i = 0; i < Length; i++) { - Bytes[i] = ReceiveBuf[5 + i]; - } - } else { - for (uint8_t i = 0; i < Length + 6; i++) { - Bytes[i] = ReceiveBuf[i]; - } - } - return 1; - } else { - return 0; - } // crc missmatch - } else { - return 0; - } // no answer yet - } else { - return 0; - } // no answer yet -} - -/* - makes all connected ESC's beep - beepFreqency = a 8 bit value from 0-255. higher make a higher beep -*/ -void AP_FETtecOneWire::FETtecOneWire_Beep(uint8_t beepFreqency) -{ - if (FETtecOneWire_IDcount > 0) { - uint8_t request[2] = {OW_BEEP, beepFreqency}; - uint8_t spacer[2] = {0, 0}; - for (uint8_t i = FETtecOneWire_minID; i < FETtecOneWire_maxID + 1; i++) { - FETtecOneWire_Transmit(i, request, FETtecOneWire_RequestLength[request[0]]); - // add two zeros to make sure all ESC's can catch their command as we dont wait for a response here - _uart->write(spacer, 2); - FETtecOneWire_IgnoreOwnBytes += 2; - } - } -} - -/* - sets the racewire color for all ESC's - R, G, B = 8bit colors -*/ -void AP_FETtecOneWire::FETtecOneWire_RW_LEDcolor(uint8_t R, uint8_t G, uint8_t B) -{ - if (FETtecOneWire_IDcount > 0) { - uint8_t request[4] = {OW_SET_LED_TMP_COLOR, R, G, B}; - uint8_t spacer[2] = {0, 0}; - for (uint8_t i = FETtecOneWire_minID; i < FETtecOneWire_maxID + 1; i++) { - FETtecOneWire_Transmit(i, request, FETtecOneWire_RequestLength[request[0]]); - // add two zeros to make sure all ESC's can catch their command as we dont wait for a response here - _uart->write(spacer, 2); - FETtecOneWire_IgnoreOwnBytes += 2; - } - } -} - -/* - Resets a pending pull request - returns nothing -*/ -void AP_FETtecOneWire::FETtecOneWire_PullReset() -{ - FETtecOneWire_PullSuccess = 0; - FETtecOneWire_PullBusy = 0; -} - -/* - Pulls a complete request between for ESC - ESC_id = id of the ESC - command = 8bit array containing the command that thould be send including the possible payload - response = 8bit array where the response will be stored in - returnFullFrame can be OW_RETURN_RESPONSE or OW_RETURN_FULL_FRAME - returns 1 if the request is completed, 0 if dont -*/ -uint8_t AP_FETtecOneWire::FETtecOneWire_PullCommand(uint8_t ESC_id, uint8_t* command, uint8_t* response, - uint8_t returnFullFrame) -{ - if (!FETtecOneWire_PullBusy) { - FETtecOneWire_PullBusy = 1; - FETtecOneWire_PullSuccess = 0; - FETtecOneWire_Transmit(ESC_id, command, FETtecOneWire_RequestLength[command[0]]); - } else { - if (FETtecOneWire_Receive(response, FETtecOneWire_ResponseLength[command[0]], returnFullFrame)) { - FETtecOneWire_PullSuccess = 1; - FETtecOneWire_PullBusy = 0; - } - } - return FETtecOneWire_PullSuccess; -} - -/* - scans for ESC's in bus. should be called intill FETtecOneWire_ScanActive >= 25 - returns the currend scanned ID -*/ -uint8_t AP_FETtecOneWire::FETtecOneWire_ScanESCs() -{ - static uint16_t delayLoops = 500; - static uint8_t scanID = 0; - static uint8_t scanState = 0; - static uint8_t scanTimeOut = 0; - uint8_t response[18] = {0}; - uint8_t request[1] = {0}; - if (FETtecOneWire_ScanActive == 0) { - delayLoops = 500; - scanID = 0; - scanState = 0; - scanTimeOut = 0; - return FETtecOneWire_ScanActive + 1; - } - if (delayLoops > 0) { - delayLoops--; - return FETtecOneWire_ScanActive; - } - if (scanID < FETtecOneWire_ScanActive) { - scanID = FETtecOneWire_ScanActive; - scanState = 0; - scanTimeOut = 0; - } - if (scanTimeOut == 3 || scanTimeOut == 6 || scanTimeOut == 9 || scanTimeOut == 12) { - FETtecOneWire_PullReset(); - } - if (scanTimeOut < 15) { - switch (scanState) { - case 0:request[0] = OW_OK; - if (FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_FULL_FRAME)) { - scanTimeOut = 0; - FETtecOneWire_activeESC_IDs[scanID] = 1; - FETtecOneWire_FoundESCs++; - if (response[0] == 0x02) { - FETtecOneWire_foundESCs[scanID].inBootLoader = 1; - } else { - FETtecOneWire_foundESCs[scanID].inBootLoader = 0; - } - delayLoops = 1; - scanState++; - } else { - scanTimeOut++; - } - break; - case 1:request[0] = OW_REQ_TYPE; - if (FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_RESPONSE)) { - scanTimeOut = 0; - FETtecOneWire_foundESCs[scanID].ESCtype = response[0]; - delayLoops = 1; - scanState++; - } else { - scanTimeOut++; - } - break; - case 2:request[0] = OW_REQ_SW_VER; - if (FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_RESPONSE)) { - scanTimeOut = 0; - FETtecOneWire_foundESCs[scanID].firmWareVersion = response[0]; - FETtecOneWire_foundESCs[scanID].firmWareSubVersion = response[1]; - delayLoops = 1; - scanState++; - } else { - scanTimeOut++; - } - break; - case 3:request[0] = OW_REQ_SN; - if (FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_RESPONSE)) { - scanTimeOut = 0; - for (uint8_t i = 0; i < 12; i++) { - FETtecOneWire_foundESCs[scanID].serialNumber[i] = response[i]; - } - delayLoops = 1; - return scanID + 1; - } else { - scanTimeOut++; - } - break; - } - } else { - FETtecOneWire_PullReset(); - return scanID + 1; - } - return scanID; -} - -/* - starts all ESC's in bus and prepares them for receiving teh fast throttle command should be called untill FETtecOneWire_SetupActive >= 25 - returns the current used ID -*/ -uint8_t AP_FETtecOneWire::FETtecOneWire_InitESCs() -{ - static uint8_t delayLoops = 0; - static uint8_t activeID = 1; - static uint8_t State = 0; - static uint8_t TimeOut = 0; - static uint8_t wakeFromBL = 1; - static uint8_t setFastCommand[4] = {OW_SET_FAST_COM_LENGTH, 0, 0, 0}; - uint8_t response[18] = {0}; - uint8_t request[1] = {0}; - if (FETtecOneWire_SetupActive == 0) { - delayLoops = 0; - activeID = 1; - State = 0; - TimeOut = 0; - wakeFromBL = 1; - return FETtecOneWire_SetupActive + 1; - } - while (FETtecOneWire_activeESC_IDs[FETtecOneWire_SetupActive] == 0 && FETtecOneWire_SetupActive < 25) { - FETtecOneWire_SetupActive++; - } - - if (FETtecOneWire_SetupActive == 25 && wakeFromBL == 0) { - return FETtecOneWire_SetupActive; - } else if (FETtecOneWire_SetupActive == 25 && wakeFromBL) { - wakeFromBL = 0; - activeID = 1; - FETtecOneWire_SetupActive = 1; - State = 0; - TimeOut = 0; - - FETtecOneWire_minID = 25; - FETtecOneWire_maxID = 0; - FETtecOneWire_IDcount = 0; - for (uint8_t i = 0; i < 25; i++) { - if (FETtecOneWire_activeESC_IDs[i] != 0) { - FETtecOneWire_IDcount++; - if (i < FETtecOneWire_minID) { - FETtecOneWire_minID = i; - } - if (i > FETtecOneWire_maxID) { - FETtecOneWire_maxID = i; - } - } - } - - if (FETtecOneWire_IDcount == 0 - || FETtecOneWire_maxID - FETtecOneWire_minID > FETtecOneWire_IDcount - 1) { // loop forever - wakeFromBL = 1; - return activeID; - } - FETtecOneWire_FastThrottleByteCount = 1; - int8_t bitCount = 12 + (FETtecOneWire_IDcount * 11); - while (bitCount > 0) { - FETtecOneWire_FastThrottleByteCount++; - bitCount -= 8; - } - setFastCommand[1] = FETtecOneWire_FastThrottleByteCount; // just for older ESC FW versions since 1.0 001 this byte is ignored as the ESC calculates it itself - setFastCommand[2] = FETtecOneWire_minID; // min ESC id - setFastCommand[3] = FETtecOneWire_IDcount; // count of ESC's that will get signals - } - - if (delayLoops > 0) { - delayLoops--; - return FETtecOneWire_SetupActive; - } - - if (activeID < FETtecOneWire_SetupActive) { - activeID = FETtecOneWire_SetupActive; - State = 0; - TimeOut = 0; - } - - if (TimeOut == 3 || TimeOut == 6 || TimeOut == 9 || TimeOut == 12) { - FETtecOneWire_PullReset(); - } - - if (TimeOut < 15) { - if (wakeFromBL) { - switch (State) { - case 0:request[0] = OW_BL_START_FW; - if (FETtecOneWire_foundESCs[activeID].inBootLoader == 1) { - FETtecOneWire_Transmit(activeID, request, FETtecOneWire_RequestLength[request[0]]); - delayLoops = 5; - } else { - return activeID + 1; - } - State = 1; - break; - case 1:request[0] = OW_OK; - if (FETtecOneWire_PullCommand(activeID, request, response, OW_RETURN_FULL_FRAME)) { - TimeOut = 0; - if (response[0] == 0x02) { - FETtecOneWire_foundESCs[activeID].inBootLoader = 1; - State = 0; - } else { - FETtecOneWire_foundESCs[activeID].inBootLoader = 0; - delayLoops = 1; - return activeID + 1; - } - } else { - TimeOut++; - } - break; - } - } else { - if (FETtecOneWire_PullCommand(activeID, setFastCommand, response, OW_RETURN_RESPONSE)) { - TimeOut = 0; - delayLoops = 1; - return activeID + 1; - } else { - TimeOut++; - } - } - } else { - FETtecOneWire_PullReset(); - return activeID + 1; - } - return activeID; -} - -/* - checks if the requested telemetry is available. - Telemetry = 16bit array where the read Telemetry will be stored in. - returns the telemetry request number or -1 if unavailable -*/ -int8_t AP_FETtecOneWire::FETtecOneWire_CheckForTLM(uint16_t* Telemetry) -{ - int8_t return_TLM_request = 0; - if (FETtecOneWire_IDcount > 0) { - // empty buffer - while (FETtecOneWire_IgnoreOwnBytes > 0 && _uart->available()) { + if (_use_hdplex) { + while (_ignore_own_bytes > 0 && _uart->available()) { + _ignore_own_bytes--; _uart->read(); - FETtecOneWire_IgnoreOwnBytes--; } - - // first two byte are the ESC Telemetry of the first ESC. next two byte of the second.... - if (_uart->available() == (FETtecOneWire_IDcount * 2) + 1u) { - // look if first byte in buffer is equal to last byte of throttle command (crc) - if (_uart->read() == FETtecOneWire_lastCRC) { - for (uint8_t i = 0; i < FETtecOneWire_IDcount; i++) { - Telemetry[i] = _uart->read() << 8; - Telemetry[i] |= _uart->read(); - } - return_TLM_request = FETtecOneWire_TLM_request; - } else { - return_TLM_request = -1; - } - } else { - return_TLM_request = -1; - } - } else { - return_TLM_request = -1; } - return return_TLM_request; +#endif + + // read as much from the uart as we can: + const uint8_t bytes_to_read = ARRAY_SIZE(receive_buf) - receive_buf_used; + uint32_t nbytes = _uart->read(&receive_buf[receive_buf_used], bytes_to_read); + if (nbytes == 0) { + return receive_response::NO_ANSWER_YET; + } + receive_buf_used += nbytes; + + move_preamble_in_receive_buffer(); + + // we know what length of message should be present + const uint8_t raw_length = FRAME_OVERHEAD + length; + if (receive_buf_used < raw_length) { + return receive_response::NO_ANSWER_YET; + } + + if (crc8_dvb_update(0, receive_buf, raw_length-1) != receive_buf[raw_length-1]) { + // bad message; shift away this preamble byte to try to find + // another message + move_preamble_in_receive_buffer(1); + return receive_response::CRC_MISSMATCH; + } + + // message is good + switch (return_full_frame) { + case return_type::RESPONSE: + memcpy(bytes, &receive_buf[5], length); + break; + case return_type::FULL_FRAME: + memcpy(bytes, receive_buf, raw_length); + break; + } + + consume_bytes(raw_length); + + return receive_response::ANSWER_VALID; } -/* - does almost all of the job. - scans for ESC's if not already done. - initializes the ESC's if not already done. - sends fast throttle signals if init is complete. - motorValues = a 16bit array containing the throttle signals that should be sent to the motors. 0-2000 where 1001-2000 is positive rotation and 999-0 reversed rotation - Telemetry = 16bit array where the read telemetry will be stored in. - motorCount = the count of motors that should get values send - tlmRequest = the requested telemetry type (OW_TLM_XXXXX) - returns the telemetry request if telemetry was available, -1 if dont +/** + 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 */ -int8_t AP_FETtecOneWire::FETtecOneWire_ESCsSetValues(uint16_t* motorValues, uint16_t* Telemetry, uint8_t motorCount, - uint8_t tlmRequest) +AP_FETtecOneWire::pull_state AP_FETtecOneWire::pull_command(const uint8_t esc_id, const uint8_t* command, uint8_t* response, + return_type return_full_frame, const uint8_t req_len) { - int8_t return_TLM_request = -2; + if (!_pull_busy) { + _pull_busy = transmit(esc_id, command, req_len); + } else if (receive(response, _response_length[command[0]], return_full_frame) == receive_response::ANSWER_VALID) { + _scan.rx_try_cnt = 0; + _scan.trans_try_cnt = 0; + _pull_busy = false; + return pull_state::COMPLETED; + } - // init should not be done too fast. as at last the bootloader has some timing requirements with messages. so loop delays must fit more or less - if (FETtecOneWire_ScanActive < 25 || FETtecOneWire_SetupActive < 25) { - const uint32_t now = AP_HAL::micros(); - if (now - last_send_us < DELAY_TIME_US) { - return 0; + // it will try multiple times to read the response of a request + if (_scan.rx_try_cnt > 1) { + _scan.rx_try_cnt = 0; + + _pull_busy = false; // re-transmit the request, in the hope of getting a valid response later + + if (_scan.trans_try_cnt > 4) { + // the request re-transmit failed multiple times + _scan.trans_try_cnt = 0; + return pull_state::FAILED; } else { - last_send_us = now; + _scan.trans_try_cnt++; + } + } else { + _scan.rx_try_cnt++; + } + return pull_state::BUSY; +} + +/** + 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 AP_FETtecOneWire::scan_escs() +{ + uint8_t response[FRAME_OVERHEAD + MAX_RECEIVE_LENGTH]; + uint8_t request[2]; + + const uint32_t now = AP_HAL::micros(); + if (now - _scan.last_us < (_scan.state == scan_state_t::WAIT_START_FW ? 5000U : 2000U)) { + // the scan_escs() call period must be bigger than 2000 US, + // as the bootloader has some message timing requirements. And we might be in bootloader + return; + } + _scan.last_us = now; + + switch (_scan.state) { + + // initial state, wait for a ESC(s) cold-start + case scan_state_t::WAIT_FOR_BOOT: + _found_escs_count = 0; + _scan.id = 0; + for (uint8_t i = 0; i < MOTOR_COUNT_MAX; i++) { + _found_escs[i].active = false; + } + if (now > 500000U) { + _scan.state = scan_state_t::IN_BOOTLOADER; + } + break; + + // is bootloader running? + case scan_state_t::IN_BOOTLOADER: + request[0] = uint8_t(msg_type::OK); + switch (pull_command(_scan.id, request, response, return_type::FULL_FRAME, 1)) { + case pull_state::BUSY: + break; + case pull_state::COMPLETED: + if (response[0] == 0x02) { + _scan.state = scan_state_t::START_FW; // is in bootloader, must start firmware + } else { + if (!_found_escs[_scan.id].active) { + _found_escs_count++; // found a new ESC not in bootloader + } + _found_escs[_scan.id].active = true; +#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO + _scan.state = scan_state_t::ESC_TYPE; +#else + _scan.state = scan_state_t::NEXT_ID; +#endif + } + break; + case pull_state::FAILED: + _scan.state = scan_state_t::NEXT_ID; + break; + } + break; + + // start the firmware + case scan_state_t::START_FW: + request[0] = uint8_t(msg_type::BL_START_FW); + if (transmit(_scan.id, request, 1)) { + _scan.state = scan_state_t::WAIT_START_FW; + } + break; + + // wait for the firmware to start + case scan_state_t::WAIT_START_FW: + _uart->discard_input(); // discard the answer to the previous transmit + _scan.state = scan_state_t::IN_BOOTLOADER; + break; + +#if HAL_AP_FETTEC_ONEWIRE_GET_STATIC_INFO + // ask the ESC type + case scan_state_t::ESC_TYPE: + request[0] = uint8_t(msg_type::REQ_TYPE); + switch (pull_command(_scan.id, request, response, return_type::RESPONSE, 1)) { + case pull_state::BUSY: + break; + case pull_state::COMPLETED: + _found_escs[_scan.id].esc_type = response[0]; + _scan.state = scan_state_t::SW_VER; + break; + case pull_state::FAILED: + _scan.state = scan_state_t::NEXT_ID; + break; + } + break; + + // ask the software version + case scan_state_t::SW_VER: + request[0] = uint8_t(msg_type::REQ_SW_VER); + switch (pull_command(_scan.id, request, response, return_type::RESPONSE, 1)) { + case pull_state::BUSY: + break; + case pull_state::COMPLETED: + _found_escs[_scan.id].firmware_version = response[0]; + _found_escs[_scan.id].firmware_sub_version = response[1]; + _scan.state = scan_state_t::SN; + break; + case pull_state::FAILED: + _scan.state = scan_state_t::NEXT_ID; + break; + } + break; + + // ask the serial number + case scan_state_t::SN: + request[0] = uint8_t(msg_type::REQ_SN); + switch (pull_command(_scan.id, request, response, return_type::RESPONSE, 1)) { + case pull_state::BUSY: + break; + case pull_state::COMPLETED: + for (uint8_t i = 0; i < SERIAL_NR_BITWIDTH; i++) { + _found_escs[_scan.id].serial_number[i] = response[i]; + } + _scan.state = scan_state_t::NEXT_ID; + break; + case pull_state::FAILED: + _scan.state = scan_state_t::NEXT_ID; + break; + } + break; +#endif + + // increment ESC ID and jump to IN_BOOTLOADER + case scan_state_t::NEXT_ID: + _scan.state = scan_state_t::IN_BOOTLOADER; + _scan.id++; // re-run this state machine with the next ESC ID + if (_scan.id == MOTOR_COUNT_MAX) { + _scan.id = 0; + if (_found_escs_count) { + // one or more ESCs found, scan is completed, now configure the ESCs found + config_fast_throttle(); + _scan.id = _fast_throttle.min_id; + _configured_escs = 0; + _scan.state = scan_state_t::CONFIG_FAST_THROTTLE; + } + } + break; + + // configure fast-throttle command header + case scan_state_t::CONFIG_FAST_THROTTLE: + switch (pull_command(_scan.id, _fast_throttle.command, response, return_type::RESPONSE, 4)) { + case pull_state::BUSY: + break; + case pull_state::COMPLETED: +#if HAL_WITH_ESC_TELEM + _scan.state = scan_state_t::CONFIG_TLM; +#else + _configured_escs++; + _scan.state = scan_state_t::CONFIG_NEXT_ACTIVE_ESC; +#endif + break; + case pull_state::FAILED: + _scan.state = scan_state_t::CONFIG_NEXT_ACTIVE_ESC; + break; + } + break; + +#if HAL_WITH_ESC_TELEM + // configure telemetry mode + case scan_state_t::CONFIG_TLM: + request[0] = uint8_t(msg_type::SET_TLM_TYPE); + request[1] = 1; // Alternative telemetry mode -> a single ESC sends it's full telem (Temp, Volt, Current, ERPM, Consumption, CrcErrCount) in a single frame + switch (pull_command(_scan.id, request, response, return_type::RESPONSE, 2)) { + case pull_state::BUSY: + break; + case pull_state::COMPLETED: + _configured_escs++; + _scan.state = scan_state_t::CONFIG_NEXT_ACTIVE_ESC; + break; + case pull_state::FAILED: + _scan.state = scan_state_t::CONFIG_NEXT_ACTIVE_ESC; + break; + } + break; +#endif + + // increment ESC ID and jump to CONFIG_FAST_THROTTLE + case scan_state_t::CONFIG_NEXT_ACTIVE_ESC: + do { + _scan.id++; + } while (_scan.id < MOTOR_COUNT_MAX && _found_escs[_scan.id].active == false); + _scan.state = scan_state_t::CONFIG_FAST_THROTTLE; + if (_scan.id == MOTOR_COUNT_MAX) { + _scan.id = 0; + _scan.state = scan_state_t::DONE; // one or more ESCs found, scan is completed + } + break; + + case scan_state_t::DONE: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } +} + +/** + configure the fast-throttle command. + Should be called once after scan_escs() is completted and before config_escs() +*/ +void AP_FETtecOneWire::config_fast_throttle() +{ + _fast_throttle.min_id = MOTOR_COUNT_MAX; + _fast_throttle.max_id = 0; + for (uint8_t i = 0; i < MOTOR_COUNT_MAX; i++) { + if (_found_escs[i].active) { + if (i < _fast_throttle.min_id) { + _fast_throttle.min_id = i; + } + if (i > _fast_throttle.max_id) { + _fast_throttle.max_id = i; + } + } + } + + _fast_throttle.byte_count = 1; + int16_t bit_count = 12 + (_found_escs_count * 11); + _fast_throttle.bits_to_add_left = bit_count - 16; + while (bit_count > 0) { + _fast_throttle.byte_count++; + bit_count -= 8; + } + _fast_throttle.command[0] = uint8_t(msg_type::SET_FAST_COM_LENGTH); + _fast_throttle.command[1] = _fast_throttle.byte_count; // just for older ESC FW versions since 1.0 001 this byte is ignored as the ESC calculates it itself + _fast_throttle.command[2] = _fast_throttle.min_id+1; // one-indexed min ESC id + _fast_throttle.command[3] = _found_escs_count; // count of ESCs that will get signals +} + +#if HAL_WITH_ESC_TELEM +/** + increment message packet count for every ESC +*/ +void AP_FETtecOneWire::inc_sent_msg_count() +{ + _sent_msg_count++; + if (_sent_msg_count > 4 * _update_rate_hz) { // resets every four seconds + _sent_msg_count = 0; + for (int i=0; i<_found_escs_count; i++) { + _found_escs[i].error_count_since_overflow = _found_escs[i].error_count; //save the current ESC error state + } + } +} + +/** + 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 current_error_count the error count given by the esc + @return the error in percent +*/ +float AP_FETtecOneWire::calc_tx_crc_error_perc(const uint8_t esc_id, uint16_t current_error_count) +{ + _found_escs[esc_id].error_count = current_error_count; //Save the error count to the esc + uint16_t corrected_error_count = (uint16_t)((uint16_t)_found_escs[esc_id].error_count - (uint16_t)_found_escs[esc_id].error_count_since_overflow); //calculates error difference since last overflow. + return (float)corrected_error_count*_crc_error_rate_factor; //calculates percentage +} + +/** + 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 +*/ +AP_FETtecOneWire::receive_response AP_FETtecOneWire::decode_single_esc_telemetry(TelemetryData& t, int16_t& centi_erpm, uint16_t& tx_err_count, uint8_t &tlm_from_id) +{ + receive_response ret = receive_response::NO_ANSWER_YET; + static constexpr uint8_t TELEM_LENGTH = 11; + static_assert(MAX_RECEIVE_LENGTH >= TELEM_LENGTH, "MAX_RECEIVE_LENGTH is too small"); + + if (_found_escs_count > 0) { + uint8_t telem[FRAME_OVERHEAD + TELEM_LENGTH]; + ret = receive((uint8_t *) telem, TELEM_LENGTH, return_type::FULL_FRAME); + + if (ret == receive_response::ANSWER_VALID) { + if (telem[1] <= _fast_throttle.min_id || telem[1] > _fast_throttle.max_id+1) { + return receive_response::NO_ANSWER_YET; // this data came from an unexpected ESC + } + tlm_from_id = (uint8_t)telem[1]-1; // convert external ESC's one-indexed IDs to Ardupilot's internal zero-indexed IDs + + t.temperature_cdeg = int16_t(telem[5+0] * 100); + t.voltage = float((telem[5+1]<<8)|telem[5+2]) * 0.01f; + t.current = float((telem[5+3]<<8)|telem[5+4]) * 0.01f; + centi_erpm = (telem[5+5]<<8)|telem[5+6]; + t.consumption_mah = float((telem[5+7]<<8)|telem[5+8]); + tx_err_count = (telem[5+9]<<8)|telem[5+10]; + } + } + return ret; +} +#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 AP_FETtecOneWire::escs_set_values(const uint16_t* motor_values, const int8_t tlm_request) +{ + if (_found_escs_count > 0) { + // 8 bits - OneWire Header + // 4 bits - telemetry request + // 11 bits - throttle value per ESC + // 8 bits - frame CRC + // 7 dummy for rounding up the division by 8 + uint8_t fast_throttle_command[(8+4+(11*MOTOR_COUNT_MAX)+8+7)/8] { 0 }; + uint8_t act_throttle_command = 0; + + // 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 + static_assert(MOTOR_COUNT_MAX<=15, "OneWire supports at most 15 ESCs, because of the 4 bit limitation bellow"); + fast_throttle_command[0] = (tlm_request+1) << 4; // convert from zero indexed to one-indexed. -1 (AP no telemetry) gets correctly converted to 0 (ESC no telemetry) + fast_throttle_command[0] |= ((motor_values[act_throttle_command] >> 10) & 0x01) << 3; + fast_throttle_command[0] |= 0x01; + + // byte 2: + // AAABBBBB + // A = next 3 bits from (11bit) throttle value + // B = 5bit target ID + fast_throttle_command[1] = (((motor_values[act_throttle_command] >> 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 values, followed by the CRC byte + uint8_t bits_left_from_command = 7; + uint8_t act_byte = 2; + uint8_t bits_from_byte_left = 8; + int16_t bits_to_add_left = _fast_throttle.bits_to_add_left; // must be signed + while (bits_to_add_left > 0) { + if (bits_from_byte_left >= bits_left_from_command) { + fast_throttle_command[act_byte] |= + (motor_values[act_throttle_command] & ((1 << bits_left_from_command) - 1)) + << (bits_from_byte_left - bits_left_from_command); + bits_to_add_left -= bits_left_from_command; + bits_from_byte_left -= bits_left_from_command; + act_throttle_command++; + bits_left_from_command = 11; + if (bits_to_add_left == 0) { + act_byte++; + bits_from_byte_left = 8; + } + } else { + fast_throttle_command[act_byte] |= + (motor_values[act_throttle_command] >> (bits_left_from_command - bits_from_byte_left)) + & ((1 << bits_from_byte_left) - 1); + bits_to_add_left -= bits_from_byte_left; + bits_left_from_command -= bits_from_byte_left; + act_byte++; + bits_from_byte_left = 8; + if (bits_left_from_command == 0) { + act_throttle_command++; + bits_left_from_command = 11; + } + } } - if (FETtecOneWire_ScanActive < 25) { - // scan for all ESC's in onewire bus - FETtecOneWire_ScanActive = FETtecOneWire_ScanESCs(); - } else if (FETtecOneWire_SetupActive < 25) { - if (FETtecOneWire_FoundESCs == 0) { - FETtecOneWire_ScanActive = 0; - } else { - // check if in bootloader, start ESC's FW if they are and prepare fast throttle command - FETtecOneWire_SetupActive = FETtecOneWire_InitESCs(); + fast_throttle_command[_fast_throttle.byte_count - 1] = + crc8_dvb_update(0, fast_throttle_command, _fast_throttle.byte_count - 1); + +#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 + if (_use_hdplex) { + _ignore_own_bytes = _fast_throttle.byte_count - 1; + } + + _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 (_uart->txspace() > _fast_throttle.byte_count) { + _uart->write(fast_throttle_command, _fast_throttle.byte_count); + } + } +} + +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 (__builtin_popcount(_motor_mask_parameter.get()) != _nr_escs_in_bitmask) { + hal.util->snprintf(failure_msg, failure_msg_len, "Invalid motor mask; need consecutive bits only"); + return false; + } + + const bool all_escs_contiguous = _fast_throttle.max_id - _fast_throttle.min_id < _found_escs_count; + if (!all_escs_contiguous){ + hal.util->snprintf(failure_msg, failure_msg_len, "gap in IDs found"); + return false; + } + + const bool all_escs_found = _found_escs_count >= _nr_escs_in_bitmask; + if (!all_escs_found) { + hal.util->snprintf(failure_msg, failure_msg_len, "found only %u of %u ESCs", _found_escs_count, _nr_escs_in_bitmask); + return false; + } + + const bool all_escs_configured = _found_escs_count == _configured_escs; + if (!all_escs_configured) { + hal.util->snprintf(failure_msg, failure_msg_len, "configured only %u of %u ESCs", _configured_escs, _found_escs_count); + return false; + } + +#if HAL_WITH_ESC_TELEM + const uint16_t active_esc_mask = AP::esc_telem().get_active_esc_mask(); + const uint8_t num_active_escs = __builtin_popcount(active_esc_mask & _motor_mask); + + bool telem_rx_missing = (num_active_escs < _nr_escs_in_bitmask) && (_sent_msg_count > 2 * MOTOR_COUNT_MAX); + if (telem_rx_missing) { + hal.util->snprintf(failure_msg, failure_msg_len, "got TLM from only %u of %u ESCs", num_active_escs, _nr_escs_in_bitmask); + return false; + } +#endif + + return true; +} + +/// periodically called from SRV_Channels::push() +void AP_FETtecOneWire::update() +{ + if (!_initialised) { + init(); + return; // the rest of this function can only run after fully initted + } + + const uint32_t now = AP_HAL::micros(); + if (now - _scan.last_us < 700U) { + // the update() call period must be bigger than 700 us, + // as to have time to receive the telemetry data + return; + } + _scan.last_us = now; + + // get ESC set points + uint16_t motor_pwm[MOTOR_COUNT_MAX] {}; + for (uint8_t i = 0; i < _nr_escs_in_bitmask; i++) { + const SRV_Channel* c = SRV_Channels::srv_channel(i); + if (c == nullptr) { // this should never ever happen, but just in case ... + motor_pwm[i] = 1000; // stop motor + continue; + } + // check if safety switch has been pushed + if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { + motor_pwm[i] = 1000; // stop motor + } else { + motor_pwm[i] = constrain_int16(c->get_output_pwm(), 1000, 2000); + } + if (_reverse_mask & (1U << i)) { + motor_pwm[i] = 2000-motor_pwm[i]; + } + } + +#if HAL_WITH_ESC_TELEM + // receive and decode the telemetry data from one ESC + // but do not process it any further to reduce timing jitter in the escs_set_values() function call + TelemetryData t {}; + int16_t centi_erpm = 0; // initialize to prevent false positive error: ‘centi_erpm’ may be used uninitialized in this function + uint16_t tx_err_count = 0; // initialize to prevent false positive error: ‘tx_err_count’ may be used uninitialized in this function + receive_response tlm_ok = receive_response::NO_ANSWER_YET; + uint8_t tlm_from_id = 0; + if (_requested_telemetry_from_esc != -1) { + tlm_ok = decode_single_esc_telemetry(t, centi_erpm, tx_err_count, tlm_from_id); + if (_nr_escs_in_bitmask) { + _requested_telemetry_from_esc++; + if (_requested_telemetry_from_esc > _fast_throttle.max_id) { + _requested_telemetry_from_esc = _fast_throttle.min_id; // restart from the first ESC } } } else { - //send fast throttle signals - if (FETtecOneWire_IDcount > 0) { + _requested_telemetry_from_esc = _fast_throttle.min_id; // start from the first ESC + } +#endif - // check for telemetry - return_TLM_request = FETtecOneWire_CheckForTLM(Telemetry); - FETtecOneWire_TLM_request = tlmRequest; + if (_nr_escs_in_bitmask) { + // send motor setpoints to ESCs, and request for telemetry data + escs_set_values(motor_pwm, _requested_telemetry_from_esc); - //prepare fast throttle signals - uint16_t useSignals[24] = {0}; - uint8_t OneWireFastThrottleCommand[36] = {0}; - if (motorCount > FETtecOneWire_IDcount) { - motorCount = FETtecOneWire_IDcount; +#if HAL_WITH_ESC_TELEM + // now that escs_set_values() has been executed we can fully process the telemetry data from the ESC + + inc_sent_msg_count(); // increment message packet count for every ESC + + if (_requested_telemetry_from_esc != -1 && tlm_ok == receive_response::ANSWER_VALID) { //only use telemetry if it is ok. + if (_pole_count_parameter < 2) { // if user set parameter is invalid use 14 Poles + _pole_count_parameter = 14; } - for (uint8_t i = 0; i < motorCount; i++) { - useSignals[i] = constrain_int16(motorValues[i], 0, 2000); + const float tx_err_rate = calc_tx_crc_error_perc(tlm_from_id, tx_err_count); + update_rpm(tlm_from_id-_fast_throttle.min_id, centi_erpm*100*2/_pole_count_parameter.get(), tx_err_rate); + + update_telem_data(tlm_from_id-_fast_throttle.min_id, t, TelemetryType::TEMPERATURE|TelemetryType::VOLTAGE|TelemetryType::CURRENT|TelemetryType::CONSUMPTION); + } +#endif + } + + // Now that all real-time tasks above have been done, do some periodic checks. + configuration_check(); +} + +#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) +{ + if (_found_escs_count > 0) { + const uint8_t request[2] = {uint8_t(msg_type::BEEP), beep_frequency}; + const uint8_t spacer[2] = {0, 0}; + for (uint8_t i = _fast_throttle.min_id; i <= _fast_throttle.max_id; i++) { + transmit(i, request, sizeof(request)); + // add two zeros to make sure all ESCs can catch their command as we don't wait for a response here + _uart->write(spacer, sizeof(spacer)); +#if HAL_AP_FETTEC_HALF_DUPLEX + if (_use_hdplex) { + _ignore_own_bytes += 2; } - - uint8_t actThrottleCommand = 0; - - // byte 1: - // bit 0 = TLMrequest, bit 1,2,3 = TLM type, bit 4 = first bit of first ESC (11bit)signal, bit 5,6,7 = frame header - // so ABBBCDDD - // A = TLM request yes or no - // B = TLM request type (temp, volt, current, erpm, consumption, debug1, debug2, debug3) - // C = first bit from first throttle signal - // D = frame header - OneWireFastThrottleCommand[0] = 128 | (FETtecOneWire_TLM_request << 4); - OneWireFastThrottleCommand[0] |= ((useSignals[actThrottleCommand] >> 10) & 0x01) << 3; - OneWireFastThrottleCommand[0] |= 0x01; - - // byte 2: - // AAABBBBB - // A = next 3 bits from (11bit)throttle signal - // B = 5bit target ID - OneWireFastThrottleCommand[1] = (((useSignals[actThrottleCommand] >> 7) & 0x07)) << 5; - OneWireFastThrottleCommand[1] |= ALL_ID; - - // following bytes are the rest 7 bit of the first (11bit)throttle signal, and all bit from all other signals, followed by the CRC byte - uint8_t BitsLeftFromCommand = 7; - uint8_t actByte = 2; - uint8_t bitsFromByteLeft = 8; - uint8_t bitsToAddLeft = (12 + (((FETtecOneWire_maxID - FETtecOneWire_minID) + 1) * 11)) - 16; - while (bitsToAddLeft > 0) { - if (bitsFromByteLeft >= BitsLeftFromCommand) { - OneWireFastThrottleCommand[actByte] |= - (useSignals[actThrottleCommand] & ((1 << BitsLeftFromCommand) - 1)) - << (bitsFromByteLeft - BitsLeftFromCommand); - bitsToAddLeft -= BitsLeftFromCommand; - bitsFromByteLeft -= BitsLeftFromCommand; - actThrottleCommand++; - BitsLeftFromCommand = 11; - if (bitsToAddLeft == 0) { - actByte++; - bitsFromByteLeft = 8; - } - } else { - OneWireFastThrottleCommand[actByte] |= - (useSignals[actThrottleCommand] >> (BitsLeftFromCommand - bitsFromByteLeft)) - & ((1 << bitsFromByteLeft) - 1); - bitsToAddLeft -= bitsFromByteLeft; - BitsLeftFromCommand -= bitsFromByteLeft; - actByte++; - bitsFromByteLeft = 8; - if (BitsLeftFromCommand == 0) { - actThrottleCommand++; - BitsLeftFromCommand = 11; - } - } - } - // empty buffer - while (_uart->available()) { - _uart->read(); - } - - // send throttle signal - OneWireFastThrottleCommand[FETtecOneWire_FastThrottleByteCount - 1] = FETtecOneWire_GetCrc8( - OneWireFastThrottleCommand, FETtecOneWire_FastThrottleByteCount - 1); - _uart->write(OneWireFastThrottleCommand, FETtecOneWire_FastThrottleByteCount); - // last byte of signal can be used to make sure the first TLM byte is correct, in case of spike corruption - FETtecOneWire_IgnoreOwnBytes = FETtecOneWire_FastThrottleByteCount - 1; - FETtecOneWire_lastCRC = OneWireFastThrottleCommand[FETtecOneWire_FastThrottleByteCount - 1]; - // the ESC's will answer the TLM as 16bit each ESC, so 2byte each ESC. +#endif } } - return return_TLM_request; // returns the readed tlm as it is 1 loop delayed } +#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 AP_FETtecOneWire::led_color(const uint8_t r, const uint8_t g, const uint8_t b) +{ + if (_found_escs_count > 0) { + const uint8_t request[4] = {uint8_t(msg_type::SET_LED_TMP_COLOR), r, g, b}; + const uint8_t spacer[2] = {0, 0}; + for (uint8_t i = _fast_throttle.min_id; i <= _fast_throttle.max_id; i++) { + transmit(i, request, sizeof(request)); + // add two zeros to make sure all ESCs can catch their command as we don't wait for a response here + _uart->write(spacer, sizeof(spacer)); +#if HAL_AP_FETTEC_HALF_DUPLEX + if (_use_hdplex) { + _ignore_own_bytes += 2; + } +#endif + } + } +} +#endif + +#endif // HAL_AP_FETTEC_ONEWIRE_ENABLED diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h index 6985515998..4763674694 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h @@ -14,243 +14,310 @@ */ /* Initial protocol implementation was provided by FETtec */ +/* Strongly modified by Amilcar Lucas, IAV GmbH */ #pragma once #include -#ifndef HAL_AP_FETTECONEWIRE_ENABLED -#define HAL_AP_FETTECONEWIRE_ENABLED !HAL_MINIMIZE_FEATURES +#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 -#if HAL_AP_FETTECONEWIRE_ENABLED +// 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 + +#include +#include + + + +class AP_FETtecOneWire : public AP_ESC_Telem_Backend +{ -class AP_FETtecOneWire { 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[]; + + 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; + } + +#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: - void init(); - bool initialised; + static AP_FETtecOneWire *_singleton; AP_HAL::UARTDriver *_uart; - uint32_t last_send_us; - static constexpr uint32_t DELAY_TIME_US = 700; - static constexpr uint8_t DETECT_ESC_COUNT = 4; // TODO needed ? - static constexpr uint8_t MOTOR_COUNT = 4; - uint16_t completeTelemetry[MOTOR_COUNT][5] = {0}; - uint16_t motorpwm[MOTOR_COUNT] = {1000}; - uint8_t TLM_request = 0; - /* - initialize FETtecOneWire protocol -*/ - void FETtecOneWire_Init(void); -/* - deinitialize FETtecOneWire protocol -*/ - void FETtecOneWire_DeInit(void); +#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 -/* - generates used 8 bit CRC - crc = byte to be added to CRC - crc_seed = CRC where it gets added too - returns 8 bit CRC -*/ - uint8_t FETtecOneWire_Update_crc8(uint8_t crc, uint8_t crc_seed); + static constexpr uint8_t FRAME_OVERHEAD = 6; + static constexpr uint8_t MAX_TRANSMIT_LENGTH = 4; + static constexpr uint8_t MAX_RECEIVE_LENGTH = 12; -/* - generates used 8 bit CRC for arrays - Buf = 8 bit byte array - BufLen = count of bytes that should be used for CRC calculation - returns 8 bit CRC -*/ - uint8_t FETtecOneWire_Get_crc8(uint8_t *Buf, uint16_t BufLen); + /** + initialize the serial port, scan the OneWire bus, setup the found ESCs + */ + void init(); -/* - transmitts a FETtecOneWire frame to a ESC - ESC_id = id of the ESC - Bytes = 8 bit array of bytes. Where byte 1 contains the command, and all following bytes can be the payload - Length = length of the Bytes array - returns nothing -*/ - void FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t *Bytes, uint8_t Length); + /** + check if the current configuration is OK + */ + void configuration_check(); -/* - reads the answer frame of a ESC - Bytes = 8 bit byte array, where the received answer gets stored in - Length = the expected answer length - returnFullFrame can be OW_RETURN_RESPONSE or OW_RETURN_FULL_FRAME - returns 1 if the expected answer frame was there, 0 if dont -*/ - uint8_t FETtecOneWire_Receive(uint8_t *Bytes, uint8_t Length, uint8_t returnFullFrame); + /** + 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 + */ + bool transmit(const uint8_t esc_id, const uint8_t *bytes, uint8_t length); -/* - makes all connected ESC's beep - beepFreqency = a 8 bit value from 0-255. higher make a higher beep -*/ - void FETtecOneWire_Beep(uint8_t beepFreqency); - -/* - sets the racewire color for all ESC's - R, G, B = 8bit colors -*/ - void FETtecOneWire_RW_LEDcolor(uint8_t R, uint8_t G, uint8_t B); - -/* - Resets a pending pull request - returns nothing -*/ - void FETtecOneWire_PullReset(void); - -/* - Pulls a complete request between for ESC - ESC_id = id of the ESC - command = 8bit array containing the command that thould be send including the possible payload - response = 8bit array where the response will be stored in - returnFullFrame can be OW_RETURN_RESPONSE or OW_RETURN_FULL_FRAME - returns 1 if the request is completed, 0 if dont -*/ - uint8_t FETtecOneWire_PullCommand(uint8_t ESC_id, uint8_t *command, uint8_t *response, uint8_t returnFullFrame); - -/* - scans for ESC's in bus. should be called intill FETtecOneWire_ScanActive >= 25 - returns the current scanned ID -*/ - uint8_t FETtecOneWire_ScanESCs(void); - -/* - starts all ESC's in bus and prepares them for receiving teh fast throttle command should be called untill FETtecOneWire_SetupActive >= 25 - returns the current used ID -*/ - uint8_t FETtecOneWire_InitESCs(void); - -/* - checks if the requested telemetry is available. - Telemetry = 16bit array where the read Telemetry will be stored in. - returns the telemetry request number or -1 if unavailable -*/ - int8_t FETtecOneWire_CheckForTLM(uint16_t *Telemetry); - -/* - does almost all of the job. - scans for ESC's if not already done. - initializes the ESC's if not already done. - sends fast throttle signals if init is complete. - motorValues = a 16bit array containing the throttle signals that should be sent to the motors. 0-2000 where 1001-2000 is positive rotation and 999-0 reversed rotation - Telemetry = 16bit array where the read telemetry will be stored in. - motorCount = the count of motors that should get values send - tlmRequest = the requested telemetry type (OW_TLM_XXXXX) - returns the telemetry request if telemetry was available, -1 if dont -*/ - int8_t FETtecOneWire_ESCsSetValues(uint16_t *motorValues, uint16_t *Telemetry, uint8_t motorCount, uint8_t tlmRequest); - - uint8_t FETtecOneWire_UpdateCrc8(uint8_t crc, uint8_t crc_seed); //TODO remove - uint8_t FETtecOneWire_GetCrc8(uint8_t* Buf, uint16_t BufLen); - static constexpr uint8_t ALL_ID = 0x1F; - typedef struct FETtecOneWireESC + enum class return_type : uint8_t { - uint8_t inBootLoader; - uint8_t firmWareVersion; - uint8_t firmWareSubVersion; - uint8_t ESCtype; - uint8_t serialNumber[12]; - } FETtecOneWireESC_t; - - uint8_t FETtecOneWire_activeESC_IDs[25] = {0}; - FETtecOneWireESC_t FETtecOneWire_foundESCs[25]; - uint8_t FETtecOneWire_FoundESCs = 0; - uint8_t FETtecOneWire_ScanActive = 0; - uint8_t FETtecOneWire_SetupActive = 0; - uint8_t FETtecOneWire_IgnoreOwnBytes = 0; - int8_t FETtecOneWire_minID = 25; - int8_t FETtecOneWire_maxID = 0; - uint8_t FETtecOneWire_IDcount = 0; - uint8_t FETtecOneWire_FastThrottleByteCount = 0; - uint8_t FETtecOneWire_PullSuccess = 0; - uint8_t FETtecOneWire_PullBusy = 0; - uint8_t FETtecOneWire_TLM_request = 0; - uint8_t FETtecOneWire_lastCRC = 0; - uint8_t FETtecOneWire_firstInitDone = 0; - - - enum - { - OW_RETURN_RESPONSE, - OW_RETURN_FULL_FRAME + RESPONSE, + FULL_FRAME }; - enum + enum class receive_response : uint8_t { - OW_OK, - OW_BL_PAGE_CORRECT, // BL only - OW_NOT_OK, - OW_BL_START_FW, // BL only - OW_BL_PAGES_TO_FLASH, // BL only - OW_REQ_TYPE, - OW_REQ_SN, - OW_REQ_SW_VER + NO_ANSWER_YET, + ANSWER_VALID, + CRC_MISSMATCH, + REQ_OVERLENGTH }; - enum - { - OW_RESET_TO_BL = 10, - OW_THROTTLE = 11, - OW_REQ_TLM = 12, - OW_BEEP = 13, - - OW_SET_FAST_COM_LENGTH = 26, - - OW_GET_ROTATION_DIRECTION = 28, - OW_SET_ROTATION_DIRECTION = 29, - - OW_GET_USE_SIN_START = 30, - OW_SET_USE_SIN_START = 31, - - OW_GET_3D_MODE = 32, - OW_SET_3D_MODE = 33, - - OW_GET_ID = 34, - OW_SET_ID = 35, - -/* - OW_GET_LINEAR_THRUST = 36, - OW_SET_LINEAR_THRUST = 37, -*/ - - OW_GET_EEVER = 38, - - OW_GET_PWM_MIN = 39, - OW_SET_PWM_MIN = 40, - - OW_GET_PWM_MAX = 41, - OW_SET_PWM_MAX = 42, - - OW_GET_ESC_BEEP = 43, - OW_SET_ESC_BEEP = 44, - - OW_GET_CURRENT_CALIB = 45, - OW_SET_CURRENT_CALIB = 46, - - OW_SET_LED_TMP_COLOR = 51, - OW_GET_LED_COLOR = 52, - OW_SET_LED_COLOR = 53 + /** + 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); + enum class pull_state : uint8_t { + BUSY, + COMPLETED, + FAILED }; - enum + /** + 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 { - OW_TLM_TEMP, - OW_TLM_VOLT, - OW_TLM_CURRENT, - OW_TLM_ERPM, - OW_TLM_CONSUMPTION, - OW_TLM_DEBUG1, - OW_TLM_DEBUG2, - OW_TLM_DEBUG3 + 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 + + 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; +#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, ...) +#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 _initialised; ///< device driver and ESCs are fully initialized + bool _pull_busy; ///< request-reply transaction is busy + + enum class msg_type : 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 + 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 +#endif }; - static uint8_t FETtecOneWire_ResponseLength[54]; - static uint8_t FETtecOneWire_RequestLength[54]; + + 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 +#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 + }; + + /// 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; + + /// 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; + + /// 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)]; + }; -#endif // HAL_AP_FETTECONEWIRE_ENABLED - +#endif // HAL_AP_FETTEC_ONEWIRE_ENABLED diff --git a/libraries/AP_FETtecOneWire/README.md b/libraries/AP_FETtecOneWire/README.md new file mode 100644 index 0000000000..c332729404 --- /dev/null +++ b/libraries/AP_FETtecOneWire/README.md @@ -0,0 +1,183 @@ +# 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 +- 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. +- 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 + +## 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. + +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 message +Consists of six frame bytes + payload bytes. + +``` + Byte 0 is the transfer direction (e.g. 0x01 Master to Slave) + 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 + +``` + 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 `FETtecOneWire.cpp` for details. + + +### 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. + +**Note:** You need at least a 4Hz motor signal (max 250ms between messages) before the motors disarm. + +## 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 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 +``` + + + + +## 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 `_found_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.