From 634673946667c83239668804230ab9fa24dd490b Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Tue, 6 Jul 2021 21:26:00 +0200 Subject: [PATCH] AP_FETtecOneWire: Conversion to C++ and initial ArduPilot support --- .../AP_FETtecOneWire/AP_FETtecOneWire.cpp | 1272 +++++++++-------- libraries/AP_FETtecOneWire/AP_FETtecOneWire.h | 425 +++--- 2 files changed, 891 insertions(+), 806 deletions(-) diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp index dfac541518..235c34777f 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.cpp @@ -14,666 +14,748 @@ */ /* Initial protocol implementation was provided by FETtec */ +#include +#include +#include "AP_FETtecOneWire.h" +#include -#include "FETtecOneWire.h" +// 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; -typedef struct FETtecOneWireESC { - uint8_t inBootLoader; - uint8_t firmWareVersion; - uint8_t firmWareSubVersion; - uint8_t ESCtype; - uint8_t serialNumber[12]; -} FETtecOneWireESC_t; + FETtecOneWire_ResponseLength[OW_SET_FAST_COM_LENGTH] = 1; -static uint8_t FETtecOneWire_activeESC_IDs[25] = {0}; -static FETtecOneWireESC_t FETtecOneWire_foundESCs[25]; -static uint8_t FETtecOneWire_FoundESCs = 0; -static uint8_t FETtecOneWire_ScanActive = 0; -static uint8_t FETtecOneWire_SetupActive = 0; -static uint8_t FETtecOneWire_IgnoreOwnBytes = 0; -static int8_t FETtecOneWire_minID = 25; -static int8_t FETtecOneWire_maxID = 0; -static uint8_t FETtecOneWire_IDcount = 0; -static uint8_t FETtecOneWire_FastThrottleByteCount = 0; -static uint8_t FETtecOneWire_PullSuccess = 0; -static uint8_t FETtecOneWire_PullBusy = 0; -static uint8_t FETtecOneWire_TLM_request = 0; -static uint8_t FETtecOneWire_lastCRC = 0; -static uint8_t FETtecOneWire_firstInitDone = 0; + FETtecOneWire_ResponseLength[OW_GET_ROTATION_DIRECTION] = 1; + FETtecOneWire_ResponseLength[OW_SET_ROTATION_DIRECTION] = 1; -uint8_t FETtecOneWire_ResponseLength[54] = { - [OW_OK] = 1, - [OW_BL_PAGE_CORRECT] = 1, // BL only - [OW_NOT_OK] = 1, - [OW_BL_START_FW] = 0, // BL only - [OW_BL_PAGES_TO_FLASH] = 1, // BL only - [OW_REQ_TYPE] = 1, - [OW_REQ_SN] = 12, - [OW_REQ_SW_VER] = 2, - [OW_RESET_TO_BL] = 0, - [OW_THROTTLE] = 1, - [OW_REQ_TLM] = 2, - [OW_BEEP] = 0, + FETtecOneWire_ResponseLength[OW_GET_USE_SIN_START] = 1; + FETtecOneWire_ResponseLength[OW_SET_USE_SIN_START] = 1; - [OW_SET_FAST_COM_LENGTH] = 1, - - [OW_GET_ROTATION_DIRECTION] = 1, - [OW_SET_ROTATION_DIRECTION] = 1, - - [OW_GET_USE_SIN_START] = 1, - [OW_SET_USE_SIN_START] = 1, - - [OW_GET_3D_MODE] = 1, - [OW_SET_3D_MODE] = 1, - - [OW_GET_ID] = 1, - [OW_SET_ID] = 1, - -/* - [OW_GET_LINEAR_THRUST] = 1, - [OW_SET_LINEAR_THRUST] = 1, -*/ - - [OW_GET_EEVER] = 1, - - [OW_GET_PWM_MIN] = 2, - [OW_SET_PWM_MIN] = 1, - - [OW_GET_PWM_MAX] = 2, - [OW_SET_PWM_MAX] = 1, - - [OW_GET_ESC_BEEP] = 1, - [OW_SET_ESC_BEEP] = 1, - - [OW_GET_CURRENT_CALIB] = 1, - [OW_SET_CURRENT_CALIB] = 1, - - [OW_SET_LED_TMP_COLOR] = 0, - [OW_GET_LED_COLOR] = 5, - [OW_SET_LED_COLOR] = 1 - -}; -uint8_t FETtecOneWire_RequestLength[54] = { - [OW_OK] = 1, - [OW_BL_PAGE_CORRECT] = 1, // BL only - [OW_NOT_OK] = 1, - [OW_BL_START_FW] = 1, // BL only - [OW_BL_PAGES_TO_FLASH] = 1, // BL only - [OW_REQ_TYPE] = 1, - [OW_REQ_SN] = 1, - [OW_REQ_SW_VER] = 1, - [OW_RESET_TO_BL] = 1, - [OW_THROTTLE] = 1, - [OW_REQ_TLM] = 1, - [OW_BEEP] = 2, - - [OW_SET_FAST_COM_LENGTH] = 4, - - [OW_GET_ROTATION_DIRECTION] = 1, - [OW_SET_ROTATION_DIRECTION] = 1, - - [OW_GET_USE_SIN_START] = 1, - [OW_SET_USE_SIN_START] = 1, - - [OW_GET_3D_MODE] = 1, - [OW_SET_3D_MODE] = 1, - - [OW_GET_ID] = 1, - [OW_SET_ID] = 1, - -/* - [OW_GET_LINEAR_THRUST] = 1, - [OW_SET_LINEAR_THRUST] = 1, -*/ - - [OW_GET_EEVER] = 1, - - [OW_GET_PWM_MIN] = 1, - [OW_SET_PWM_MIN] = 2, - - [OW_GET_PWM_MAX] = 1, - [OW_SET_PWM_MAX] = 2, - - [OW_GET_ESC_BEEP] = 1, - [OW_SET_ESC_BEEP] = 1, - - [OW_GET_CURRENT_CALIB] = 1, - [OW_SET_CURRENT_CALIB] = 1, - - [OW_SET_LED_TMP_COLOR] = 4, - [OW_GET_LED_COLOR] = 1, - [OW_SET_LED_COLOR] = 5 -}; + 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; /* - initialize FETtecOneWire protocol + FETtecOneWire_ResponseLength[OW_GET_LINEAR_THRUST] = 1; + FETtecOneWire_ResponseLength[OW_SET_LINEAR_THRUST] = 1; */ -void FETtecOneWire_Init(){ - 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; - } - FETtecOneWire_IgnoreOwnBytes = 0; - FETtecOneWire_PullSuccess = 0; - FETtecOneWire_PullBusy = 0; - FETOW_UART_DEINIT; - FETOW_UART_INIT; - FETtecOneWire_firstInitDone = 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; + +} + +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) { + 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"); + } + } } /* - deinitialize FETtecOneWire protocol + initialize FETtecOneWire protocol */ -void FETtecOneWire_DeInit(){ - FETOW_UART_DEINIT; - +void AP_FETtecOneWire::FETtecOneWire_Init() +{ + 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; + } + } + 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 + 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_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); +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 + 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_GetCrc8(uint8_t *Buf, uint16_t BufLen){ - uint8_t crc = 0; - for(uint16_t i=0; iwrite(transmitArr, Length + 6); + FETtecOneWire_IgnoreOwnBytes += Length + 6; } /* - 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 + 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){ - /* - a frame lookes like: - byte 1 = fremae header (0x02 = bootloader, 0x03 = ESC firmware) - byte 2 = sender ID (5bit) - byte 3 & 4 = frame type (always 0x00, 0x00 used for bootloader. here just for compatibility) - byte 5 = frame length over all bytes - byte 6 - X = answer type, followed by the payload - byte X+1 = 8bit CRC - */ - - //ignore own bytes - while(FETtecOneWire_IgnoreOwnBytes > 0 && FETOW_UART_BYTES_AVAILABLE){ - FETtecOneWire_IgnoreOwnBytes--; - FETOW_UART_READ_BYTE; - } - // look for the real answer - if(FETOW_UART_BYTES_AVAILABLE >= Length+6){ - // sync to frame starte byte - uint8_t testFrameStart = 0; - do{ - testFrameStart = FETOW_UART_READ_BYTE; - }while(testFrameStart != 0x02 && testFrameStart != 0x03 && FETOW_UART_BYTES_AVAILABLE); - // copy message - if(FETOW_UART_BYTES_AVAILABLE >= Length+5){ - uint8_t ReceiveBuf[20] = {0}; - ReceiveBuf[0] = testFrameStart; - for(uint8_t i=1; i 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 + 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){ - if(FETtecOneWire_IDcount > 0){ - uint8_t request[2] = {OW_BEEP, beepFreqency}; - uint8_t spacer[2] = {0,0}; - for(uint8_t i=FETtecOneWire_minID; i 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 + 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){ - 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 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 + Resets a pending pull request + returns nothing */ -void FETtecOneWire_PullReset(){ - FETtecOneWire_PullSuccess = 0; - FETtecOneWire_PullBusy = 0; +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 + 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){ - 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 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; +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; } /* - 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 + scans for ESC's in bus. should be called intill FETtecOneWire_ScanActive >= 25 + returns the currend scanned ID */ -uint8_t 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; - +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; } /* - 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 + 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 */ -int8_t FETtecOneWire_CheckForTLM(uint16_t *Telemetry){ - int8_t return_TLM_request = 0; - if(FETtecOneWire_IDcount > 0){ - // empty buffer - while(FETtecOneWire_IgnoreOwnBytes > 0 && FETOW_UART_BYTES_AVAILABLE){ - FETOW_UART_READ_BYTE; - FETtecOneWire_IgnoreOwnBytes--; - } - - // first two byte are the ESC Telemetry of the first ESC. next two byte of the second.... - if(FETOW_UART_BYTES_AVAILABLE == (FETtecOneWire_IDcount*2)+1){ - // look if first byte in buffer is equal to last byte of throttle command (crc) - if(FETOW_UART_READ_BYTE == FETtecOneWire_lastCRC){ - for(uint8_t i=0; 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; } /* - 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 + 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_ESCsSetValues(uint16_t *motorValues, uint16_t *Telemetry, uint8_t motorCount, uint8_t tlmRequest){ - static uint32_t lastTime = 0; - int8_t return_TLM_request = -2; - - // 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){ - if((uint32_t)micros() -(uint32_t)lastTime < 700) return 0; - else lastTime = micros(); - - 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(); - } - } - }else{ - //send fast throttle signals - if(FETtecOneWire_IDcount > 0){ - - // check for telemetry - return_TLM_request = FETtecOneWire_CheckForTLM(Telemetry); - FETtecOneWire_TLM_request = tlmRequest; - - //prepare fast throttle signals - uint16_t useSignals[24] = {0}; - uint8_t OneWireFastThrottleCommand[36] = {0}; - if(motorCount > FETtecOneWire_IDcount) motorCount = FETtecOneWire_IDcount; - for(uint8_t i=0; i < motorCount; i++) useSignals[i] = constrain(motorValues[i], 0, 2000); - - 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(FETOW_UART_BYTES_AVAILABLE) FETOW_UART_READ_BYTE; - - // send throttle signal - OneWireFastThrottleCommand[FETtecOneWire_FastThrottleByteCount-1] = FETtecOneWire_GetCrc8(OneWireFastThrottleCommand, FETtecOneWire_FastThrottleByteCount-1); - FETOW_UART_SEND_BYTES(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. - } - } - return return_TLM_request; // returns the readed tlm as it is 1 loop delayed +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()) { + _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; } +/* + 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 AP_FETtecOneWire::FETtecOneWire_ESCsSetValues(uint16_t* motorValues, uint16_t* Telemetry, uint8_t motorCount, + uint8_t tlmRequest) +{ + int8_t return_TLM_request = -2; + // 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; + } else { + last_send_us = now; + } + 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(); + } + } + } else { + //send fast throttle signals + if (FETtecOneWire_IDcount > 0) { + // check for telemetry + return_TLM_request = FETtecOneWire_CheckForTLM(Telemetry); + FETtecOneWire_TLM_request = tlmRequest; + //prepare fast throttle signals + uint16_t useSignals[24] = {0}; + uint8_t OneWireFastThrottleCommand[36] = {0}; + if (motorCount > FETtecOneWire_IDcount) { + motorCount = FETtecOneWire_IDcount; + } + for (uint8_t i = 0; i < motorCount; i++) { + useSignals[i] = constrain_int16(motorValues[i], 0, 2000); + } + 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. + } + } + return return_TLM_request; // returns the readed tlm as it is 1 loop delayed +} diff --git a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h index b6748377fe..6985515998 100644 --- a/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h +++ b/libraries/AP_FETtecOneWire/AP_FETtecOneWire.h @@ -15,239 +15,242 @@ /* Initial protocol implementation was provided by FETtec */ -// includes -#include "config.h" -#include "serial.h" -#ifdef STM32F3 - #include "driversF3/GEN_SERIAL_Driver_F3.h" -#endif -#ifdef STM32L4 - #include "driversL4/GEN_SERIAL_Driver_L4.h" -#endif -#ifdef STM32G4 - #include "driversG4/GEN_SERIAL_Driver_G4.h" -#endif -#ifdef STM32F7 - #include "driversF7/GEN_SERIAL_Driver_F7.h" +#pragma once + +#include + +#ifndef HAL_AP_FETTECONEWIRE_ENABLED +#define HAL_AP_FETTECONEWIRE_ENABLED !HAL_MINIMIZE_FEATURES #endif -//UART uasage +#if HAL_AP_FETTECONEWIRE_ENABLED + +class AP_FETtecOneWire { +public: + AP_FETtecOneWire(); + + void update(); +private: + void init(); + bool initialised; + 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); /* - inits the UART with 2000000 baud 8n1 half duplex + deinitialize FETtecOneWire protocol */ -#define FETOW_UART_INIT \ - GEN_SERIAL_swapRXTXpins(FETTEC_ONE_WIRE_UART_NUM, FETTEC_ONE_WIRE_UART_SWAPP_PINS); \ - GEN_SERIAL_initUART(FETTEC_ONE_WIRE_UART_NUM, FETTEC_ONE_WIRE_UART_PINSET, 2000000, 1, 0, 1, 0); + void FETtecOneWire_DeInit(void); /* - de inits the UART with 2000000 baud 8n1 half duplex + generates used 8 bit CRC + crc = byte to be added to CRC + crc_seed = CRC where it gets added too + returns 8 bit CRC */ -#define FETOW_UART_DEINIT GEN_SERIAL_DeInitUART(FETTEC_ONE_WIRE_UART_NUM); + uint8_t FETtecOneWire_Update_crc8(uint8_t crc, uint8_t crc_seed); /* - should return the available bytes in RX buffer + 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 */ -#define FETOW_UART_BYTES_AVAILABLE GEN_SERIAL_bytesAvailable(FETTEC_ONE_WIRE_UART_NUM) + uint8_t FETtecOneWire_Get_crc8(uint8_t *Buf, uint16_t BufLen); /* - should return one byte of the RX buffer + 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 */ -#define FETOW_UART_READ_BYTE GEN_SERIAL_readByte(FETTEC_ONE_WIRE_UART_NUM) + void FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t *Bytes, uint8_t Length); /* - to transmit a array of bytes. where buf will be a uint8_t byte array and len is the byte count that should be sent + 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 */ -#define FETOW_UART_SEND_BYTES(buf, len) GEN_SERIAL_sendBytes(FETTEC_ONE_WIRE_UART_NUM, buf, len) + uint8_t FETtecOneWire_Receive(uint8_t *Bytes, uint8_t Length, uint8_t returnFullFrame); /* - emtys the RX buffer + makes all connected ESC's beep + beepFreqency = a 8 bit value from 0-255. higher make a higher beep */ -#define FETOW_UART_EMPTY_RX while(FETOW_UART_BYTES_AVAILABLE) FETOW_UART_READ_BYTE; + 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); - - -#define ALL_ID 0x1F +/* + Resets a pending pull request + returns nothing +*/ + void FETtecOneWire_PullReset(void); -enum { - OW_RETURN_RESPONSE, - OW_RETURN_FULL_FRAME +/* + 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 + { + 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 + }; + + enum + { + 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 + }; + + 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 + + }; + + enum + { + OW_TLM_TEMP, + OW_TLM_VOLT, + OW_TLM_CURRENT, + OW_TLM_ERPM, + OW_TLM_CONSUMPTION, + OW_TLM_DEBUG1, + OW_TLM_DEBUG2, + OW_TLM_DEBUG3 + }; + static uint8_t FETtecOneWire_ResponseLength[54]; + static uint8_t FETtecOneWire_RequestLength[54]; }; +#endif // HAL_AP_FETTECONEWIRE_ENABLED -enum { - 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 -}; - -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 - -}; - -enum { - OW_TLM_TEMP, - OW_TLM_VOLT, - OW_TLM_CURRENT, - OW_TLM_ERPM, - OW_TLM_CONSUMPTION, - OW_TLM_DEBUG1, - OW_TLM_DEBUG2, - OW_TLM_DEBUG3 -}; - -extern uint8_t FETtecOneWire_ResponseLength[54]; -extern uint8_t FETtecOneWire_RequestLength[54]; - - -/* - initialize FETtecOneWire protocol -*/ -void FETtecOneWire_Init(void); - -/* - deinitialize FETtecOneWire protocol -*/ -void FETtecOneWire_DeInit(void); - -/* - 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); - -/* - 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); - -/* - 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); - -/* - 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); - -/* - 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); \ No newline at end of file