mirror of https://github.com/ArduPilot/ardupilot
AP_FETtecOneWire: Conversion to C++ and initial ArduPilot support
This commit is contained in:
Pierre Kancir
Peter Barker
parent
35861ab616
commit
6346739466
|
|
@ -14,143 +14,168 @@
|
|||
*/
|
||||
|
||||
/* Initial protocol implementation was provided by FETtec */
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include "AP_FETtecOneWire.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#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,
|
||||
FETtecOneWire_ResponseLength[OW_GET_3D_MODE] = 1;
|
||||
FETtecOneWire_ResponseLength[OW_SET_3D_MODE] = 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,
|
||||
FETtecOneWire_ResponseLength[OW_GET_ID] = 1;
|
||||
FETtecOneWire_ResponseLength[OW_SET_ID] = 1;
|
||||
|
||||
/*
|
||||
[OW_GET_LINEAR_THRUST] = 1,
|
||||
[OW_SET_LINEAR_THRUST] = 1,
|
||||
FETtecOneWire_ResponseLength[OW_GET_LINEAR_THRUST] = 1;
|
||||
FETtecOneWire_ResponseLength[OW_SET_LINEAR_THRUST] = 1;
|
||||
*/
|
||||
|
||||
[OW_GET_EEVER] = 1,
|
||||
FETtecOneWire_ResponseLength[OW_GET_EEVER] = 1;
|
||||
|
||||
[OW_GET_PWM_MIN] = 2,
|
||||
[OW_SET_PWM_MIN] = 1,
|
||||
FETtecOneWire_ResponseLength[OW_GET_PWM_MIN] = 2;
|
||||
FETtecOneWire_ResponseLength[OW_SET_PWM_MIN] = 1;
|
||||
|
||||
[OW_GET_PWM_MAX] = 2,
|
||||
[OW_SET_PWM_MAX] = 1,
|
||||
FETtecOneWire_ResponseLength[OW_GET_PWM_MAX] = 2;
|
||||
FETtecOneWire_ResponseLength[OW_SET_PWM_MAX] = 1;
|
||||
|
||||
[OW_GET_ESC_BEEP] = 1,
|
||||
[OW_SET_ESC_BEEP] = 1,
|
||||
FETtecOneWire_ResponseLength[OW_GET_ESC_BEEP] = 1;
|
||||
FETtecOneWire_ResponseLength[OW_SET_ESC_BEEP] = 1;
|
||||
|
||||
[OW_GET_CURRENT_CALIB] = 1,
|
||||
[OW_SET_CURRENT_CALIB] = 1,
|
||||
FETtecOneWire_ResponseLength[OW_GET_CURRENT_CALIB] = 1;
|
||||
FETtecOneWire_ResponseLength[OW_SET_CURRENT_CALIB] = 1;
|
||||
|
||||
[OW_SET_LED_TMP_COLOR] = 0,
|
||||
[OW_GET_LED_COLOR] = 5,
|
||||
[OW_SET_LED_COLOR] = 1
|
||||
FETtecOneWire_ResponseLength[OW_SET_LED_TMP_COLOR] = 0;
|
||||
FETtecOneWire_ResponseLength[OW_GET_LED_COLOR] = 5;
|
||||
FETtecOneWire_ResponseLength[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,
|
||||
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;
|
||||
|
||||
[OW_SET_FAST_COM_LENGTH] = 4,
|
||||
FETtecOneWire_RequestLength[OW_SET_FAST_COM_LENGTH] = 4;
|
||||
|
||||
[OW_GET_ROTATION_DIRECTION] = 1,
|
||||
[OW_SET_ROTATION_DIRECTION] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_ROTATION_DIRECTION] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_ROTATION_DIRECTION] = 1;
|
||||
|
||||
[OW_GET_USE_SIN_START] = 1,
|
||||
[OW_SET_USE_SIN_START] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_USE_SIN_START] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_USE_SIN_START] = 1;
|
||||
|
||||
[OW_GET_3D_MODE] = 1,
|
||||
[OW_SET_3D_MODE] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_3D_MODE] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_3D_MODE] = 1;
|
||||
|
||||
[OW_GET_ID] = 1,
|
||||
[OW_SET_ID] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_ID] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_ID] = 1;
|
||||
|
||||
/*
|
||||
[OW_GET_LINEAR_THRUST] = 1,
|
||||
[OW_SET_LINEAR_THRUST] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_LINEAR_THRUST] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_LINEAR_THRUST] = 1;
|
||||
*/
|
||||
|
||||
[OW_GET_EEVER] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_EEVER] = 1;
|
||||
|
||||
[OW_GET_PWM_MIN] = 1,
|
||||
[OW_SET_PWM_MIN] = 2,
|
||||
FETtecOneWire_RequestLength[OW_GET_PWM_MIN] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_PWM_MIN] = 2;
|
||||
|
||||
[OW_GET_PWM_MAX] = 1,
|
||||
[OW_SET_PWM_MAX] = 2,
|
||||
FETtecOneWire_RequestLength[OW_GET_PWM_MAX] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_PWM_MAX] = 2;
|
||||
|
||||
[OW_GET_ESC_BEEP] = 1,
|
||||
[OW_SET_ESC_BEEP] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_ESC_BEEP] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_ESC_BEEP] = 1;
|
||||
|
||||
[OW_GET_CURRENT_CALIB] = 1,
|
||||
[OW_SET_CURRENT_CALIB] = 1,
|
||||
FETtecOneWire_RequestLength[OW_GET_CURRENT_CALIB] = 1;
|
||||
FETtecOneWire_RequestLength[OW_SET_CURRENT_CALIB] = 1;
|
||||
|
||||
[OW_SET_LED_TMP_COLOR] = 4,
|
||||
[OW_GET_LED_COLOR] = 1,
|
||||
[OW_SET_LED_COLOR] = 5
|
||||
};
|
||||
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");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
initialize FETtecOneWire protocol
|
||||
*/
|
||||
void FETtecOneWire_Init(){
|
||||
if(FETtecOneWire_firstInitDone == 0){
|
||||
void AP_FETtecOneWire::FETtecOneWire_Init()
|
||||
{
|
||||
if (FETtecOneWire_firstInitDone == 0) {
|
||||
FETtecOneWire_FoundESCs = 0;
|
||||
FETtecOneWire_ScanActive = 0;
|
||||
FETtecOneWire_SetupActive = 0;
|
||||
|
|
@ -158,35 +183,30 @@ void FETtecOneWire_Init(){
|
|||
FETtecOneWire_maxID = 0;
|
||||
FETtecOneWire_IDcount = 0;
|
||||
FETtecOneWire_FastThrottleByteCount = 0;
|
||||
for(uint8_t i = 0; i < 25; i++) FETtecOneWire_activeESC_IDs[i] = 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;
|
||||
}
|
||||
|
||||
/*
|
||||
deinitialize FETtecOneWire protocol
|
||||
*/
|
||||
void FETtecOneWire_DeInit(){
|
||||
FETOW_UART_DEINIT;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
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 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 );
|
||||
for (i = 0; i < 8; i++) {
|
||||
crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
|
||||
}
|
||||
return (crc_u);
|
||||
}
|
||||
|
||||
|
|
@ -196,9 +216,12 @@ uint8_t FETtecOneWire_UpdateCrc8(uint8_t crc, uint8_t crc_seed){
|
|||
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 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);
|
||||
for (uint16_t i = 0; i < BufLen; i++) {
|
||||
crc = FETtecOneWire_UpdateCrc8(Buf[i], crc);
|
||||
}
|
||||
return (crc);
|
||||
}
|
||||
|
||||
|
|
@ -209,7 +232,8 @@ uint8_t FETtecOneWire_GetCrc8(uint8_t *Buf, uint16_t BufLen){
|
|||
Length = length of the Bytes array
|
||||
returns nothing
|
||||
*/
|
||||
void FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t *Bytes, uint8_t Length){
|
||||
void AP_FETtecOneWire::FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t* Bytes, uint8_t Length)
|
||||
{
|
||||
/*
|
||||
a frame lookes like:
|
||||
byte 1 = fremae header (master is always 0x01)
|
||||
|
|
@ -219,12 +243,14 @@ void FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t *Bytes, uint8_t Length){
|
|||
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];
|
||||
transmitArr[Length+5] = FETtecOneWire_GetCrc8(transmitArr, Length+5); // crc
|
||||
FETOW_UART_SEND_BYTES(transmitArr, Length+6);
|
||||
FETtecOneWire_IgnoreOwnBytes += Length+6;
|
||||
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];
|
||||
}
|
||||
transmitArr[Length + 5] = FETtecOneWire_GetCrc8(transmitArr, Length + 5); // crc
|
||||
_uart->write(transmitArr, Length + 6);
|
||||
FETtecOneWire_IgnoreOwnBytes += Length + 6;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -234,7 +260,8 @@ void FETtecOneWire_Transmit(uint8_t ESC_id, uint8_t *Bytes, uint8_t 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){
|
||||
uint8_t AP_FETtecOneWire::FETtecOneWire_Receive(uint8_t* Bytes, uint8_t Length, uint8_t returnFullFrame)
|
||||
{
|
||||
/*
|
||||
a frame lookes like:
|
||||
byte 1 = fremae header (0x02 = bootloader, 0x03 = ESC firmware)
|
||||
|
|
@ -246,44 +273,61 @@ uint8_t FETtecOneWire_Receive(uint8_t *Bytes, uint8_t Length, uint8_t returnFull
|
|||
*/
|
||||
|
||||
//ignore own bytes
|
||||
while(FETtecOneWire_IgnoreOwnBytes > 0 && FETOW_UART_BYTES_AVAILABLE){
|
||||
while (FETtecOneWire_IgnoreOwnBytes > 0 && _uart->available()) {
|
||||
FETtecOneWire_IgnoreOwnBytes--;
|
||||
FETOW_UART_READ_BYTE;
|
||||
_uart->read();
|
||||
}
|
||||
// look for the real answer
|
||||
if(FETOW_UART_BYTES_AVAILABLE >= Length+6){
|
||||
if (_uart->available() >= Length + 6u) {
|
||||
// sync to frame starte byte
|
||||
uint8_t testFrameStart = 0;
|
||||
do{
|
||||
testFrameStart = FETOW_UART_READ_BYTE;
|
||||
}while(testFrameStart != 0x02 && testFrameStart != 0x03 && FETOW_UART_BYTES_AVAILABLE);
|
||||
do {
|
||||
testFrameStart = _uart->read();
|
||||
}
|
||||
while (testFrameStart != 0x02 && testFrameStart != 0x03 && _uart->available());
|
||||
// copy message
|
||||
if(FETOW_UART_BYTES_AVAILABLE >= Length+5){
|
||||
if (_uart->available() >= Length + 5u) {
|
||||
uint8_t ReceiveBuf[20] = {0};
|
||||
ReceiveBuf[0] = testFrameStart;
|
||||
for(uint8_t i=1; i<Length+6;i++) ReceiveBuf[i] = FETOW_UART_READ_BYTE;
|
||||
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];
|
||||
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
|
||||
} 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 FETtecOneWire_Beep(uint8_t beepFreqency){
|
||||
if(FETtecOneWire_IDcount > 0){
|
||||
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++){
|
||||
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
|
||||
FETOW_UART_SEND_BYTES(spacer, 2);
|
||||
_uart->write(spacer, 2);
|
||||
FETtecOneWire_IgnoreOwnBytes += 2;
|
||||
}
|
||||
}
|
||||
|
|
@ -293,14 +337,15 @@ 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){
|
||||
if(FETtecOneWire_IDcount > 0){
|
||||
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++){
|
||||
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
|
||||
FETOW_UART_SEND_BYTES(spacer, 2);
|
||||
_uart->write(spacer, 2);
|
||||
FETtecOneWire_IgnoreOwnBytes += 2;
|
||||
}
|
||||
}
|
||||
|
|
@ -310,7 +355,8 @@ void FETtecOneWire_RW_LEDcolor(uint8_t R, uint8_t G, uint8_t B){
|
|||
Resets a pending pull request
|
||||
returns nothing
|
||||
*/
|
||||
void FETtecOneWire_PullReset(){
|
||||
void AP_FETtecOneWire::FETtecOneWire_PullReset()
|
||||
{
|
||||
FETtecOneWire_PullSuccess = 0;
|
||||
FETtecOneWire_PullBusy = 0;
|
||||
}
|
||||
|
|
@ -323,13 +369,15 @@ void FETtecOneWire_PullReset(){
|
|||
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){
|
||||
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)){
|
||||
} else {
|
||||
if (FETtecOneWire_Receive(response, FETtecOneWire_ResponseLength[command[0]], returnFullFrame)) {
|
||||
FETtecOneWire_PullSuccess = 1;
|
||||
FETtecOneWire_PullBusy = 0;
|
||||
}
|
||||
|
|
@ -337,81 +385,92 @@ uint8_t FETtecOneWire_PullCommand(uint8_t ESC_id, uint8_t *command, uint8_t *res
|
|||
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(){
|
||||
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){
|
||||
if (FETtecOneWire_ScanActive == 0) {
|
||||
delayLoops = 500;
|
||||
scanID = 0;
|
||||
scanState = 0;
|
||||
scanTimeOut = 0;
|
||||
return FETtecOneWire_ScanActive+1;
|
||||
return FETtecOneWire_ScanActive + 1;
|
||||
}
|
||||
if(delayLoops > 0){
|
||||
if (delayLoops > 0) {
|
||||
delayLoops--;
|
||||
return FETtecOneWire_ScanActive;
|
||||
}
|
||||
if(scanID < 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)){
|
||||
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;
|
||||
if (response[0] == 0x02) {
|
||||
FETtecOneWire_foundESCs[scanID].inBootLoader = 1;
|
||||
} else {
|
||||
FETtecOneWire_foundESCs[scanID].inBootLoader = 0;
|
||||
}
|
||||
delayLoops = 1;
|
||||
scanState++;
|
||||
}else scanTimeOut++;
|
||||
} else {
|
||||
scanTimeOut++;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
request[0] = OW_REQ_TYPE;
|
||||
if(FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_RESPONSE)){
|
||||
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++;
|
||||
} else {
|
||||
scanTimeOut++;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
request[0] = OW_REQ_SW_VER;
|
||||
if(FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_RESPONSE)){
|
||||
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++;
|
||||
} else {
|
||||
scanTimeOut++;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
request[0] = OW_REQ_SN;
|
||||
if(FETtecOneWire_PullCommand(scanID, request, response, OW_RETURN_RESPONSE)){
|
||||
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];
|
||||
for (uint8_t i = 0; i < 12; i++) {
|
||||
FETtecOneWire_foundESCs[scanID].serialNumber[i] = response[i];
|
||||
}
|
||||
delayLoops = 1;
|
||||
return scanID+1;
|
||||
}else scanTimeOut++;
|
||||
return scanID + 1;
|
||||
} else {
|
||||
scanTimeOut++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}else{
|
||||
} else {
|
||||
FETtecOneWire_PullReset();
|
||||
return scanID+1;
|
||||
return scanID + 1;
|
||||
}
|
||||
return scanID;
|
||||
}
|
||||
|
|
@ -420,27 +479,31 @@ uint8_t FETtecOneWire_ScanESCs(){
|
|||
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(){
|
||||
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};
|
||||
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){
|
||||
if (FETtecOneWire_SetupActive == 0) {
|
||||
delayLoops = 0;
|
||||
activeID = 1;
|
||||
State = 0;
|
||||
TimeOut = 0;
|
||||
wakeFromBL = 1;
|
||||
return FETtecOneWire_SetupActive+1;
|
||||
return FETtecOneWire_SetupActive + 1;
|
||||
}
|
||||
while (FETtecOneWire_activeESC_IDs[FETtecOneWire_SetupActive] == 0 && FETtecOneWire_SetupActive < 25) {
|
||||
FETtecOneWire_SetupActive++;
|
||||
}
|
||||
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){
|
||||
if (FETtecOneWire_SetupActive == 25 && wakeFromBL == 0) {
|
||||
return FETtecOneWire_SetupActive;
|
||||
} else if (FETtecOneWire_SetupActive == 25 && wakeFromBL) {
|
||||
wakeFromBL = 0;
|
||||
activeID = 1;
|
||||
FETtecOneWire_SetupActive = 1;
|
||||
|
|
@ -450,82 +513,91 @@ uint8_t FETtecOneWire_InitESCs(){
|
|||
FETtecOneWire_minID = 25;
|
||||
FETtecOneWire_maxID = 0;
|
||||
FETtecOneWire_IDcount = 0;
|
||||
for(uint8_t i=0; i<25; i++){
|
||||
if(FETtecOneWire_activeESC_IDs[i] != 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 (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
|
||||
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){
|
||||
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){
|
||||
if (delayLoops > 0) {
|
||||
delayLoops--;
|
||||
return FETtecOneWire_SetupActive;
|
||||
}
|
||||
|
||||
if(activeID < 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 == 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){
|
||||
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;
|
||||
} else {
|
||||
return activeID + 1;
|
||||
}
|
||||
State = 1;
|
||||
break;
|
||||
case 1:
|
||||
request[0] = OW_OK;
|
||||
if(FETtecOneWire_PullCommand(activeID, request, response, OW_RETURN_FULL_FRAME)){
|
||||
case 1:request[0] = OW_OK;
|
||||
if (FETtecOneWire_PullCommand(activeID, request, response, OW_RETURN_FULL_FRAME)) {
|
||||
TimeOut = 0;
|
||||
if(response[0] == 0x02){
|
||||
if (response[0] == 0x02) {
|
||||
FETtecOneWire_foundESCs[activeID].inBootLoader = 1;
|
||||
State = 0;
|
||||
}else{
|
||||
} else {
|
||||
FETtecOneWire_foundESCs[activeID].inBootLoader = 0;
|
||||
delayLoops = 1;
|
||||
return activeID+1;
|
||||
return activeID + 1;
|
||||
}
|
||||
} else {
|
||||
TimeOut++;
|
||||
}
|
||||
}else TimeOut++;
|
||||
break;
|
||||
}
|
||||
}else{
|
||||
if(FETtecOneWire_PullCommand(activeID, setFastCommand, response, OW_RETURN_RESPONSE)){
|
||||
} else {
|
||||
if (FETtecOneWire_PullCommand(activeID, setFastCommand, response, OW_RETURN_RESPONSE)) {
|
||||
TimeOut = 0;
|
||||
delayLoops = 1;
|
||||
return activeID+1;
|
||||
}else TimeOut++;
|
||||
return activeID + 1;
|
||||
} else {
|
||||
TimeOut++;
|
||||
}
|
||||
}else{
|
||||
}
|
||||
} else {
|
||||
FETtecOneWire_PullReset();
|
||||
return activeID+1;
|
||||
return activeID + 1;
|
||||
}
|
||||
return activeID;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -533,27 +605,34 @@ uint8_t FETtecOneWire_InitESCs(){
|
|||
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){
|
||||
int8_t AP_FETtecOneWire::FETtecOneWire_CheckForTLM(uint16_t* Telemetry)
|
||||
{
|
||||
int8_t return_TLM_request = 0;
|
||||
if(FETtecOneWire_IDcount > 0){
|
||||
if (FETtecOneWire_IDcount > 0) {
|
||||
// empty buffer
|
||||
while(FETtecOneWire_IgnoreOwnBytes > 0 && FETOW_UART_BYTES_AVAILABLE){
|
||||
FETOW_UART_READ_BYTE;
|
||||
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(FETOW_UART_BYTES_AVAILABLE == (FETtecOneWire_IDcount*2)+1){
|
||||
if (_uart->available() == (FETtecOneWire_IDcount * 2) + 1u) {
|
||||
// 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_IDcount; i++){
|
||||
Telemetry[i] = FETOW_UART_READ_BYTE<<8;
|
||||
Telemetry[i] |= FETOW_UART_READ_BYTE;
|
||||
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;
|
||||
} else {
|
||||
return_TLM_request = -1;
|
||||
}
|
||||
} else {
|
||||
return_TLM_request = -1;
|
||||
}
|
||||
} else {
|
||||
return_TLM_request = -1;
|
||||
}
|
||||
return return_TLM_request;
|
||||
}
|
||||
|
||||
|
|
@ -568,29 +647,34 @@ int8_t FETtecOneWire_CheckForTLM(uint16_t *Telemetry){
|
|||
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){
|
||||
static uint32_t lastTime = 0;
|
||||
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){
|
||||
if((uint32_t)micros() -(uint32_t)lastTime < 700) return 0;
|
||||
else lastTime = micros();
|
||||
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){
|
||||
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){
|
||||
} else if (FETtecOneWire_SetupActive < 25) {
|
||||
if (FETtecOneWire_FoundESCs == 0) {
|
||||
FETtecOneWire_ScanActive = 0;
|
||||
}else{
|
||||
} else {
|
||||
// check if in bootloader, start ESC's FW if they are and prepare fast throttle command
|
||||
FETtecOneWire_SetupActive = FETtecOneWire_InitESCs();
|
||||
}
|
||||
}
|
||||
}else{
|
||||
} else {
|
||||
//send fast throttle signals
|
||||
if(FETtecOneWire_IDcount > 0){
|
||||
if (FETtecOneWire_IDcount > 0) {
|
||||
|
||||
// check for telemetry
|
||||
return_TLM_request = FETtecOneWire_CheckForTLM(Telemetry);
|
||||
|
|
@ -599,8 +683,12 @@ int8_t FETtecOneWire_ESCsSetValues(uint16_t *motorValues, uint16_t *Telemetry, u
|
|||
//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);
|
||||
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;
|
||||
|
||||
|
|
@ -629,7 +717,9 @@ int8_t FETtecOneWire_ESCsSetValues(uint16_t *motorValues, uint16_t *Telemetry, u
|
|||
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);
|
||||
OneWireFastThrottleCommand[actByte] |=
|
||||
(useSignals[actThrottleCommand] & ((1 << BitsLeftFromCommand) - 1))
|
||||
<< (bitsFromByteLeft - BitsLeftFromCommand);
|
||||
bitsToAddLeft -= BitsLeftFromCommand;
|
||||
bitsFromByteLeft -= BitsLeftFromCommand;
|
||||
actThrottleCommand++;
|
||||
|
|
@ -639,7 +729,9 @@ int8_t FETtecOneWire_ESCsSetValues(uint16_t *motorValues, uint16_t *Telemetry, u
|
|||
bitsFromByteLeft = 8;
|
||||
}
|
||||
} else {
|
||||
OneWireFastThrottleCommand[actByte] |= (useSignals[actThrottleCommand] >> (BitsLeftFromCommand - bitsFromByteLeft)) & ((1 << bitsFromByteLeft) - 1);
|
||||
OneWireFastThrottleCommand[actByte] |=
|
||||
(useSignals[actThrottleCommand] >> (BitsLeftFromCommand - bitsFromByteLeft))
|
||||
& ((1 << bitsFromByteLeft) - 1);
|
||||
bitsToAddLeft -= bitsFromByteLeft;
|
||||
BitsLeftFromCommand -= bitsFromByteLeft;
|
||||
actByte++;
|
||||
|
|
@ -651,29 +743,19 @@ int8_t FETtecOneWire_ESCsSetValues(uint16_t *motorValues, uint16_t *Telemetry, u
|
|||
}
|
||||
}
|
||||
// empty buffer
|
||||
while(FETOW_UART_BYTES_AVAILABLE) FETOW_UART_READ_BYTE;
|
||||
while (_uart->available()) {
|
||||
_uart->read();
|
||||
}
|
||||
|
||||
// send throttle signal
|
||||
OneWireFastThrottleCommand[FETtecOneWire_FastThrottleByteCount-1] = FETtecOneWire_GetCrc8(OneWireFastThrottleCommand, FETtecOneWire_FastThrottleByteCount-1);
|
||||
FETOW_UART_SEND_BYTES(OneWireFastThrottleCommand, FETtecOneWire_FastThrottleByteCount);
|
||||
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];
|
||||
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
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -15,69 +15,173 @@
|
|||
|
||||
/* 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 <AP_HAL/AP_HAL.h>
|
||||
|
||||
#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);
|
||||
|
||||
/*
|
||||
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
|
||||
{
|
||||
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;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#define ALL_ID 0x1F
|
||||
|
||||
enum {
|
||||
enum
|
||||
{
|
||||
OW_RETURN_RESPONSE,
|
||||
OW_RETURN_FULL_FRAME
|
||||
};
|
||||
};
|
||||
|
||||
enum {
|
||||
enum
|
||||
{
|
||||
OW_OK,
|
||||
OW_BL_PAGE_CORRECT, // BL only
|
||||
OW_NOT_OK,
|
||||
|
|
@ -86,9 +190,10 @@ enum {
|
|||
OW_REQ_TYPE,
|
||||
OW_REQ_SN,
|
||||
OW_REQ_SW_VER
|
||||
};
|
||||
};
|
||||
|
||||
enum {
|
||||
enum
|
||||
{
|
||||
OW_RESET_TO_BL = 10,
|
||||
OW_THROTTLE = 11,
|
||||
OW_REQ_TLM = 12,
|
||||
|
|
@ -131,9 +236,10 @@ enum {
|
|||
OW_GET_LED_COLOR = 52,
|
||||
OW_SET_LED_COLOR = 53
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
enum {
|
||||
enum
|
||||
{
|
||||
OW_TLM_TEMP,
|
||||
OW_TLM_VOLT,
|
||||
OW_TLM_CURRENT,
|
||||
|
|
@ -142,112 +248,9 @@ enum {
|
|||
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
|
||||
|
||||
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);
|
||||
Loading…
Reference in New Issue