From 4c9c3423510bcb3c5381e7736a8a7f5d188fcf91 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 17 Dec 2023 23:34:32 +0100 Subject: [PATCH 01/28] 4way interface init --- lib/betaflight/src/io/serial_4way.c | 916 ++++++++++++++++++ lib/betaflight/src/io/serial_4way.h | 56 ++ .../src/io/serial_4way_avrootloader.c | 908 +++++++++++++++++ .../src/io/serial_4way_avrootloader.h | 44 + lib/betaflight/src/io/serial_4way_impl.h | 53 + lib/betaflight/src/io/serial_4way_stk500v2.c | 427 ++++++++ lib/betaflight/src/io/serial_4way_stk500v2.h | 32 + 7 files changed, 2436 insertions(+) create mode 100644 lib/betaflight/src/io/serial_4way.c create mode 100644 lib/betaflight/src/io/serial_4way.h create mode 100644 lib/betaflight/src/io/serial_4way_avrootloader.c create mode 100644 lib/betaflight/src/io/serial_4way_avrootloader.h create mode 100644 lib/betaflight/src/io/serial_4way_impl.h create mode 100644 lib/betaflight/src/io/serial_4way_stk500v2.c create mode 100644 lib/betaflight/src/io/serial_4way_stk500v2.h diff --git a/lib/betaflight/src/io/serial_4way.c b/lib/betaflight/src/io/serial_4way.c new file mode 100644 index 00000000..091c7e0f --- /dev/null +++ b/lib/betaflight/src/io/serial_4way.c @@ -0,0 +1,916 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 +*/ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + +#include "drivers/buf_writer.h" +#include "drivers/io.h" +#include "drivers/serial.h" +#include "drivers/time.h" +#include "drivers/timer.h" +#include "drivers/light_led.h" + +#include "flight/mixer.h" + +#include "io/beeper.h" +#include "io/serial_4way.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#include "io/serial_4way_avrootloader.h" +#endif +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) +#include "io/serial_4way_stk500v2.h" +#endif + +#if defined(USE_HAL_DRIVER) +#define Bit_RESET GPIO_PIN_RESET +#elif defined(AT32F435) +#define Bit_RESET 0 +#endif + +#define USE_TXRX_LED + +#ifdef USE_TXRX_LED +#define RX_LED_OFF LED0_OFF +#define RX_LED_ON LED0_ON +#ifdef LED1 +#define TX_LED_OFF LED1_OFF +#define TX_LED_ON LED1_ON +#else +#define TX_LED_OFF LED0_OFF +#define TX_LED_ON LED0_ON +#endif +#else +#define RX_LED_OFF +#define RX_LED_ON +#define TX_LED_OFF +#define TX_LED_ON +#endif + +#define SERIAL_4WAY_INTERFACE_NAME_STR "m4wFCIntf" +// *** change to adapt Revision +#define SERIAL_4WAY_VER_MAIN 20 +#define SERIAL_4WAY_VER_SUB_1 (uint8_t) 0 +#define SERIAL_4WAY_VER_SUB_2 (uint8_t) 06 + +#define SERIAL_4WAY_PROTOCOL_VER 108 +// *** end + +#if (SERIAL_4WAY_VER_MAIN > 24) +#error "beware of SERIAL_4WAY_VER_SUB_1 is uint8_t" +#endif + +#define SERIAL_4WAY_VERSION (uint16_t) ((SERIAL_4WAY_VER_MAIN * 1000) + (SERIAL_4WAY_VER_SUB_1 * 100) + SERIAL_4WAY_VER_SUB_2) + +#define SERIAL_4WAY_VERSION_HI (uint8_t) (SERIAL_4WAY_VERSION / 100) +#define SERIAL_4WAY_VERSION_LO (uint8_t) (SERIAL_4WAY_VERSION % 100) + +static uint8_t escCount; + +escHardware_t escHardware[MAX_SUPPORTED_MOTORS]; + +uint8_t selected_esc; + +uint8_32_u DeviceInfo; + +#define DeviceInfoSize 4 + +inline bool isMcuConnected(void) +{ + return (DeviceInfo.bytes[0] > 0); +} + +inline bool isEscHi(uint8_t selEsc) +{ + return (IORead(escHardware[selEsc].io) != Bit_RESET); +} +inline bool isEscLo(uint8_t selEsc) +{ + return (IORead(escHardware[selEsc].io) == Bit_RESET); +} + +inline void setEscHi(uint8_t selEsc) +{ + IOHi(escHardware[selEsc].io); +} + +inline void setEscLo(uint8_t selEsc) +{ + IOLo(escHardware[selEsc].io); +} + +inline void setEscInput(uint8_t selEsc) +{ + IOConfigGPIO(escHardware[selEsc].io, IOCFG_IPU); +} + +inline void setEscOutput(uint8_t selEsc) +{ + IOConfigGPIO(escHardware[selEsc].io, IOCFG_OUT_PP); +} + +uint8_t esc4wayInit(void) +{ + // StopPwmAllMotors(); + // XXX Review effect of motor refactor + //pwmDisableMotors(); + escCount = 0; + memset(&escHardware, 0, sizeof(escHardware)); + pwmOutputPort_t *pwmMotors = pwmGetMotors(); + for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (pwmMotors[i].enabled) { + if (pwmMotors[i].io != IO_NONE) { + escHardware[escCount].io = pwmMotors[i].io; + setEscInput(escCount); + setEscHi(escCount); + escCount++; + } + } + } + motorDisable(); + return escCount; +} + +void esc4wayRelease(void) +{ + while (escCount > 0) { + escCount--; + IOConfigGPIO(escHardware[escCount].io, IOCFG_AF_PP); + setEscLo(escCount); + } + motorEnable(); +} + + +#define SET_DISCONNECTED DeviceInfo.words[0] = 0 + +#define INTF_MODE_IDX 3 // index for DeviceInfostate + +// Interface related only +// establish and test connection to the Interface + +// Send Structure +// ESC + CMD PARAM_LEN [PARAM (if len > 0)] CRC16_Hi CRC16_Lo +// Return +// ESC CMD PARAM_LEN [PARAM (if len > 0)] + ACK (uint8_t OK or ERR) + CRC16_Hi CRC16_Lo + +#define cmd_Remote_Escape 0x2E // '.' +#define cmd_Local_Escape 0x2F // '/' + +// Test Interface still present +#define cmd_InterfaceTestAlive 0x30 // '0' alive +// RETURN: ACK + +// get Protocol Version Number 01..255 +#define cmd_ProtocolGetVersion 0x31 // '1' version +// RETURN: uint8_t VersionNumber + ACK + +// get Version String +#define cmd_InterfaceGetName 0x32 // '2' name +// RETURN: String + ACK + +//get Version Number 01..255 +#define cmd_InterfaceGetVersion 0x33 // '3' version +// RETURN: uint8_t AVersionNumber + ACK + + +// Exit / Restart Interface - can be used to switch to Box Mode +#define cmd_InterfaceExit 0x34 // '4' exit +// RETURN: ACK + +// Reset the Device connected to the Interface +#define cmd_DeviceReset 0x35 // '5' reset +// RETURN: ACK + +// Get the Device ID connected +// #define cmd_DeviceGetID 0x36 //'6' device id removed since 06/106 +// RETURN: uint8_t DeviceID + ACK + +// Initialize Flash Access for Device connected +#define cmd_DeviceInitFlash 0x37 // '7' init flash access +// RETURN: ACK + +// Erase the whole Device Memory of connected Device +#define cmd_DeviceEraseAll 0x38 // '8' erase all +// RETURN: ACK + +// Erase one Page of Device Memory of connected Device +#define cmd_DevicePageErase 0x39 // '9' page erase +// PARAM: uint8_t APageNumber +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceRead 0x3A // ':' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWrite 0x3B // ';' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set C2CK low infinite ) permanent Reset state +#define cmd_DeviceC2CK_LOW 0x3C // '<' +// RETURN: ACK + +// Read to Buffer from Device Memory of connected Device //Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceReadEEprom 0x3D // '=' read Device +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BuffLen[0..255] +// RETURN: PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] ACK + +// Write to Buffer for Device Memory of connected Device // Buffer Len is Max 256 Bytes +// BuffLen = 0 means 256 Bytes +#define cmd_DeviceWriteEEprom 0x3E // '>' write +// PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +// RETURN: ACK + +// Set Interface Mode +#define cmd_InterfaceSetMode 0x3F // '?' +// #define imC2 0 +// #define imSIL_BLB 1 +// #define imATM_BLB 2 +// #define imSK 3 +// PARAM: uint8_t Mode +// RETURN: ACK or ACK_I_INVALID_CHANNEL + +//Write to Buffer for Verify Device Memory of connected Device //Buffer Len is Max 256 Bytes +//BuffLen = 0 means 256 Bytes +#define cmd_DeviceVerify 0x40 //'@' write +//PARAM: uint8_t ADRESS_Hi + ADRESS_Lo + BUffLen + Buffer[0..255] +//RETURN: ACK + + +// responses +#define ACK_OK 0x00 +// #define ACK_I_UNKNOWN_ERROR 0x01 +#define ACK_I_INVALID_CMD 0x02 +#define ACK_I_INVALID_CRC 0x03 +#define ACK_I_VERIFY_ERROR 0x04 +// #define ACK_D_INVALID_COMMAND 0x05 +// #define ACK_D_COMMAND_FAILED 0x06 +// #define ACK_D_UNKNOWN_ERROR 0x07 + +#define ACK_I_INVALID_CHANNEL 0x08 +#define ACK_I_INVALID_PARAM 0x09 +#define ACK_D_GENERAL_ERROR 0x0F + +/* Copyright (c) 2002, 2003, 2004 Marek Michalkiewicz + Copyright (c) 2005, 2007 Joerg Wunsch + Copyright (c) 2013 Dave Hylands + Copyright (c) 2013 Frederic Nadeau + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in + the documentation and/or other materials provided with the + distribution. + + * Neither the name of the copyright holders nor the names of + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. */ +uint16_t _crc_xmodem_update (uint16_t crc, uint8_t data) +{ + int i; + + crc = crc ^ ((uint16_t)data << 8); + for (i=0; i < 8; i++) { + if (crc & 0x8000) + crc = (crc << 1) ^ 0x1021; + else + crc <<= 1; + } + return crc; +} +// * End copyright + + +#define ATMEL_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x9307) || (pDeviceInfo->words[0] == 0x930A) || \ + (pDeviceInfo->words[0] == 0x930F) || (pDeviceInfo->words[0] == 0x940B)) + +#define SILABS_DEVICE_MATCH ((pDeviceInfo->words[0] > 0xE800) && (pDeviceInfo->words[0] < 0xF900)) + + +// BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06 +#define ARM_DEVICE_MATCH ((pDeviceInfo->bytes[1] > 0x00) && (pDeviceInfo->bytes[1] < 0x90) && (pDeviceInfo->bytes[0] == 0x06)) + +static uint8_t CurrentInterfaceMode; + +static uint8_t Connect(uint8_32_u *pDeviceInfo) +{ + for (uint8_t I = 0; I < 3; ++I) { +#if (defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER)) + if ((CurrentInterfaceMode != imARM_BLB) && Stk_ConnectEx(pDeviceInfo) && ATMEL_DEVICE_MATCH) { + CurrentInterfaceMode = imSK; + return 1; + } else { + if (BL_ConnectEx(pDeviceInfo)) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } else if ARM_DEVICE_MATCH { + CurrentInterfaceMode = imARM_BLB; + return 1; + } + } + } +#elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if (BL_ConnectEx(pDeviceInfo)) { + if SILABS_DEVICE_MATCH { + CurrentInterfaceMode = imSIL_BLB; + return 1; + } else if ATMEL_DEVICE_MATCH { + CurrentInterfaceMode = imATM_BLB; + return 1; + } else if ARM_DEVICE_MATCH { + CurrentInterfaceMode = imARM_BLB; + return 1; + } + } +#elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (Stk_ConnectEx(pDeviceInfo)) { + CurrentInterfaceMode = imSK; + if ATMEL_DEVICE_MATCH return 1; + } +#endif + } + return 0; +} + +static serialPort_t *port; + +static uint8_t ReadByte(void) +{ + // need timeout? + while (!serialRxBytesWaiting(port)); + return serialRead(port); +} + +static uint8_16_u CRC_in; +static uint8_t ReadByteCrc(void) +{ + uint8_t b = ReadByte(); + CRC_in.word = _crc_xmodem_update(CRC_in.word, b); + return b; +} + +static void WriteByte(uint8_t b) +{ + serialWrite(port, b); +} + +static uint8_16_u CRCout; +static void WriteByteCrc(uint8_t b) +{ + WriteByte(b); + CRCout.word = _crc_xmodem_update(CRCout.word, b); +} + +void esc4wayProcess(serialPort_t *mspPort) +{ + + uint8_t ParamBuf[256]; + uint8_t ESC; + uint8_t I_PARAM_LEN; + uint8_t CMD; + uint8_t ACK_OUT; + uint8_16_u CRC_check; + uint8_16_u Dummy; + uint8_t O_PARAM_LEN; + uint8_t *O_PARAM; + uint8_t *InBuff; + ioMem_t ioMem; + + port = mspPort; + + // Start here with UART Main loop +#ifdef USE_BEEPER + // fix for buzzer often starts beeping continuously when the ESCs are read + // switch beeper silent here + beeperSilence(); +#endif + bool isExitScheduled = false; + + while (1) { + // restart looking for new sequence from host + do { + CRC_in.word = 0; + ESC = ReadByteCrc(); + } while (ESC != cmd_Local_Escape); + + RX_LED_ON; + + Dummy.word = 0; + O_PARAM = &Dummy.bytes[0]; + O_PARAM_LEN = 1; + CMD = ReadByteCrc(); + ioMem.D_FLASH_ADDR_H = ReadByteCrc(); + ioMem.D_FLASH_ADDR_L = ReadByteCrc(); + I_PARAM_LEN = ReadByteCrc(); + + InBuff = ParamBuf; + uint8_t i = I_PARAM_LEN; + do { + *InBuff = ReadByteCrc(); + InBuff++; + i--; + } while (i != 0); + + CRC_check.bytes[1] = ReadByte(); + CRC_check.bytes[0] = ReadByte(); + + if (CRC_check.word == CRC_in.word) { + ACK_OUT = ACK_OK; + } else { + ACK_OUT = ACK_I_INVALID_CRC; + } + + TX_LED_ON; + + if (ACK_OUT == ACK_OK) + { + // wtf.D_FLASH_ADDR_H=Adress_H; + // wtf.D_FLASH_ADDR_L=Adress_L; + ioMem.D_PTR_I = ParamBuf; + + + switch (CMD) { + // ******* Interface related stuff ******* + case cmd_InterfaceTestAlive: + { + if (isMcuConnected()) { + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imSIL_BLB: + case imARM_BLB: + { + if (!BL_SendCMDKeepAlive()) { // SetStateDisconnected() included + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_SignOn()) { // SetStateDisconnected(); + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_D_GENERAL_ERROR; + } + if ( ACK_OUT != ACK_OK) SET_DISCONNECTED; + } + break; + } + case cmd_ProtocolGetVersion: + { + // Only interface itself, no matter what Device + Dummy.bytes[0] = SERIAL_4WAY_PROTOCOL_VER; + break; + } + + case cmd_InterfaceGetName: + { + // Only interface itself, no matter what Device + // O_PARAM_LEN=16; + O_PARAM_LEN = strlen(SERIAL_4WAY_INTERFACE_NAME_STR); + O_PARAM = (uint8_t *)SERIAL_4WAY_INTERFACE_NAME_STR; + break; + } + + case cmd_InterfaceGetVersion: + { + // Only interface itself, no matter what Device + // Dummy = iUart_res_InterfVersion; + O_PARAM_LEN = 2; + Dummy.bytes[0] = SERIAL_4WAY_VERSION_HI; + Dummy.bytes[1] = SERIAL_4WAY_VERSION_LO; + break; + } + case cmd_InterfaceExit: + { + isExitScheduled = true; + break; + } + case cmd_InterfaceSetMode: + { +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if ((ParamBuf[0] <= imARM_BLB) && (ParamBuf[0] >= imSIL_BLB)) { +#elif defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) + if (((ParamBuf[0] <= imATM_BLB)||(ParamBuf[0] == imARM_BLB)) && (ParamBuf[0] >= imSIL_BLB)) { +#elif defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + if (ParamBuf[0] == imSK) { +#endif + CurrentInterfaceMode = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_PARAM; + } + break; + } + + case cmd_DeviceReset: + { + bool rebootEsc = false; + if (ParamBuf[0] < escCount) { + // Channel may change here + selected_esc = ParamBuf[0]; + if (ioMem.D_FLASH_ADDR_L == 1) { + rebootEsc = true; + } + } + else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + case imARM_BLB: + { + BL_SendCMDRunRestartBootloader(&DeviceInfo); + if (rebootEsc) { + ESC_OUTPUT; + setEscLo(selected_esc); + timeMs_t m = millis(); + while (millis() - m < 300); + setEscHi(selected_esc); + ESC_INPUT; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + break; + } + #endif + } + SET_DISCONNECTED; + break; + } + case cmd_DeviceInitFlash: + { + SET_DISCONNECTED; + if (ParamBuf[0] < escCount) { + //Channel may change here + //ESC_LO or ESC_HI; Halt state for prev channel + selected_esc = ParamBuf[0]; + } else { + ACK_OUT = ACK_I_INVALID_CHANNEL; + break; + } + O_PARAM_LEN = DeviceInfoSize; //4 + O_PARAM = (uint8_t *)&DeviceInfo; + if (Connect(&DeviceInfo)) { + DeviceInfo.bytes[INTF_MODE_IDX] = CurrentInterfaceMode; + } else { + SET_DISCONNECTED; + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case cmd_DeviceEraseAll: + { + switch (CurrentInterfaceMode) + { + case imSK: + { + if (!Stk_Chip_Erase()) ACK_OUT=ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DevicePageErase: + { + switch (CurrentInterfaceMode) + { + case imSIL_BLB: + case imARM_BLB: + { + Dummy.bytes[0] = ParamBuf[0]; + if (CurrentInterfaceMode == imARM_BLB) { + // Address =Page * 1024 + ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 2); + } else { + // Address =Page * 512 + ioMem.D_FLASH_ADDR_H = (Dummy.bytes[0] << 1); + } + ioMem.D_FLASH_ADDR_L = 0; + if (!BL_PageErase(&ioMem)) ACK_OUT = ACK_D_GENERAL_ERROR; + break; + } + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + break; + } + #endif + + //*** Device Memory Read Ops *** + case cmd_DeviceRead: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + case imARM_BLB: + { + if (!BL_ReadFlash(CurrentInterfaceMode, &ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_ReadFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if (ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + case cmd_DeviceReadEEprom: + { + ioMem.D_NUM_BYTES = ParamBuf[0]; + /* + wtf.D_FLASH_ADDR_H = Adress_H; + wtf.D_FLASH_ADDR_L = Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imATM_BLB: + { + if (!BL_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_ReadEEprom(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + default: + ACK_OUT = ACK_I_INVALID_CMD; + } + if (ACK_OUT == ACK_OK) + { + O_PARAM_LEN = ioMem.D_NUM_BYTES; + O_PARAM = (uint8_t *)&ParamBuf; + } + break; + } + + //*** Device Memory Write Ops *** + case cmd_DeviceWrite: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + case imATM_BLB: + case imARM_BLB: + { + if (!BL_WriteFlash(&ioMem)) { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (!Stk_WriteFlash(&ioMem)) + { + ACK_OUT = ACK_D_GENERAL_ERROR; + } + break; + } + #endif + } + break; + } + + case cmd_DeviceWriteEEprom: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + ACK_OUT = ACK_D_GENERAL_ERROR; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + switch (CurrentInterfaceMode) + { + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case imSIL_BLB: + { + ACK_OUT = ACK_I_INVALID_CMD; + break; + } + case imATM_BLB: + { + if (BL_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + #ifdef USE_SERIAL_4WAY_SK_BOOTLOADER + case imSK: + { + if (Stk_WriteEEprom(&ioMem)) + { + ACK_OUT = ACK_OK; + } + break; + } + #endif + } + break; + } + //*** Device Memory Verify Ops *** + #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER + case cmd_DeviceVerify: + { + switch (CurrentInterfaceMode) + { + case imARM_BLB: + { + ioMem.D_NUM_BYTES = I_PARAM_LEN; + /* + wtf.D_FLASH_ADDR_H=Adress_H; + wtf.D_FLASH_ADDR_L=Adress_L; + wtf.D_PTR_I = BUF_I; + */ + + ACK_OUT = BL_VerifyFlash(&ioMem); + switch (ACK_OUT) { + case brSUCCESS: + ACK_OUT = ACK_OK; + break; + case brERRORVERIFY: + ACK_OUT = ACK_I_VERIFY_ERROR; + break; + default: + ACK_OUT = ACK_D_GENERAL_ERROR; + break; + } + break; + } + default: + { + ACK_OUT = ACK_I_INVALID_CMD; + break; + } + } + break; + } + #endif + default: + { + ACK_OUT = ACK_I_INVALID_CMD; + } + } + } + + CRCout.word = 0; + + RX_LED_OFF; + + serialBeginWrite(port); + WriteByteCrc(cmd_Remote_Escape); + WriteByteCrc(CMD); + WriteByteCrc(ioMem.D_FLASH_ADDR_H); + WriteByteCrc(ioMem.D_FLASH_ADDR_L); + WriteByteCrc(O_PARAM_LEN); + + i=O_PARAM_LEN; + do { + while (!serialTxBytesFree(port)); + + WriteByteCrc(*O_PARAM); + O_PARAM++; + i--; + } while (i > 0); + + WriteByteCrc(ACK_OUT); + WriteByte(CRCout.bytes[1]); + WriteByte(CRCout.bytes[0]); + serialEndWrite(port); + + TX_LED_OFF; + if (isExitScheduled) { + esc4wayRelease(); + return; + } + }; +} + + + +#endif diff --git a/lib/betaflight/src/io/serial_4way.h b/lib/betaflight/src/io/serial_4way.h new file mode 100644 index 00000000..37b1bf42 --- /dev/null +++ b/lib/betaflight/src/io/serial_4way.h @@ -0,0 +1,56 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 +*/ +#pragma once + +#include "drivers/io_types.h" +#include "io/serial_4way_impl.h" + +#define imC2 0 +#define imSIL_BLB 1 +#define imATM_BLB 2 +#define imSK 3 +#define imARM_BLB 4 + +extern uint8_t selected_esc; + +extern ioMem_t ioMem; + +typedef union __attribute__ ((packed)) { + uint8_t bytes[2]; + uint16_t word; +} uint8_16_u; + +typedef union __attribute__ ((packed)) { + uint8_t bytes[4]; + uint16_t words[2]; + uint32_t dword; +} uint8_32_u; + +//extern uint8_32_u DeviceInfo; + +bool isMcuConnected(void); +uint8_t esc4wayInit(void); +struct serialPort_s; +void esc4wayProcess(struct serialPort_s *mspPort); +void esc4wayRelease(void); diff --git a/lib/betaflight/src/io/serial_4way_avrootloader.c b/lib/betaflight/src/io/serial_4way_avrootloader.c new file mode 100644 index 00000000..425c1f61 --- /dev/null +++ b/lib/betaflight/src/io/serial_4way_avrootloader.c @@ -0,0 +1,908 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 + * for info about Hagens AVRootloader: + * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung +*/ + +#include +#include +#include + +#include "platform.h" + +#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE + +#include "drivers/io.h" +#include "drivers/serial.h" +#include "drivers/time.h" +#include "drivers/timer.h" + +#include "io/serial.h" +#include "io/serial_4way.h" +#include "io/serial_4way_impl.h" +#include "io/serial_4way_avrootloader.h" + +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && !defined(USE_VIRTUAL_ESC) + +// Bootloader commands +// RunCmd +#define RestartBootloader 0 +#define ExitBootloader 1 + +#define CMD_RUN 0x00 +#define CMD_PROG_FLASH 0x01 +#define CMD_ERASE_FLASH 0x02 +#define CMD_READ_FLASH_SIL 0x03 +#define CMD_VERIFY_FLASH 0x03 +#define CMD_VERIFY_FLASH_ARM 0x04 +#define CMD_READ_EEPROM 0x04 +#define CMD_PROG_EEPROM 0x05 +#define CMD_READ_SRAM 0x06 +#define CMD_READ_FLASH_ATM 0x07 +#define CMD_KEEP_ALIVE 0xFD +#define CMD_SET_ADDRESS 0xFF +#define CMD_SET_BUFFER 0xFE + +#define CMD_BOOTINIT 0x07 +#define CMD_BOOTSIGN 0x08 + +#define START_BIT_TIMEOUT_MS 2 + +#define BIT_TIME (52) // 52uS +#define BIT_TIME_HALVE (BIT_TIME >> 1) // 26uS +#define BIT_TIME_3_4 (BIT_TIME_HALVE + (BIT_TIME_HALVE >> 1)) // 39uS +#define START_BIT_TIME (BIT_TIME_3_4) +//#define STOP_BIT_TIME ((BIT_TIME * 9) + BIT_TIME_HALVE) + +static uint8_t suart_getc_(uint8_t *bt) +{ + uint32_t btime; + uint32_t start_time; + + uint32_t wait_time = millis() + START_BIT_TIMEOUT_MS; + while (ESC_IS_HI) { + // check for startbit begin + if (millis() >= wait_time) { + return 0; + } + } + // start bit + start_time = micros(); + btime = start_time + START_BIT_TIME; + uint16_t bitmask = 0; + uint8_t bit = 0; + while (micros() < btime); + while (1) { + if (ESC_IS_HI) + { + bitmask |= (1 << bit); + } + btime = btime + BIT_TIME; + bit++; + if (bit == 10) break; + while (micros() < btime); + } + // check start bit and stop bit + if ((bitmask & 1) || (!(bitmask & (1 << 9)))) { + return 0; + } + *bt = bitmask >> 1; + return 1; +} + +static void suart_putc_(uint8_t *tx_b) +{ + // shift out stopbit first + uint16_t bitmask = (*tx_b << 2) | 1 | (1 << 10); + uint32_t btime = micros(); + while (1) { + if (bitmask & 1) { + ESC_SET_HI; // 1 + } + else { + ESC_SET_LO; // 0 + } + btime = btime + BIT_TIME; + bitmask = (bitmask >> 1); + if (bitmask == 0) break; // stopbit shifted out - but don't wait + while (micros() < btime); + } +} + +static uint8_16_u CRC_16; +static uint8_16_u LastCRC_16; + +static void ByteCrc(uint8_t *bt) +{ + uint8_t xb = *bt; + for (uint8_t i = 0; i < 8; i++) + { + if (((xb & 0x01) ^ (CRC_16.word & 0x0001)) !=0 ) { + CRC_16.word = CRC_16.word >> 1; + CRC_16.word = CRC_16.word ^ 0xA001; + } else { + CRC_16.word = CRC_16.word >> 1; + } + xb = xb >> 1; + } +} + +static uint8_t BL_ReadBuf(uint8_t *pstring, uint8_t len) +{ + // len 0 means 256 + CRC_16.word = 0; + LastCRC_16.word = 0; + uint8_t LastACK = brNONE; + do { + if (!suart_getc_(pstring)) goto timeout; + ByteCrc(pstring); + pstring++; + len--; + } while (len > 0); + + if (isMcuConnected()) { + //With CRC read 3 more + if (!suart_getc_(&LastCRC_16.bytes[0])) goto timeout; + if (!suart_getc_(&LastCRC_16.bytes[1])) goto timeout; + if (!suart_getc_(&LastACK)) goto timeout; + if (CRC_16.word != LastCRC_16.word) { + LastACK = brERRORCRC; + } + } else { + if (!suart_getc_(&LastACK)) goto timeout; + } +timeout: + return (LastACK == brSUCCESS); +} + +static void BL_SendBuf(uint8_t *pstring, uint8_t len) +{ + ESC_OUTPUT; + CRC_16.word=0; + do { + suart_putc_(pstring); + ByteCrc(pstring); + pstring++; + len--; + } while (len > 0); + + if (isMcuConnected()) { + suart_putc_(&CRC_16.bytes[0]); + suart_putc_(&CRC_16.bytes[1]); + } + ESC_INPUT; +} + +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) +{ + #define BootMsgLen 4 + #define DevSignHi (BootMsgLen) + #define DevSignLo (BootMsgLen+1) + + //DeviceInfo.dword=0; is set before + uint8_t BootInfo[9]; + uint8_t BootMsg[BootMsgLen-1] = "471"; + // x * 0 + 9 +#if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 21); +#else + uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D}; + BL_SendBuf(BootInit, 17); +#endif + if (!BL_ReadBuf(BootInfo, BootMsgLen + 4)) { + return 0; + } + // BootInfo has no CRC (ACK byte already analyzed... ) + // Format = BootMsg("471c") SIGNATURE_001, SIGNATURE_002, BootVersion (always 6), BootPages (,ACK) + for (uint8_t i = 0; i < (BootMsgLen - 1); i++) { // Check only the first 3 letters -> 471x OK + if (BootInfo[i] != BootMsg[i]) { + return (0); + } + } + + //only 2 bytes used $1E9307 -> 0x9307 + pDeviceInfo->bytes[2] = BootInfo[BootMsgLen - 1]; + pDeviceInfo->bytes[1] = BootInfo[DevSignHi]; + pDeviceInfo->bytes[0] = BootInfo[DevSignLo]; + return (1); +} + +static uint8_t BL_GetACK(uint32_t Timeout) +{ + uint8_t LastACK = brNONE; + while (!(suart_getc_(&LastACK)) && (Timeout)) { + Timeout--; + } ; + return (LastACK); +} + +uint8_t BL_SendCMDKeepAlive(void) +{ + uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0}; + BL_SendBuf(sCMD, 2); + if (BL_GetACK(1) != brERRORCOMMAND) { + return 0; + } + return 1; +} + +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) +{ + uint8_t sCMD[] = {RestartBootloader, 0}; + pDeviceInfo->bytes[0] = 1; + BL_SendBuf(sCMD, 2); //sends simply 4 x 0x00 (CRC =00) + return; +} + +static uint8_t BL_SendCMDSetAddress(ioMem_t *pMem) //supports only 16 bit Adr +{ + // skip if adr == 0xFFFF + if ((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, pMem->D_FLASH_ADDR_H, pMem->D_FLASH_ADDR_L }; + BL_SendBuf(sCMD, 4); + return (BL_GetACK(2) == brSUCCESS); +} + +static uint8_t BL_SendCMDSetBuffer(ioMem_t *pMem) +{ + uint8_t sCMD[] = {CMD_SET_BUFFER, 0, 0, pMem->D_NUM_BYTES}; + if (pMem->D_NUM_BYTES == 0) { + // set high byte + sCMD[2] = 1; + } + BL_SendBuf(sCMD, 4); + if (BL_GetACK(2) != brNONE) return 0; + BL_SendBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES); + return (BL_GetACK(40) == brSUCCESS); +} + +static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) +{ + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {cmd, pMem->D_NUM_BYTES}; + BL_SendBuf(sCMD, 2); + return (BL_ReadBuf(pMem->D_PTR_I, pMem->D_NUM_BYTES )); + } + return 0; +} + +static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) +{ + if (BL_SendCMDSetAddress(pMem)) { + if (!BL_SendCMDSetBuffer(pMem)) return 0; + uint8_t sCMD[] = {cmd, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK(timeout) == brSUCCESS); + } + return 0; +} + +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) +{ + if (interface_mode == imATM_BLB) { + return BL_ReadA(CMD_READ_FLASH_ATM, pMem); + } else { + return BL_ReadA(CMD_READ_FLASH_SIL, pMem); + } +} + +uint8_t BL_ReadEEprom(ioMem_t *pMem) +{ + return BL_ReadA(CMD_READ_EEPROM, pMem); +} + +uint8_t BL_PageErase(ioMem_t *pMem) +{ + if (BL_SendCMDSetAddress(pMem)) { + uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK((3000 / START_BIT_TIMEOUT_MS)) == brSUCCESS); + } + return 0; +} + +uint8_t BL_WriteEEprom(ioMem_t *pMem) +{ + return BL_WriteA(CMD_PROG_EEPROM, pMem, (3000 / START_BIT_TIMEOUT_MS)); +} + +uint8_t BL_WriteFlash(ioMem_t *pMem) +{ + return BL_WriteA(CMD_PROG_FLASH, pMem, (500 / START_BIT_TIMEOUT_MS)); +} + +uint8_t BL_VerifyFlash(ioMem_t *pMem) +{ + if (BL_SendCMDSetAddress(pMem)) { + if (!BL_SendCMDSetBuffer(pMem)) return 0; + uint8_t sCMD[] = {CMD_VERIFY_FLASH_ARM, 0x01}; + BL_SendBuf(sCMD, 2); + return (BL_GetACK(40 / START_BIT_TIMEOUT_MS)); + } + return 0; +} + +#endif +#if defined(USE_SERIAL_4WAY_BLHELI_BOOTLOADER) && defined(USE_VIRTUAL_ESC) + +#define VIRTUAL_PAGE_SIZE 512 +#define VIRTUAL_FLASH_SIZE 16385 + +static uint8_t virtualFlash[VIRTUAL_FLASH_SIZE] = +{ + 0x02, 0x19, 0xFD, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x00, 0xAA, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x01, 0x87, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x03, 0x44, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0x02, 0x03, 0x31, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0x02, 0x03, 0x04, 0x06, 0x08, 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0x04, 0x06, 0x08, + 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0xA0, 0xC0, 0x00, 0x03, 0x07, 0x0F, 0x1F, 0x0D, + 0x0D, 0x04, 0x05, 0x0D, 0x05, 0x03, 0x05, 0x03, 0x03, 0x02, 0xC2, 0xAF, 0x30, 0x63, 0x08, 0xC2, + 0x63, 0x85, 0x79, 0x8A, 0xD2, 0xAF, 0x32, 0xC0, 0xD0, 0xC0, 0xE0, 0x20, 0x62, 0x21, 0xE5, 0x25, + 0x60, 0x16, 0xE4, 0x73, 0xE5, 0x25, 0xF4, 0xC3, 0x33, 0x40, 0x09, 0x75, 0x8A, 0x00, 0xD2, 0x63, + 0xF5, 0x79, 0x01, 0xD6, 0xF5, 0x8A, 0xD2, 0x62, 0xD0, 0xE0, 0xD0, 0xD0, 0xD2, 0xAF, 0x32, 0xC3, + 0xE5, 0x26, 0x33, 0x40, 0x09, 0x75, 0x8A, 0x00, 0xD2, 0x63, 0xF5, 0x79, 0x01, 0xF0, 0xF5, 0x8A, + 0xC2, 0x62, 0xE5, 0x26, 0xF4, 0x60, 0x2F, 0x30, 0x68, 0x25, 0x20, 0x72, 0x0D, 0xD0, 0xE0, 0xD0, + 0xD0, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, 0xD2, 0xAF, 0x32, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, + 0x20, 0x6F, 0x0C, 0x20, 0x66, 0x09, 0x74, 0x06, 0xD5, 0xE0, 0xFD, 0xE5, 0x7B, 0x42, 0x90, 0xD0, + 0xE0, 0xD0, 0xD0, 0xD2, 0xAF, 0x32, 0x75, 0x8A, 0x00, 0xD2, 0x63, 0x75, 0x79, 0x00, 0xC2, 0x8D, + 0xD2, 0x62, 0x21, 0x1F, 0x01, 0xC4, 0x30, 0x68, 0x05, 0x20, 0x66, 0x02, 0xD2, 0x93, 0x01, 0xC4, + 0x30, 0x68, 0x05, 0x20, 0x66, 0x02, 0xD2, 0x94, 0x01, 0xC4, 0x30, 0x68, 0x05, 0x20, 0x66, 0x02, + 0xD2, 0x97, 0x01, 0xC4, 0xC2, 0x92, 0x30, 0x68, 0x0A, 0x20, 0x66, 0x07, 0x74, 0x06, 0xD5, 0xE0, + 0xFD, 0xD2, 0x93, 0x01, 0xC4, 0xC2, 0x95, 0x30, 0x68, 0x0A, 0x20, 0x66, 0x07, 0x74, 0x06, 0xD5, + 0xE0, 0xFD, 0xD2, 0x94, 0x01, 0xC4, 0xC2, 0x96, 0x30, 0x68, 0x0A, 0x20, 0x66, 0x07, 0x74, 0x06, + 0xD5, 0xE0, 0xFD, 0xD2, 0x97, 0x01, 0xC4, 0xC2, 0xAF, 0xC2, 0xAD, 0x53, 0xE6, 0xEF, 0xC0, 0xD0, + 0xC0, 0xE0, 0xD2, 0xD3, 0xD2, 0xAF, 0xE5, 0x7A, 0x60, 0x09, 0xE5, 0x77, 0x60, 0x05, 0x75, 0x77, + 0x00, 0x41, 0xDE, 0x75, 0x77, 0x01, 0xC2, 0xCE, 0xE5, 0x2A, 0x60, 0x07, 0x20, 0x74, 0x49, 0x15, + 0x2A, 0x21, 0xF8, 0x78, 0x00, 0x79, 0x00, 0x20, 0x74, 0x2E, 0xE5, 0x80, 0x30, 0x7E, 0x01, 0xF4, + 0x30, 0xE5, 0x02, 0x78, 0xFF, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, 0x7E, + 0x03, 0x43, 0xDA, 0x10, 0xC2, 0xD8, 0xC2, 0x71, 0xE5, 0x80, 0x30, 0x7E, 0x01, 0xF4, 0x30, 0xE5, + 0x02, 0x79, 0xFF, 0xC3, 0xE8, 0x99, 0x70, 0xCB, 0x30, 0x61, 0x03, 0x75, 0x2A, 0x18, 0x20, 0x74, + 0x03, 0x75, 0x2A, 0x18, 0x88, 0x5C, 0xD2, 0x70, 0x20, 0x74, 0x0D, 0xE5, 0x2B, 0x60, 0x04, 0x15, + 0x2B, 0x41, 0x08, 0x43, 0xDA, 0x01, 0xC2, 0xD8, 0x20, 0x70, 0x02, 0x41, 0x77, 0xA8, 0x5C, 0x20, + 0x61, 0x02, 0xC2, 0x70, 0x20, 0x74, 0x36, 0x79, 0x82, 0xB7, 0x04, 0x31, 0xC3, 0xE8, 0x94, 0xF0, + 0x40, 0x03, 0x74, 0xF0, 0xF8, 0xE8, 0xC4, 0x54, 0x0F, 0x28, 0xF8, 0x79, 0x84, 0xB7, 0x03, 0x02, + 0x41, 0x4D, 0xC3, 0x13, 0xC3, 0x13, 0x87, 0x21, 0x20, 0x08, 0x02, 0xC3, 0x13, 0x20, 0x0A, 0x06, + 0xC3, 0xC8, 0x98, 0xF8, 0x41, 0x4D, 0x28, 0xF8, 0x50, 0x03, 0x74, 0xFF, 0xF8, 0x88, 0x22, 0xE5, + 0x2D, 0x54, 0x06, 0x60, 0x22, 0x20, 0x6B, 0x1F, 0xE5, 0x36, 0xC3, 0x33, 0xC3, 0x33, 0xF8, 0xE5, + 0x64, 0xC3, 0x13, 0xC3, 0x13, 0xF9, 0xC3, 0x95, 0x22, 0x40, 0x02, 0x89, 0x22, 0xE8, 0x25, 0x22, + 0xF5, 0x22, 0x50, 0x03, 0x75, 0x22, 0xFF, 0x78, 0x82, 0xB6, 0x04, 0x59, 0x85, 0x22, 0x24, 0xA8, + 0x24, 0xC3, 0xE5, 0x24, 0x95, 0x61, 0x40, 0x02, 0xA8, 0x61, 0xC3, 0xE8, 0x95, 0x63, 0x40, 0x02, + 0xA8, 0x63, 0x88, 0x25, 0xE5, 0x66, 0x70, 0x02, 0x41, 0xC8, 0xC3, 0xE8, 0xAA, 0x66, 0x9A, 0x50, + 0x03, 0xE8, 0xFA, 0xE4, 0xF9, 0xEA, 0xC3, 0x33, 0xFB, 0xE5, 0x68, 0xF4, 0x5B, 0x29, 0x40, 0x0D, + 0x25, 0x67, 0xF8, 0xE5, 0x67, 0x60, 0x02, 0x15, 0x67, 0x40, 0x0B, 0x41, 0xC8, 0x0A, 0xC3, 0xE5, + 0x67, 0x9A, 0x50, 0x02, 0x05, 0x67, 0x78, 0xFF, 0x88, 0x26, 0xC2, 0x6F, 0xC3, 0xE5, 0x26, 0x94, + 0xF8, 0x40, 0x02, 0xD2, 0x6F, 0xC3, 0xE5, 0x25, 0x94, 0x40, 0x40, 0x02, 0xD2, 0x64, 0x20, 0xCF, + 0x0A, 0xD0, 0xE0, 0xD0, 0xD0, 0x43, 0xE6, 0x10, 0xD2, 0xAD, 0x32, 0xC2, 0xCF, 0x05, 0x3A, 0xE5, + 0x7A, 0x60, 0x09, 0xE5, 0x78, 0x60, 0x05, 0x75, 0x78, 0x00, 0x61, 0x27, 0x75, 0x78, 0x01, 0x78, + 0x01, 0xE5, 0x2A, 0x60, 0x05, 0x30, 0x74, 0x02, 0x15, 0x2A, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, + 0x05, 0x75, 0x5F, 0x00, 0x61, 0x27, 0x75, 0x60, 0x00, 0x75, 0x69, 0x00, 0xE5, 0x5F, 0x24, 0x01, + 0xF5, 0x5F, 0x50, 0x03, 0x75, 0x5F, 0xFF, 0xD0, 0xE0, 0xD0, 0xD0, 0x43, 0xE6, 0x10, 0xD2, 0xAD, + 0x32, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, 0x75, 0x92, 0xFA, 0x75, 0x93, 0xFF, 0xC2, 0x60, 0x75, 0x91, + 0x04, 0xD2, 0xAF, 0x32, 0xC2, 0xAF, 0x53, 0xE6, 0xEF, 0xC2, 0xAD, 0xC0, 0xD0, 0xC0, 0xE0, 0xC0, + 0xF0, 0xD2, 0xD3, 0xD2, 0xAF, 0xA8, 0xFB, 0xA9, 0xFC, 0xE5, 0x7A, 0x60, 0x07, 0xC3, 0xE9, 0x13, + 0xF9, 0xE8, 0x13, 0xF8, 0xC2, 0xD8, 0x30, 0x71, 0x02, 0x61, 0xB6, 0x53, 0xDA, 0xCF, 0x20, 0x7E, + 0x03, 0x43, 0xDA, 0x10, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0xD2, 0x71, 0xE5, 0x80, 0x30, 0x7E, + 0x01, 0xF4, 0x20, 0xE5, 0x02, 0x61, 0x8D, 0x88, 0x27, 0x89, 0x28, 0xA1, 0xE3, 0x53, 0xDA, 0xCF, + 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x10, 0xC2, 0xD8, 0xC2, 0x71, + 0x30, 0x74, 0x02, 0xA1, 0xCF, 0x78, 0x00, 0xE5, 0x80, 0x30, 0x7E, 0x01, 0xF4, 0x30, 0xE5, 0x02, + 0xA1, 0xCF, 0x88, 0x5C, 0xA1, 0xB7, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, + 0x7E, 0x03, 0x43, 0xDA, 0x10, 0xC2, 0x71, 0x20, 0x61, 0x02, 0x81, 0x8F, 0x53, 0xDA, 0xCF, 0x20, + 0x7E, 0x03, 0x43, 0xDA, 0x10, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0xC2, 0xD8, 0xD2, 0x71, 0x88, + 0x57, 0x89, 0x58, 0xC3, 0xE8, 0x95, 0x55, 0xF8, 0xE9, 0x95, 0x56, 0xF9, 0x7B, 0x00, 0x7E, 0x08, + 0x7A, 0x00, 0xC3, 0xE8, 0x94, 0x8C, 0xE9, 0x94, 0x00, 0x50, 0x05, 0x75, 0x5B, 0x00, 0x81, 0x81, + 0x88, 0x21, 0x78, 0xA2, 0xE6, 0xA8, 0x21, 0x60, 0x55, 0xC3, 0xE8, 0x94, 0xC8, 0xE9, 0x94, 0x00, + 0x50, 0x08, 0xE4, 0xD2, 0xE4, 0xFB, 0x7A, 0x0A, 0x81, 0x5C, 0xC3, 0xE8, 0x94, 0x68, 0xE9, 0x94, + 0x01, 0x50, 0x08, 0xE4, 0xD2, 0xE3, 0xFB, 0x7A, 0x0F, 0x81, 0x5C, 0xC3, 0xE8, 0x94, 0xD0, 0xE9, + 0x94, 0x02, 0x50, 0x08, 0xE4, 0xD2, 0xE2, 0xFB, 0x7A, 0x1E, 0x81, 0x5C, 0xC3, 0xE8, 0x94, 0xA0, + 0xE9, 0x94, 0x05, 0x50, 0x08, 0xE4, 0xD2, 0xE1, 0xFB, 0x7A, 0x3C, 0x81, 0x5C, 0xC3, 0xE8, 0x94, + 0x98, 0xE9, 0x94, 0x08, 0x50, 0x08, 0xE4, 0xD2, 0xE0, 0xFB, 0x7A, 0x78, 0x7E, 0x00, 0xC3, 0xE8, + 0x95, 0x59, 0xFC, 0xE9, 0x95, 0x5A, 0xFD, 0x30, 0xE7, 0x0A, 0xEC, 0xF4, 0x24, 0x01, 0xFC, 0xED, + 0xF4, 0x34, 0x00, 0xFD, 0x75, 0x5B, 0x00, 0xC3, 0xEC, 0x9A, 0xED, 0x9E, 0x50, 0x03, 0x75, 0x5B, + 0x01, 0x88, 0x59, 0x89, 0x5A, 0x85, 0x57, 0x55, 0x85, 0x58, 0x56, 0x78, 0x02, 0xA1, 0xB7, 0xC3, + 0xE8, 0x95, 0x27, 0xF8, 0xE9, 0x95, 0x28, 0xF9, 0x30, 0x7C, 0x02, 0xA1, 0x9B, 0x30, 0x7B, 0x02, + 0xA1, 0x9B, 0x30, 0x7A, 0x02, 0xA1, 0x94, 0x20, 0x75, 0x02, 0x81, 0xB2, 0xE9, 0xFD, 0xE8, 0xFC, + 0x81, 0xD1, 0xE9, 0xC3, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0x30, 0x79, 0x02, 0xA1, 0x94, 0xE9, 0xC3, + 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0x30, 0x78, 0x02, 0xA1, 0x94, 0xE9, 0xC3, 0x13, 0xFD, 0xE8, 0x13, + 0xFC, 0x20, 0x61, 0x2C, 0xC3, 0xEC, 0x94, 0x1C, 0xED, 0x94, 0x02, 0x40, 0x02, 0x81, 0xE8, 0xED, + 0x70, 0x1E, 0xC3, 0xEC, 0x94, 0xC8, 0x50, 0x18, 0x05, 0x29, 0xE5, 0x29, 0x70, 0x02, 0x15, 0x29, + 0xC3, 0xE5, 0x29, 0x94, 0x0A, 0x50, 0x02, 0xA1, 0xCF, 0x75, 0x5C, 0x00, 0xD2, 0x70, 0xA1, 0xCF, + 0xE5, 0x29, 0x60, 0x02, 0x15, 0x29, 0x78, 0x88, 0xE6, 0xF9, 0x74, 0x00, 0x20, 0x7F, 0x08, 0x78, + 0x96, 0xB9, 0x03, 0x02, 0x78, 0x9E, 0xE6, 0x24, 0xFA, 0xFE, 0xE4, 0x34, 0x00, 0xFF, 0xC3, 0xEC, + 0x9E, 0xFC, 0xED, 0x9F, 0xFD, 0x92, 0x08, 0xB9, 0x03, 0x10, 0xA2, 0x08, 0x50, 0x07, 0x20, 0x76, + 0x09, 0xD2, 0x76, 0xA1, 0x3A, 0x30, 0x76, 0x02, 0xC2, 0x76, 0xA2, 0x08, 0x50, 0x16, 0xB9, 0x03, + 0x0D, 0xEC, 0xF4, 0x24, 0x01, 0xFC, 0xED, 0xF4, 0x34, 0x00, 0xFD, 0x02, 0x05, 0x54, 0x78, 0x00, + 0x79, 0x00, 0xA1, 0x9B, 0xB9, 0x03, 0x15, 0xEC, 0x33, 0xFC, 0xED, 0x33, 0xFD, 0xC3, 0xEC, 0x94, + 0x0A, 0xFC, 0xED, 0x94, 0x00, 0xFD, 0x50, 0x04, 0x7C, 0x00, 0x7D, 0x00, 0xC3, 0xEC, 0x94, 0xFF, + 0xED, 0x94, 0x00, 0x40, 0x06, 0x78, 0xFF, 0x79, 0x00, 0xA1, 0x9B, 0xEC, 0x85, 0x72, 0xF0, 0xA4, + 0xC5, 0xF0, 0xA2, 0xF7, 0x33, 0xF8, 0x79, 0x00, 0x40, 0x03, 0x02, 0x05, 0xB7, 0x78, 0xFF, 0x79, + 0x00, 0x02, 0x05, 0xB7, 0xE9, 0xC3, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0x30, 0x7C, 0x0E, 0xE9, 0x60, + 0x02, 0x78, 0xFF, 0xC3, 0xE8, 0x13, 0x38, 0xF8, 0xE4, 0x34, 0x00, 0xF9, 0xC3, 0xE8, 0x94, 0xFF, + 0xE9, 0x94, 0x00, 0x40, 0x02, 0x78, 0xFF, 0x88, 0x5C, 0xD2, 0x70, 0x20, 0x61, 0x02, 0xA1, 0xCF, + 0x74, 0x1F, 0xF4, 0x55, 0x2F, 0x4B, 0xF5, 0x2F, 0xC2, 0x74, 0xEB, 0x70, 0x02, 0xD2, 0x74, 0x75, + 0x2A, 0x18, 0x30, 0x74, 0x03, 0x75, 0x2A, 0x0A, 0x30, 0x61, 0x02, 0xA1, 0xE3, 0x20, 0x74, 0x03, + 0x53, 0xDA, 0xFE, 0x20, 0x74, 0x03, 0x75, 0x2B, 0x06, 0xD0, 0xF0, 0xD0, 0xE0, 0xD0, 0xD0, 0xD2, + 0xAD, 0x43, 0xE6, 0x10, 0x32, 0x79, 0x01, 0x02, 0x06, 0x13, 0x79, 0x03, 0x02, 0x06, 0x13, 0x79, + 0x0A, 0x02, 0x06, 0x13, 0x79, 0x1E, 0x02, 0x06, 0x13, 0x79, 0x64, 0x02, 0x06, 0x13, 0x79, 0xC8, + 0x02, 0x06, 0x13, 0x78, 0x17, 0xE4, 0xD5, 0xE0, 0xFD, 0xD8, 0xFA, 0xD9, 0xF6, 0x22, 0x7A, 0x14, + 0x7B, 0x78, 0x02, 0x06, 0x3A, 0x7A, 0x10, 0x7B, 0x8C, 0x02, 0x06, 0x3A, 0x7A, 0x0D, 0x7B, 0xB4, + 0x02, 0x06, 0x3A, 0x7A, 0x0B, 0x7B, 0xC8, 0x02, 0x06, 0x3A, 0x79, 0x02, 0xE4, 0xC2, 0x95, 0xD5, + 0xE0, 0xFD, 0xD2, 0x94, 0xD5, 0xE0, 0xFD, 0xC2, 0x94, 0xD5, 0xE0, 0xFD, 0xD2, 0x95, 0xD5, 0xE0, + 0xFD, 0xE9, 0x20, 0xE0, 0x02, 0xD2, 0x93, 0x30, 0xE0, 0x02, 0xD2, 0x97, 0xE5, 0x73, 0xD5, 0xE0, + 0xFD, 0xE9, 0x20, 0xE0, 0x02, 0xC2, 0x93, 0x30, 0xE0, 0x02, 0xC2, 0x97, 0x74, 0x96, 0xD5, 0xE0, + 0xFD, 0xD9, 0xC9, 0xEA, 0xF8, 0xD5, 0xE0, 0xFD, 0xD8, 0xFB, 0xDB, 0xBE, 0xC2, 0x95, 0x22, 0xC3, + 0x7C, 0x00, 0x7D, 0x00, 0x75, 0xF0, 0x00, 0x05, 0xF0, 0xEA, 0x33, 0xFA, 0xEB, 0x33, 0xFB, 0x50, + 0xF6, 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xC3, 0xE9, 0xFF, 0xE8, 0xFE, 0xE8, 0x9A, 0xF8, 0xE9, + 0x9B, 0xF9, 0x50, 0x04, 0xEF, 0xF9, 0xEE, 0xF8, 0xB3, 0xEC, 0x33, 0xFC, 0xED, 0x33, 0xFD, 0xD5, + 0xF0, 0xDF, 0xED, 0xF9, 0xEC, 0xF8, 0x22, 0xE8, 0x89, 0xF0, 0x8A, 0x20, 0xD2, 0xD4, 0xF8, 0xA9, + 0xF0, 0x7B, 0x00, 0x30, 0xF7, 0x0B, 0x7B, 0xFF, 0xF4, 0x24, 0x01, 0xF8, 0xE9, 0xF4, 0x34, 0x00, + 0xF9, 0xE8, 0x85, 0x20, 0xF0, 0xA4, 0xAD, 0xF0, 0xF8, 0xE9, 0x85, 0x20, 0xF0, 0xA4, 0xAF, 0xF0, + 0xFE, 0xED, 0x2E, 0xF9, 0x74, 0x00, 0x3F, 0xFA, 0x7C, 0x04, 0xC3, 0xEA, 0x13, 0xFA, 0xE9, 0x13, + 0xF9, 0xE8, 0x13, 0xF8, 0xDC, 0xF4, 0x8B, 0xF0, 0x30, 0xF7, 0x0A, 0xE8, 0xF4, 0x24, 0x01, 0xF8, + 0xE9, 0xF4, 0x34, 0x00, 0xF9, 0xE8, 0x89, 0xF0, 0xC2, 0xD4, 0xF8, 0xA9, 0xF0, 0x22, 0x78, 0x82, + 0xB6, 0x04, 0x03, 0x02, 0x07, 0x69, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x03, 0x02, 0x07, 0x33, + 0x85, 0x22, 0x24, 0xE4, 0xF5, 0x44, 0xF5, 0x45, 0xF5, 0x46, 0xF5, 0x47, 0xF5, 0x48, 0xC2, 0x6E, + 0x02, 0x07, 0x69, 0x78, 0x82, 0xE6, 0xFC, 0xD2, 0x6E, 0xE5, 0x22, 0xF5, 0x23, 0x78, 0x38, 0x79, + 0xC7, 0xAA, 0x40, 0xAB, 0x41, 0xC3, 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xEC, 0x14, 0x60, 0x13, + 0xC3, 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xEC, 0x14, 0x14, 0x60, 0x07, 0xC3, 0xEB, 0x13, 0xFB, + 0xEA, 0x13, 0xFA, 0xD1, 0x7F, 0x88, 0x44, 0x89, 0x45, 0x22, 0xC3, 0xE5, 0x44, 0x95, 0x23, 0xF8, + 0xE5, 0x45, 0x94, 0x00, 0xF9, 0x50, 0x0C, 0xC3, 0xE8, 0x94, 0x80, 0xE9, 0x94, 0xFF, 0x40, 0x16, + 0x02, 0x07, 0x9A, 0xC3, 0xE8, 0x94, 0x7F, 0xE9, 0x94, 0x00, 0x50, 0x03, 0x02, 0x07, 0x9A, 0x78, + 0x7F, 0x79, 0x00, 0x02, 0x07, 0x9A, 0x78, 0x80, 0x79, 0xFF, 0x88, 0x49, 0x89, 0x4A, 0x22, 0xE5, + 0x49, 0x25, 0x46, 0xF8, 0xE5, 0x4A, 0x35, 0x47, 0xF9, 0x85, 0x4A, 0x20, 0xE4, 0x30, 0x07, 0x01, + 0xF4, 0x35, 0x48, 0xFA, 0x30, 0xE7, 0x09, 0xC3, 0xEA, 0x94, 0xF0, 0x40, 0x15, 0x02, 0x07, 0xD8, + 0xC3, 0xEA, 0x94, 0x0F, 0x50, 0x03, 0x02, 0x07, 0xD8, 0x78, 0xFF, 0x79, 0xFF, 0x7A, 0x0F, 0x02, + 0x07, 0xD8, 0x78, 0x00, 0x79, 0x00, 0x7A, 0xF0, 0xC3, 0xE5, 0x24, 0x95, 0x61, 0x50, 0x0A, 0xC3, + 0x74, 0x01, 0x95, 0x24, 0x50, 0x0B, 0x02, 0x07, 0xF6, 0xE5, 0x4A, 0x20, 0xE7, 0x0E, 0x02, 0x07, + 0xF6, 0xE5, 0x4A, 0x30, 0xE7, 0x06, 0x88, 0x46, 0x89, 0x47, 0x8A, 0x48, 0x22, 0x78, 0xA5, 0xE6, + 0xFA, 0xC3, 0xE5, 0x49, 0x33, 0xF8, 0xE5, 0x4A, 0x33, 0xF9, 0x12, 0x06, 0xB7, 0xE9, 0x30, 0xE7, + 0x0B, 0xC3, 0xE8, 0x94, 0x80, 0xE9, 0x94, 0xFF, 0x40, 0x13, 0x01, 0x31, 0xC3, 0xE8, 0x94, 0x7F, + 0xE9, 0x94, 0x00, 0x50, 0x02, 0x01, 0x31, 0x78, 0x7F, 0x79, 0x00, 0x01, 0x31, 0x78, 0x80, 0x79, + 0xFF, 0xE8, 0x20, 0xE7, 0x15, 0xC3, 0xE5, 0x23, 0x98, 0xF8, 0x40, 0x09, 0xC3, 0xE8, 0x94, 0x01, + 0x40, 0x03, 0x02, 0x08, 0x58, 0x78, 0x01, 0x02, 0x08, 0x58, 0xE8, 0xF4, 0x24, 0x01, 0x25, 0x23, + 0xF8, 0x40, 0x03, 0x02, 0x08, 0x58, 0x78, 0xFF, 0x88, 0x4B, 0x22, 0x78, 0xA6, 0xE6, 0xFA, 0xA8, + 0x47, 0xA9, 0x48, 0x12, 0x06, 0xB7, 0xE9, 0x30, 0xE7, 0x0C, 0xC3, 0xE8, 0x94, 0x01, 0xE9, 0x94, + 0xFF, 0x40, 0x16, 0x02, 0x08, 0x8D, 0xC3, 0xE8, 0x94, 0xFF, 0xE9, 0x94, 0x00, 0x50, 0x03, 0x02, + 0x08, 0x8D, 0x78, 0xFF, 0x79, 0x00, 0x02, 0x08, 0x8D, 0x78, 0x01, 0x79, 0xFF, 0xE9, 0x20, 0xE7, + 0x15, 0xC3, 0xE5, 0x4B, 0x98, 0xF8, 0x40, 0x09, 0xC3, 0xE8, 0x94, 0x01, 0x40, 0x03, 0x02, 0x08, + 0xB4, 0x78, 0x01, 0x02, 0x08, 0xB4, 0xE8, 0xF4, 0x24, 0x01, 0x25, 0x4B, 0xF8, 0x40, 0x03, 0x02, + 0x08, 0xB4, 0x78, 0xFF, 0x88, 0x24, 0x22, 0x78, 0xFF, 0xC2, 0x64, 0x20, 0x69, 0x3D, 0x20, 0x6A, + 0x12, 0xD2, 0x64, 0xC3, 0xE5, 0x41, 0x94, 0x0A, 0x40, 0x09, 0xC3, 0xE5, 0x25, 0x94, 0x40, 0x50, + 0x02, 0xC2, 0x64, 0x79, 0xA1, 0xE7, 0x60, 0x23, 0xE5, 0x41, 0x60, 0x1F, 0x74, 0xFF, 0x85, 0x41, + 0xF0, 0x84, 0x85, 0x39, 0xF0, 0x30, 0x6A, 0x03, 0x75, 0xF0, 0x05, 0xA4, 0xF8, 0xC5, 0xF0, 0x60, + 0x02, 0x78, 0xFF, 0xC3, 0xE8, 0x95, 0x64, 0x50, 0x02, 0xA8, 0x64, 0x88, 0x63, 0x22, 0xC3, 0xE5, + 0x40, 0x94, 0xC8, 0xE5, 0x41, 0x94, 0x00, 0xE5, 0x63, 0x50, 0x03, 0x14, 0x21, 0x0F, 0x04, 0x60, + 0x02, 0xF5, 0x63, 0x22, 0x02, 0x09, 0x17, 0x22, 0x75, 0xE8, 0x90, 0x22, 0x78, 0x83, 0xE6, 0xFF, + 0xE5, 0xE8, 0x20, 0xEC, 0xF7, 0xA8, 0xBD, 0xA9, 0xBE, 0x05, 0x70, 0xC3, 0xE5, 0x70, 0x94, 0x08, + 0x40, 0x52, 0x75, 0x70, 0x00, 0xE9, 0xFA, 0x79, 0xA0, 0xE7, 0x60, 0x44, 0xEA, 0x70, 0x07, 0xE5, + 0x71, 0x60, 0x1B, 0x02, 0x09, 0x52, 0xC3, 0xE8, 0x95, 0x71, 0x60, 0x10, 0xE5, 0x71, 0x50, 0x06, + 0x60, 0x0C, 0x14, 0x02, 0x09, 0x5E, 0x04, 0x60, 0xF9, 0x02, 0x09, 0x5E, 0xE5, 0x71, 0xF5, 0x71, + 0xC3, 0x94, 0x72, 0x40, 0x1B, 0x75, 0x61, 0xC0, 0xC3, 0x94, 0x04, 0x40, 0x13, 0x75, 0x61, 0x80, + 0xC3, 0x94, 0x04, 0x40, 0x0B, 0x75, 0x61, 0x40, 0xC3, 0x94, 0x04, 0x40, 0x03, 0x75, 0x61, 0x00, + 0x75, 0xBB, 0x09, 0x22, 0xE5, 0x61, 0x24, 0x10, 0x50, 0x02, 0x74, 0xFF, 0xF5, 0x61, 0x79, 0x82, + 0xB7, 0x04, 0x02, 0x21, 0xAB, 0xC3, 0xA8, 0x61, 0xE5, 0x24, 0x98, 0x50, 0x02, 0xA8, 0x24, 0xC3, + 0xE8, 0x95, 0x63, 0x40, 0x02, 0xA8, 0x63, 0x88, 0x25, 0x88, 0x26, 0xE5, 0x70, 0xB4, 0x07, 0x03, + 0x75, 0xBB, 0x10, 0x22, 0x74, 0x32, 0x79, 0xA7, 0x87, 0xF0, 0xA4, 0xC5, 0xF0, 0xA2, 0xF7, 0x33, + 0xF8, 0xC3, 0xE8, 0x95, 0x61, 0x40, 0x02, 0xA8, 0x61, 0x88, 0x22, 0x88, 0x24, 0x88, 0x25, 0x88, + 0x26, 0x88, 0x64, 0x22, 0x75, 0x40, 0x00, 0x75, 0x41, 0xF0, 0x22, 0xC2, 0xAF, 0x75, 0xC8, 0x20, + 0xA8, 0xCC, 0xA9, 0xCD, 0xAA, 0x3A, 0x30, 0xCF, 0x01, 0x0A, 0x75, 0xC8, 0x24, 0xD2, 0xAF, 0xC3, + 0xEA, 0x13, 0xFA, 0xE9, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0xAB, 0x3B, 0xAC, 0x3C, 0x88, 0x3B, 0x89, + 0x3C, 0xC3, 0xE8, 0x9B, 0xF8, 0xE9, 0x9C, 0x20, 0x69, 0x0A, 0x54, 0x7F, 0xF9, 0x30, 0x67, 0x02, + 0x41, 0xF5, 0x41, 0x52, 0xAD, 0x3D, 0x8A, 0x3D, 0xF9, 0xEA, 0x9D, 0x54, 0x7F, 0xFA, 0x60, 0x04, + 0x78, 0xFF, 0x79, 0xFF, 0xAE, 0x3E, 0xAF, 0x3F, 0x8B, 0x3E, 0x8C, 0x3F, 0xA8, 0x3B, 0xA9, 0x3C, + 0xC3, 0xE8, 0x9E, 0xF8, 0xE9, 0x9F, 0xF9, 0xC3, 0xE5, 0x41, 0x13, 0xFB, 0xE5, 0x40, 0x13, 0xFA, + 0xE8, 0x2A, 0xF5, 0x40, 0xE9, 0x3B, 0xF5, 0x41, 0x50, 0x06, 0x75, 0x40, 0xFF, 0x75, 0x41, 0xFF, + 0x41, 0x9C, 0xAA, 0x40, 0xAB, 0x41, 0xAC, 0x40, 0xAD, 0x41, 0x7E, 0x04, 0x7F, 0x02, 0xC3, 0xEB, + 0x94, 0x04, 0x40, 0x02, 0x1E, 0x1F, 0xC3, 0xEB, 0x94, 0x08, 0x40, 0x02, 0x1E, 0x1F, 0xC3, 0xED, + 0x13, 0xFD, 0xEC, 0x13, 0xFC, 0xDE, 0xF7, 0xC3, 0xEA, 0x9C, 0xFA, 0xEB, 0x9D, 0xFB, 0xEF, 0x60, + 0x09, 0xC3, 0xE9, 0x13, 0xF9, 0xE8, 0x13, 0xF8, 0xDF, 0xF7, 0xEA, 0x28, 0xFA, 0xEB, 0x39, 0xFB, + 0x8A, 0x40, 0x8B, 0x41, 0x50, 0x06, 0x7B, 0xFF, 0x8B, 0x40, 0x8B, 0x41, 0xC3, 0xEB, 0x94, 0x02, + 0x50, 0x02, 0xD2, 0x67, 0x30, 0x69, 0x04, 0x7F, 0x03, 0x41, 0xC7, 0x78, 0x92, 0xE6, 0xFF, 0xC3, + 0xE5, 0x37, 0x94, 0x82, 0x40, 0x11, 0x0F, 0xC3, 0xE5, 0x37, 0x94, 0xA0, 0x40, 0x01, 0x0F, 0xC3, + 0xEF, 0x94, 0x06, 0x40, 0x02, 0x7F, 0x05, 0x7E, 0x02, 0xE5, 0x41, 0xC4, 0x54, 0x0F, 0xF9, 0xE5, + 0x41, 0xC4, 0x54, 0xF0, 0xF8, 0xE5, 0x40, 0xC4, 0x54, 0x0F, 0x28, 0xF8, 0xC3, 0xE8, 0x9E, 0xFA, + 0xE9, 0x94, 0x00, 0xFB, 0x40, 0x09, 0xC3, 0xEA, 0x94, 0x02, 0xEB, 0x94, 0x00, 0x50, 0x04, 0x7A, + 0x02, 0xE4, 0xFB, 0x61, 0x41, 0xAA, 0x40, 0xAB, 0x41, 0xEB, 0xC4, 0xFE, 0xEA, 0xC4, 0x54, 0x0F, + 0x4E, 0xFC, 0xC3, 0xEA, 0x9C, 0xFA, 0xEB, 0x94, 0x00, 0xFB, 0xC3, 0xE8, 0x13, 0xC3, 0x13, 0xF8, + 0xEA, 0x28, 0xFA, 0xEB, 0x34, 0x00, 0xFB, 0x8A, 0x40, 0x8B, 0x41, 0xC3, 0xEB, 0x94, 0x02, 0x40, + 0x02, 0xC2, 0x67, 0x78, 0x02, 0xEB, 0xC4, 0xFE, 0x7B, 0x00, 0xEA, 0xC4, 0x54, 0x0F, 0x4E, 0xFA, + 0xC3, 0xEA, 0x98, 0xFA, 0x40, 0x05, 0xC3, 0x94, 0x02, 0x50, 0x02, 0x7A, 0x02, 0x78, 0x92, 0xE6, + 0xFF, 0x30, 0x60, 0x02, 0x61, 0x41, 0x85, 0x51, 0x92, 0x85, 0x52, 0x93, 0xD2, 0x60, 0x43, 0xE6, + 0x80, 0xC3, 0xE4, 0x9A, 0xF8, 0xE4, 0x9B, 0xF9, 0xC3, 0xE8, 0x33, 0xF8, 0xE9, 0x33, 0xF9, 0x30, + 0x67, 0x02, 0x61, 0xD4, 0xE8, 0xFA, 0xE9, 0xFB, 0xD3, 0xE9, 0x13, 0xFD, 0xE8, 0x13, 0xFC, 0x88, + 0x51, 0x89, 0x52, 0xC3, 0xEF, 0x94, 0x03, 0x60, 0x38, 0xEF, 0x20, 0xE0, 0x0D, 0xE8, 0x2C, 0xF8, + 0xE9, 0x3D, 0xF9, 0xEC, 0xFA, 0xED, 0xFB, 0x02, 0x0B, 0x9D, 0xE8, 0x28, 0xF8, 0xE9, 0x39, 0xF9, + 0xC3, 0xE8, 0x24, 0x02, 0xF8, 0xE9, 0x34, 0x00, 0xF9, 0x7A, 0xFE, 0x7B, 0xFF, 0xC3, 0xEF, 0x94, + 0x03, 0x40, 0x0E, 0x8A, 0x53, 0x8B, 0x54, 0x88, 0x4D, 0x89, 0x4E, 0x8C, 0x4F, 0x8D, 0x50, 0x81, + 0x08, 0x88, 0x53, 0x89, 0x54, 0x8A, 0x4D, 0x8B, 0x4E, 0x8C, 0x4F, 0x8D, 0x50, 0x30, 0x69, 0x12, + 0x75, 0x53, 0xF0, 0x75, 0x54, 0xFF, 0x75, 0x4F, 0xF0, 0x75, 0x50, 0xFF, 0x75, 0x51, 0xF0, 0x75, + 0x52, 0xFF, 0x81, 0x08, 0xE8, 0xFA, 0xD3, 0xE8, 0x13, 0xFC, 0x88, 0x51, 0xC3, 0xEF, 0x94, 0x03, + 0x60, 0x20, 0xEF, 0x20, 0xE0, 0x07, 0xE8, 0x2C, 0xF8, 0xEC, 0xFA, 0x61, 0xF4, 0xE8, 0x28, 0x24, + 0x02, 0xF8, 0x7A, 0xFE, 0xC3, 0xEF, 0x94, 0x03, 0x40, 0x08, 0x8A, 0x53, 0x88, 0x4D, 0x8C, 0x4F, + 0x81, 0x08, 0x88, 0x53, 0x8A, 0x4D, 0x8C, 0x4F, 0xE5, 0x68, 0xC3, 0x33, 0x50, 0x02, 0x64, 0x6B, + 0xF5, 0x68, 0x30, 0x60, 0x02, 0x81, 0x12, 0x75, 0x34, 0x02, 0xD2, 0x60, 0x43, 0xE6, 0x80, 0xE5, + 0x2D, 0x54, 0x06, 0x60, 0x2D, 0xA8, 0x40, 0xA9, 0x41, 0xC3, 0xE9, 0x13, 0xF9, 0xE8, 0x13, 0xF8, + 0x30, 0x69, 0x04, 0xE9, 0x24, 0x40, 0xF9, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, 0x75, 0x91, 0x00, 0xC3, + 0xE4, 0x98, 0xF5, 0x94, 0xE4, 0x99, 0xF5, 0x95, 0x75, 0x91, 0x04, 0xD2, 0x60, 0x43, 0xE6, 0x80, + 0xD2, 0xAF, 0x22, 0xD2, 0x65, 0x75, 0x43, 0x00, 0x75, 0x20, 0x00, 0x30, 0x6C, 0x03, 0x75, 0x20, + 0x40, 0x02, 0x0C, 0x72, 0xD2, 0x65, 0x75, 0x43, 0x00, 0x75, 0x20, 0x40, 0x30, 0x6C, 0x03, 0x75, + 0x20, 0x00, 0x78, 0x01, 0x79, 0x01, 0x20, 0x67, 0x20, 0xE5, 0x2D, 0x54, 0x06, 0x60, 0x02, 0xC2, + 0x65, 0x79, 0x14, 0xE5, 0x41, 0xC3, 0x13, 0x70, 0x01, 0x04, 0xF8, 0xC3, 0x94, 0x14, 0x40, 0x02, + 0x78, 0x14, 0x30, 0x69, 0x04, 0x78, 0x1B, 0x79, 0x1B, 0xC3, 0xE8, 0x33, 0xF8, 0xC3, 0xE9, 0x33, + 0xF9, 0x20, 0x60, 0x10, 0xE5, 0x43, 0x60, 0x0C, 0x30, 0x69, 0x03, 0xD5, 0x34, 0x04, 0xD2, 0x6D, + 0xA1, 0x2A, 0x91, 0x1A, 0x05, 0x43, 0xE5, 0x9B, 0xF4, 0x54, 0x40, 0xB5, 0x20, 0x02, 0xA1, 0x14, + 0x30, 0x69, 0x09, 0x08, 0xC3, 0xE8, 0x99, 0x40, 0x01, 0x18, 0x81, 0xA1, 0x20, 0x65, 0x0A, 0x08, + 0xC3, 0xE8, 0x99, 0x40, 0x02, 0x81, 0x72, 0x81, 0xA1, 0xC2, 0x65, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, + 0x75, 0x91, 0x00, 0x30, 0x67, 0x12, 0x75, 0x94, 0x00, 0x75, 0x95, 0xFC, 0x75, 0x91, 0x04, 0xD2, + 0x60, 0x43, 0xE6, 0x80, 0xD2, 0xAF, 0x81, 0x72, 0xAE, 0x40, 0xAF, 0x41, 0xC3, 0xEE, 0x33, 0xFE, + 0xEF, 0x33, 0xFF, 0x50, 0x04, 0x7E, 0xFF, 0x7F, 0xFF, 0xC3, 0xE4, 0x9E, 0xF5, 0x94, 0xE4, 0x9F, + 0xF5, 0x95, 0x81, 0xEC, 0xC3, 0xE5, 0x33, 0x94, 0x01, 0x50, 0x02, 0x81, 0x72, 0x30, 0x65, 0x02, + 0x81, 0x72, 0xD8, 0x02, 0xA1, 0x28, 0x81, 0xA1, 0xC2, 0x6D, 0xC2, 0xAF, 0x53, 0xE6, 0x7F, 0x75, + 0x91, 0x00, 0x85, 0x53, 0x94, 0x85, 0x54, 0x95, 0x75, 0x91, 0x04, 0x85, 0x4D, 0x92, 0x85, 0x4E, + 0x93, 0xD2, 0x60, 0x43, 0xE6, 0x80, 0xD2, 0xAF, 0xE5, 0x2D, 0x54, 0x06, 0x60, 0x08, 0x20, 0x6A, + 0x02, 0x05, 0x33, 0x02, 0x0D, 0x66, 0x30, 0x6D, 0x0D, 0x20, 0x6C, 0x0A, 0x20, 0x65, 0x07, 0x15, + 0x81, 0x15, 0x81, 0x02, 0x16, 0x03, 0x22, 0x78, 0x00, 0x30, 0x64, 0x05, 0x30, 0x65, 0x02, 0x78, + 0x01, 0xE5, 0x37, 0x75, 0xF0, 0x07, 0xA4, 0xF9, 0xE5, 0xF0, 0x28, 0xF5, 0xF0, 0xE9, 0xA2, 0xF0, + 0x13, 0xA2, 0xF1, 0x13, 0xA2, 0xF2, 0x13, 0xF5, 0x37, 0xC3, 0x94, 0x78, 0x50, 0x03, 0x75, 0x37, + 0x78, 0xC3, 0xE5, 0x37, 0x95, 0x38, 0x40, 0x08, 0xD2, 0x66, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, + 0x30, 0x60, 0x02, 0xA1, 0xA0, 0x85, 0x4F, 0x92, 0x85, 0x50, 0x93, 0xD2, 0x60, 0x43, 0xE6, 0x80, + 0x22, 0x20, 0x7D, 0x16, 0xC2, 0xAF, 0x75, 0x42, 0x02, 0xC2, 0x95, 0xD2, 0x92, 0x30, 0x62, 0x02, + 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x02, 0xC2, + 0x95, 0xD2, 0x96, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, + 0x30, 0x72, 0x43, 0x20, 0x7D, 0x20, 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, 0x65, 0x75, 0x7B, + 0x20, 0xC2, 0x97, 0xC2, 0x96, 0x30, 0x62, 0x04, 0xD2, 0x94, 0xA1, 0xFE, 0xD2, 0x95, 0xD2, 0xAF, + 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, 0x65, 0x75, 0x7B, + 0x20, 0xC2, 0x93, 0xC2, 0x92, 0x30, 0x62, 0x04, 0xD2, 0x94, 0xC1, 0x1E, 0xD2, 0x95, 0xD2, 0xAF, + 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x17, 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, + 0x40, 0xC2, 0x97, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, 0xAF, 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, + 0xC2, 0xAF, 0x75, 0x42, 0x03, 0x90, 0x01, 0x40, 0xC2, 0x93, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, + 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x16, 0xC2, 0xAF, 0x75, 0x42, 0x04, 0xC2, + 0x92, 0xD2, 0x96, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, + 0xC2, 0xAF, 0x75, 0x42, 0x04, 0xC2, 0x96, 0xD2, 0x92, 0x30, 0x62, 0x02, 0xD2, 0x94, 0xD2, 0xAF, + 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, 0x30, 0x72, 0x43, 0x20, 0x7D, 0x20, 0xC2, 0xAF, 0x75, 0x42, + 0x05, 0x90, 0x01, 0x54, 0x75, 0x7B, 0x04, 0xC2, 0x94, 0xC2, 0x95, 0x30, 0x62, 0x04, 0xD2, 0x93, + 0xC1, 0xA4, 0xD2, 0x92, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, + 0x05, 0x90, 0x01, 0x76, 0x75, 0x7B, 0x40, 0xC2, 0x94, 0xC2, 0x95, 0x30, 0x62, 0x04, 0xD2, 0x97, + 0xC1, 0xC4, 0xD2, 0x96, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x17, 0xC2, + 0xAF, 0x75, 0x42, 0x05, 0x90, 0x01, 0x36, 0xC2, 0x94, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, + 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x05, 0x90, 0x01, 0x4A, 0xC2, 0x94, + 0x30, 0x62, 0xEB, 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x80, 0x02, 0x0F, 0xA0, 0x20, 0x7D, 0x16, + 0xC2, 0xAF, 0x75, 0x42, 0x06, 0xC2, 0x96, 0xD2, 0x95, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, + 0x75, 0x9F, 0x89, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x06, 0xC2, 0x92, 0xD2, 0x95, 0x30, + 0x62, 0x02, 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0x30, 0x72, 0x43, 0x20, + 0x7D, 0x20, 0xC2, 0xAF, 0x75, 0x42, 0x01, 0x90, 0x01, 0x76, 0x75, 0x7B, 0x40, 0xC2, 0x93, 0xC2, + 0x92, 0x30, 0x62, 0x04, 0xD2, 0x97, 0xE1, 0x4A, 0xD2, 0x96, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, + 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, 0x01, 0x90, 0x01, 0x54, 0x75, 0x7B, 0x04, 0xC2, 0x97, 0xC2, + 0x96, 0x30, 0x62, 0x04, 0xD2, 0x93, 0xE1, 0x6A, 0xD2, 0x92, 0xD2, 0xAF, 0x75, 0x9F, 0x89, 0x02, + 0x0F, 0xA0, 0x20, 0x7D, 0x17, 0xC2, 0xAF, 0x75, 0x42, 0x01, 0x90, 0x01, 0x4A, 0xC2, 0x93, 0x30, + 0x62, 0x02, 0xD2, 0x97, 0xD2, 0xAF, 0x75, 0x9F, 0x81, 0x02, 0x0F, 0xA0, 0xC2, 0xAF, 0x75, 0x42, + 0x01, 0x90, 0x01, 0x36, 0xC2, 0x97, 0x30, 0x62, 0x02, 0xD2, 0x93, 0xD2, 0xAF, 0x75, 0x9F, 0x89, + 0xC2, 0x66, 0x22, 0x90, 0x01, 0x34, 0x75, 0x7B, 0x00, 0xC2, 0x93, 0xC2, 0x97, 0xC2, 0x94, 0xC2, + 0x92, 0xC2, 0x96, 0xC2, 0x95, 0xC2, 0x62, 0x22, 0x78, 0x80, 0x76, 0x09, 0x08, 0x76, 0x09, 0x08, + 0x76, 0x04, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x03, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x09, 0x08, 0x76, + 0x03, 0x08, 0x76, 0x01, 0x08, 0x76, 0x01, 0x78, 0x8C, 0x76, 0x01, 0x08, 0x76, 0xFF, 0x08, 0x76, + 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x03, 0x08, 0x76, 0xFF, + 0x08, 0x76, 0xFF, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x25, 0x08, 0x76, 0xD0, 0x08, 0x76, 0x28, 0x08, + 0x76, 0x50, 0x08, 0x76, 0x04, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x02, 0x08, 0x76, 0x00, 0x08, 0x76, + 0x7A, 0x08, 0x76, 0xFF, 0x08, 0x76, 0x01, 0x08, 0x76, 0x01, 0x08, 0x76, 0x00, 0x08, 0x76, 0x03, + 0x08, 0x76, 0x00, 0x22, 0x78, 0x87, 0xE6, 0xFF, 0xC2, 0x72, 0xBF, 0x03, 0x02, 0xD2, 0x72, 0x78, + 0x88, 0xE6, 0xC3, 0x94, 0x03, 0x60, 0x08, 0xC2, 0x7D, 0xE6, 0x30, 0xE1, 0x02, 0xD2, 0x7D, 0xC2, + 0x7E, 0x78, 0x89, 0xE6, 0x30, 0xE1, 0x02, 0xD2, 0x7E, 0xC3, 0xEF, 0x94, 0x02, 0x60, 0x08, 0x75, + 0x8E, 0x01, 0xD2, 0x73, 0x02, 0x10, 0x5C, 0x75, 0x8E, 0x00, 0xC2, 0x73, 0x22, 0x78, 0x80, 0xE6, + 0x14, 0x90, 0x00, 0x80, 0x93, 0x78, 0xA5, 0xF6, 0x78, 0x81, 0xE6, 0x14, 0x90, 0x00, 0x80, 0x93, + 0x78, 0xA6, 0xF6, 0x78, 0x86, 0xE6, 0x14, 0x90, 0x00, 0x8D, 0x93, 0x78, 0xA7, 0xF6, 0x78, 0x86, + 0xE6, 0xF5, 0x39, 0xC3, 0x94, 0x02, 0x50, 0x03, 0x75, 0x39, 0x02, 0x78, 0x9C, 0xE6, 0x75, 0x38, + 0xFF, 0xB4, 0x02, 0x03, 0x75, 0x38, 0xA0, 0xB4, 0x03, 0x03, 0x75, 0x38, 0x82, 0x78, 0xA3, 0xE6, + 0x14, 0x90, 0x00, 0x9A, 0x93, 0xF5, 0x66, 0x12, 0x0F, 0xA3, 0x22, 0x22, 0x78, 0x96, 0xE6, 0xFA, + 0x78, 0x97, 0xE6, 0xFB, 0x78, 0x88, 0xE6, 0xB4, 0x03, 0x05, 0xC3, 0xEB, 0x94, 0x0E, 0xFB, 0x30, + 0x7F, 0x04, 0x7A, 0x00, 0x7B, 0xFF, 0xC3, 0xEB, 0x9A, 0xFC, 0xC3, 0x94, 0x82, 0x50, 0x02, 0x7C, + 0x82, 0x75, 0x72, 0x00, 0x05, 0x72, 0xEC, 0x85, 0x72, 0xF0, 0xA4, 0xC3, 0xE5, 0xF0, 0x94, 0x7D, + 0x40, 0xF2, 0x22, 0xD2, 0x7F, 0x11, 0xAC, 0x12, 0x06, 0x04, 0x7A, 0x00, 0x7B, 0x00, 0x7C, 0x10, + 0x12, 0x05, 0xFA, 0xE5, 0x5C, 0x2A, 0xFA, 0x74, 0x00, 0x3B, 0xFB, 0xDC, 0xF3, 0x7C, 0x04, 0xC3, + 0xEB, 0x13, 0xFB, 0xEA, 0x13, 0xFA, 0xDC, 0xF7, 0xFE, 0xC2, 0x7F, 0x11, 0xAC, 0x22, 0xE5, 0xEF, + 0x20, 0xE6, 0x03, 0x75, 0x20, 0x00, 0x05, 0x20, 0x90, 0x3F, 0xFF, 0xE5, 0x20, 0x14, 0x60, 0x06, + 0x90, 0x1F, 0xFF, 0x14, 0x60, 0x00, 0x93, 0x04, 0x60, 0x03, 0x75, 0xEF, 0x12, 0x53, 0xD9, 0xBF, + 0x75, 0x81, 0xC0, 0x43, 0xFF, 0x80, 0x12, 0x05, 0xF5, 0x75, 0xEF, 0x02, 0x43, 0xB2, 0x03, 0xE5, + 0xB3, 0x24, 0x02, 0x20, 0xE7, 0x0D, 0xF5, 0x21, 0xE5, 0xE3, 0x20, 0xE0, 0x06, 0x85, 0x21, 0xB3, + 0x43, 0xE3, 0x01, 0x12, 0x0F, 0xA3, 0x75, 0x80, 0xFF, 0x75, 0xA4, 0x00, 0x75, 0xF1, 0xF0, 0x75, + 0xD4, 0xDF, 0x75, 0x90, 0x02, 0x75, 0xA5, 0xFC, 0x75, 0xF2, 0xFD, 0x75, 0xD5, 0x02, 0x75, 0xA6, + 0x10, 0x75, 0xA0, 0xFF, 0x75, 0xF3, 0xF1, 0x75, 0xE2, 0x41, 0x12, 0x0F, 0xA3, 0xE4, 0xF8, 0xF6, + 0xD8, 0xFD, 0x75, 0x68, 0x01, 0x12, 0x0F, 0xB8, 0x12, 0x16, 0x5C, 0x78, 0x98, 0x86, 0x73, 0x75, + 0x30, 0x01, 0xC2, 0xAF, 0x12, 0x06, 0x0E, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x04, 0x12, 0x06, 0x25, + 0x12, 0x06, 0x04, 0x12, 0x06, 0x2C, 0x12, 0x06, 0x04, 0xC2, 0xAF, 0x78, 0xFA, 0x79, 0xFA, 0x30, + 0x85, 0x07, 0xD9, 0xFB, 0xD8, 0xF7, 0x02, 0x1C, 0x00, 0x11, 0x24, 0x11, 0x5D, 0x11, 0xAB, 0x11, + 0xAC, 0x78, 0x98, 0x86, 0x73, 0x12, 0x0F, 0xA3, 0x75, 0xA9, 0x00, 0x75, 0xB6, 0x80, 0x75, 0x7A, + 0x00, 0x75, 0x88, 0x10, 0x75, 0x89, 0x02, 0x75, 0xC8, 0x24, 0x75, 0x91, 0x04, 0x75, 0xD8, 0x40, + 0x75, 0xA8, 0x22, 0x75, 0xB8, 0x02, 0x75, 0xE6, 0x90, 0x75, 0x9B, 0x80, 0x75, 0x9D, 0x00, 0x75, + 0xD1, 0x0E, 0x75, 0xBC, 0xC0, 0x75, 0xBB, 0x09, 0x75, 0xBA, 0x11, 0x75, 0xE8, 0x80, 0x12, 0x05, + 0xF5, 0xD2, 0xAF, 0x12, 0x09, 0x14, 0x75, 0x36, 0x00, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, + 0xDA, 0x20, 0x30, 0x7E, 0x03, 0x43, 0xDA, 0x10, 0x43, 0xDA, 0x01, 0xC2, 0xD8, 0xC2, 0x71, 0x12, + 0x06, 0x0E, 0xD2, 0x61, 0x7B, 0x03, 0x7A, 0x0C, 0xE5, 0x5B, 0x70, 0x07, 0x7A, 0x0C, 0xDB, 0x03, + 0x02, 0x11, 0xA9, 0x12, 0x06, 0x04, 0x20, 0x70, 0x03, 0x02, 0x11, 0xA9, 0xC2, 0x70, 0xE5, 0x5C, + 0xC3, 0x94, 0x02, 0x40, 0xE1, 0xE5, 0x2F, 0x54, 0x1F, 0x85, 0x5E, 0x5D, 0xF5, 0x5E, 0xB5, 0x5D, + 0xD5, 0xDA, 0xD5, 0xC2, 0x61, 0x53, 0xDA, 0xCF, 0x20, 0x7E, 0x03, 0x43, 0xDA, 0x20, 0x30, 0x7E, + 0x03, 0x43, 0xDA, 0x10, 0xC2, 0xD8, 0xC2, 0x71, 0x78, 0xA2, 0xE6, 0x70, 0x08, 0xD2, 0x74, 0xE5, + 0x2F, 0x54, 0xE0, 0xF5, 0x2F, 0xC2, 0x75, 0x75, 0x29, 0x00, 0x12, 0x06, 0x09, 0x30, 0x74, 0x09, + 0xC3, 0xE5, 0x29, 0x94, 0x0A, 0x40, 0x02, 0xD2, 0x75, 0x12, 0x05, 0xFA, 0x78, 0x02, 0x30, 0x74, + 0x02, 0x78, 0x00, 0xC3, 0xE5, 0x5C, 0x98, 0x40, 0xF0, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x06, + 0x1E, 0x12, 0x06, 0x1E, 0xD2, 0xAF, 0x12, 0x06, 0x0E, 0x75, 0x4C, 0x00, 0x78, 0x88, 0xE6, 0xB4, + 0x03, 0x02, 0x61, 0x88, 0x12, 0x05, 0xFA, 0x78, 0x8C, 0xE6, 0xC3, 0x94, 0x01, 0x50, 0x03, 0x02, + 0x13, 0x88, 0xE5, 0x30, 0xC3, 0x94, 0x01, 0x50, 0x03, 0x02, 0x13, 0x88, 0x20, 0x74, 0x35, 0xC3, + 0xE5, 0x5C, 0x94, 0xFF, 0x50, 0x03, 0x02, 0x13, 0x88, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0xD2, 0xAF, + 0x12, 0x06, 0x09, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x50, 0xEF, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, + 0x05, 0xFF, 0x12, 0x06, 0x1E, 0xD2, 0xAF, 0x12, 0x06, 0x09, 0xC3, 0xE5, 0x5C, 0x94, 0xFF, 0x40, + 0xE9, 0x02, 0x18, 0x04, 0x7F, 0x02, 0xD2, 0x7F, 0x11, 0xAC, 0x12, 0x06, 0x09, 0xC2, 0xAF, 0xC2, + 0x7F, 0x11, 0xAC, 0xAE, 0x5C, 0xC3, 0xE5, 0x5C, 0x94, 0x7F, 0xD2, 0xAF, 0x40, 0x74, 0x12, 0x05, + 0xF5, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0xD2, 0xAF, 0xDF, 0xDC, 0x11, 0xE3, 0xC3, 0xEE, 0x78, 0x97, + 0xF6, 0x12, 0x06, 0x0E, 0x12, 0x16, 0x9E, 0x12, 0x17, 0x87, 0x7F, 0x0A, 0xD2, 0x7F, 0x11, 0xAC, + 0x12, 0x06, 0x09, 0xC2, 0xAF, 0xC2, 0x7F, 0x11, 0xAC, 0xAE, 0x5C, 0xC3, 0xE5, 0x5C, 0x94, 0x7F, + 0xD2, 0xAF, 0x50, 0xE6, 0x12, 0x05, 0xF5, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0x12, + 0x06, 0x1E, 0xD2, 0xAF, 0xDF, 0xD6, 0x11, 0xE3, 0xEE, 0x24, 0x03, 0x78, 0x96, 0xF6, 0x12, 0x06, + 0x0E, 0x12, 0x16, 0x9E, 0x12, 0x17, 0xB6, 0x12, 0x06, 0x09, 0x11, 0xAC, 0xC3, 0xE5, 0x5C, 0x94, + 0xFF, 0x50, 0x02, 0x41, 0xCC, 0x02, 0x18, 0x04, 0xC3, 0xE5, 0x5C, 0x95, 0x4C, 0x40, 0x03, 0x85, + 0x5C, 0x4C, 0x12, 0x06, 0x09, 0x78, 0x01, 0x79, 0x88, 0xE7, 0xB4, 0x03, 0x02, 0x78, 0x05, 0xC3, + 0xE5, 0x5C, 0x98, 0x40, 0x02, 0x41, 0xAC, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x33, 0x12, + 0x06, 0x33, 0xD2, 0xAF, 0x12, 0x06, 0x0E, 0x75, 0x30, 0x00, 0xE4, 0xF5, 0x31, 0xF5, 0x32, 0x05, + 0x31, 0xE5, 0x31, 0xF4, 0x70, 0x3F, 0x05, 0x32, 0x78, 0x9A, 0xE6, 0x78, 0x19, 0x14, 0x60, 0x12, + 0x78, 0x32, 0x14, 0x60, 0x0D, 0x78, 0x7D, 0x14, 0x60, 0x08, 0x78, 0xFA, 0x14, 0x60, 0x03, 0x75, + 0x32, 0x00, 0xC3, 0xE5, 0x32, 0x98, 0x40, 0x1D, 0x12, 0x0F, 0xA3, 0x12, 0x05, 0xF5, 0x15, 0x32, + 0x75, 0x31, 0x00, 0x78, 0x99, 0x86, 0x73, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0xD2, 0xAF, 0x78, 0x98, + 0x86, 0x73, 0x12, 0x06, 0x09, 0x12, 0x05, 0xFF, 0xE5, 0x2A, 0x70, 0x05, 0x30, 0x74, 0x02, 0x21, + 0xA9, 0x78, 0x01, 0x20, 0x74, 0x02, 0x78, 0x06, 0xC3, 0xE5, 0x5C, 0x98, 0x40, 0xA1, 0x78, 0x88, + 0xE6, 0xC3, 0x94, 0x03, 0x60, 0x03, 0x12, 0x06, 0x09, 0xE5, 0x2A, 0x70, 0x03, 0x02, 0x11, 0xA9, + 0xC2, 0xAF, 0x12, 0x0F, 0xA3, 0xE4, 0xF5, 0x22, 0xF5, 0x23, 0xF5, 0x24, 0xF5, 0x25, 0xF5, 0x26, + 0xF5, 0x67, 0xD2, 0xAF, 0x78, 0x85, 0xE6, 0xC3, 0x33, 0xF5, 0x65, 0xE4, 0xF5, 0x44, 0xF5, 0x45, + 0xF5, 0x46, 0xF5, 0x47, 0xF5, 0x48, 0xF5, 0x70, 0xF5, 0x2C, 0xF5, 0x2D, 0xF5, 0x37, 0x75, 0x70, + 0x08, 0x75, 0xBB, 0x10, 0x12, 0x05, 0xF5, 0x12, 0x09, 0x18, 0xE5, 0xE8, 0x20, 0xEC, 0xFB, 0xA8, + 0xBD, 0xA9, 0xBE, 0xE9, 0x70, 0x01, 0xF8, 0x88, 0x71, 0x12, 0x09, 0x1C, 0x75, 0x70, 0x08, 0x75, + 0xBB, 0x10, 0x11, 0x24, 0xC2, 0xAF, 0x75, 0x61, 0xFF, 0x12, 0x09, 0xB4, 0x85, 0x22, 0x61, 0x85, + 0x22, 0x62, 0x85, 0x22, 0x63, 0xD2, 0xAF, 0x75, 0x22, 0x01, 0x75, 0x24, 0x01, 0x75, 0x25, 0x01, + 0x75, 0x26, 0x01, 0x85, 0x60, 0x69, 0x75, 0x6A, 0x01, 0x75, 0xB6, 0x90, 0x75, 0xA9, 0x03, 0x75, + 0x7A, 0x01, 0x78, 0x88, 0xE6, 0xB4, 0x03, 0x07, 0xC2, 0x7D, 0x30, 0x76, 0x02, 0xD2, 0x7D, 0xD2, + 0x68, 0xD2, 0x69, 0x75, 0x33, 0x00, 0x12, 0x0E, 0xFD, 0x12, 0x0F, 0x2C, 0x12, 0x09, 0xD4, 0x12, + 0x09, 0xDB, 0x12, 0x09, 0xD4, 0x12, 0x09, 0xDB, 0x12, 0x09, 0xD4, 0x12, 0x0C, 0x64, 0x12, 0x07, + 0x0E, 0x12, 0x0D, 0x67, 0x12, 0x0D, 0xB1, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x53, 0x30, 0x6E, 0x03, + 0x12, 0x07, 0x6A, 0x20, 0x67, 0x03, 0x12, 0x08, 0xB7, 0x30, 0x67, 0x03, 0x12, 0x08, 0xFE, 0x12, + 0x0D, 0x67, 0x12, 0x0D, 0xE0, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x64, 0x30, 0x6E, 0x03, 0x12, 0x07, + 0x9F, 0x12, 0x0D, 0x67, 0x12, 0x0E, 0x57, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x53, 0x30, 0x6E, 0x03, + 0x12, 0x07, 0xFD, 0x12, 0x0D, 0x67, 0x12, 0x0E, 0x86, 0x12, 0x09, 0xDB, 0x12, 0x0C, 0x64, 0x30, + 0x6E, 0x03, 0x12, 0x08, 0x5B, 0x12, 0x0D, 0x67, 0x12, 0x0E, 0xFD, 0x12, 0x09, 0xDB, 0x12, 0x09, + 0x18, 0x12, 0x0C, 0x53, 0x12, 0x0D, 0x67, 0x12, 0x0F, 0x2C, 0x12, 0x09, 0x1C, 0x12, 0x09, 0xDB, + 0x30, 0x69, 0x32, 0x85, 0x64, 0x61, 0x85, 0x64, 0x62, 0x85, 0x60, 0x69, 0x75, 0x6A, 0x01, 0x79, + 0x18, 0x7A, 0x0C, 0xC3, 0xE5, 0x33, 0x99, 0x40, 0x0F, 0xC2, 0x69, 0xD2, 0x6A, 0x8A, 0x35, 0x85, + 0x64, 0x61, 0x85, 0x64, 0x63, 0x02, 0x15, 0x85, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x03, 0x02, + 0x14, 0xDB, 0x02, 0x16, 0x0B, 0x30, 0x6A, 0x1D, 0x20, 0x6C, 0x1A, 0xE5, 0x35, 0x14, 0x70, 0x06, + 0xC2, 0x6A, 0xD2, 0x6B, 0x81, 0xDB, 0xF5, 0x35, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x03, 0x02, + 0x14, 0xDB, 0x02, 0x16, 0x0B, 0x75, 0x36, 0x00, 0x78, 0xFA, 0x79, 0xA4, 0xE7, 0x60, 0x02, 0x78, + 0x03, 0xC3, 0xE5, 0x5F, 0x98, 0x50, 0x54, 0x30, 0x74, 0x04, 0xE5, 0x2A, 0x60, 0x4D, 0x78, 0x88, + 0xE6, 0xB4, 0x03, 0x17, 0x20, 0x7D, 0x05, 0x20, 0x76, 0x07, 0xA1, 0xDB, 0x30, 0x76, 0x02, 0xA1, + 0xDB, 0x20, 0x6C, 0x07, 0xD2, 0x6C, 0x85, 0x64, 0x61, 0xA1, 0x1A, 0x78, 0xF0, 0x30, 0x6C, 0x05, + 0x85, 0x64, 0x61, 0x78, 0x20, 0xC3, 0xE5, 0x41, 0x98, 0x50, 0x02, 0x81, 0xDB, 0x30, 0x6C, 0x1B, + 0xC2, 0x6C, 0xC2, 0x7D, 0x30, 0x76, 0x02, 0xD2, 0x7D, 0xD2, 0x6A, 0x75, 0x35, 0x12, 0x85, 0x64, + 0x61, 0x81, 0xDB, 0x05, 0x36, 0xE5, 0x5C, 0x60, 0x02, 0xC1, 0x0E, 0x75, 0x36, 0x00, 0xC2, 0xAF, + 0x12, 0x0F, 0xA3, 0x78, 0x87, 0xE6, 0xFE, 0x76, 0x02, 0x11, 0x24, 0x78, 0x87, 0xEE, 0xF6, 0xE4, + 0xF5, 0x22, 0xF5, 0x23, 0xF5, 0x24, 0xF5, 0x25, 0xF5, 0x26, 0xF5, 0x65, 0x75, 0x2C, 0x00, 0x75, + 0x2D, 0x00, 0x75, 0xA9, 0x00, 0x75, 0xB6, 0x80, 0x75, 0x7A, 0x00, 0xD2, 0xAF, 0x12, 0x06, 0x09, + 0x12, 0x0F, 0xA3, 0x78, 0xA4, 0xE6, 0x60, 0x06, 0xD2, 0x92, 0xD2, 0x95, 0xD2, 0x96, 0x30, 0x74, + 0x09, 0xC3, 0xE5, 0x36, 0x94, 0x04, 0x40, 0x02, 0x21, 0xA9, 0x61, 0xBA, 0x90, 0x1A, 0x0D, 0x78, + 0x20, 0x12, 0x16, 0xE0, 0xE5, 0x20, 0xB4, 0x55, 0x0C, 0xA3, 0x12, 0x16, 0xE0, 0xE5, 0x20, 0xB4, + 0xAA, 0x03, 0x02, 0x16, 0x7E, 0x12, 0x0F, 0xB8, 0x12, 0x16, 0x9E, 0x02, 0x16, 0x9D, 0x90, 0x1A, + 0x03, 0x78, 0x80, 0x7B, 0x0A, 0x12, 0x16, 0xE0, 0xA3, 0x08, 0xDB, 0xF9, 0x90, 0x1A, 0x0F, 0x78, + 0x8C, 0x7B, 0x19, 0x12, 0x16, 0xE0, 0xA3, 0x08, 0xDB, 0xF9, 0x90, 0x1A, 0x28, 0x22, 0xC2, 0xAF, + 0x12, 0x17, 0x1F, 0x12, 0x16, 0xF9, 0x90, 0x1A, 0x00, 0x74, 0x0E, 0x12, 0x16, 0xE5, 0xA3, 0x74, + 0x06, 0x12, 0x16, 0xE5, 0xA3, 0x74, 0x15, 0x12, 0x16, 0xE5, 0x90, 0x1A, 0x03, 0x78, 0x80, 0x7B, + 0x0A, 0x12, 0x16, 0xE4, 0xA3, 0x08, 0xDB, 0xF9, 0x90, 0x1A, 0x0F, 0x78, 0x8C, 0x7B, 0x19, 0x12, + 0x16, 0xE4, 0xA3, 0x08, 0xDB, 0xF9, 0x12, 0x17, 0x32, 0x12, 0x17, 0x10, 0x90, 0x1A, 0x28, 0x22, + 0xE4, 0x93, 0xF6, 0x22, 0xE6, 0x43, 0x8F, 0x01, 0x53, 0x8F, 0xFD, 0x75, 0xEF, 0x02, 0x75, 0xB7, + 0xA5, 0x75, 0xB7, 0xF1, 0xF0, 0x53, 0x8F, 0xFE, 0x22, 0x43, 0x8F, 0x02, 0x43, 0x8F, 0x01, 0x75, + 0xEF, 0x02, 0x75, 0xB7, 0xA5, 0x75, 0xB7, 0xF1, 0x90, 0x1A, 0x0D, 0xF0, 0x53, 0x8F, 0xFC, 0x22, + 0x90, 0x1A, 0x0D, 0x74, 0x55, 0xD1, 0xE5, 0x90, 0x1A, 0x0E, 0x74, 0xAA, 0xD1, 0xE5, 0x22, 0x7A, + 0x30, 0x79, 0xD0, 0x78, 0x20, 0x90, 0x1A, 0x40, 0xD1, 0xE0, 0xE5, 0x20, 0xF7, 0x09, 0xA3, 0xDA, + 0xF7, 0x22, 0x7A, 0x30, 0x79, 0xD0, 0x90, 0x1A, 0x40, 0xE7, 0xD1, 0xE5, 0x09, 0xA3, 0xDA, 0xF9, + 0x22, 0xAB, 0x74, 0xA8, 0x75, 0xBB, 0x01, 0x02, 0x79, 0x80, 0xBB, 0x02, 0x02, 0x79, 0x81, 0xBB, + 0x03, 0x02, 0x79, 0x82, 0xBB, 0x04, 0x02, 0x79, 0x84, 0xBB, 0x05, 0x02, 0x79, 0x86, 0xBB, 0x06, + 0x02, 0x79, 0x92, 0xBB, 0x07, 0x02, 0x79, 0x87, 0xBB, 0x08, 0x02, 0x79, 0xA3, 0xBB, 0x09, 0x02, + 0x79, 0x9C, 0xBB, 0x0A, 0x02, 0x79, 0x88, 0xBB, 0x0B, 0x02, 0x79, 0x89, 0xE8, 0xF7, 0x22, 0x7C, + 0x05, 0x12, 0x06, 0x0E, 0xDC, 0xFB, 0x22, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x25, 0x12, + 0x06, 0x2C, 0x12, 0x06, 0x33, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x25, 0x12, 0x06, + 0x2C, 0x12, 0x06, 0x33, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x1E, 0x12, 0x06, 0x25, 0x12, 0x06, 0x2C, + 0x12, 0x06, 0x33, 0xD2, 0xAF, 0x22, 0xC2, 0xAF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x2C, 0x12, 0x06, + 0x25, 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x2C, 0x12, 0x06, 0x25, + 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0x12, 0x06, 0x33, 0x12, 0x06, 0x2C, 0x12, 0x06, 0x25, 0x12, + 0x06, 0x1E, 0xD2, 0xAF, 0x22, 0xAE, 0x74, 0xAF, 0x75, 0xC2, 0xAF, 0x12, 0x06, 0x1E, 0x12, 0x06, + 0x1E, 0x12, 0x06, 0x1E, 0x12, 0x05, 0xFF, 0xDE, 0xF2, 0x12, 0x06, 0x33, 0x12, 0x05, 0xFF, 0xDF, + 0xF8, 0xD2, 0xAF, 0x22, 0x12, 0x17, 0x87, 0x12, 0x17, 0x7F, 0x12, 0x17, 0x7F, 0x75, 0x74, 0x01, + 0x75, 0x75, 0x01, 0x75, 0x76, 0x00, 0x12, 0x17, 0xE5, 0x7C, 0x05, 0xAD, 0x5C, 0x12, 0x06, 0x0E, + 0xC3, 0xED, 0x95, 0x5C, 0x70, 0xF5, 0xC3, 0xE5, 0x5C, 0x94, 0x01, 0x40, 0x0A, 0xC3, 0xE5, 0x5C, + 0x94, 0xFF, 0x40, 0x37, 0x02, 0x18, 0x48, 0x12, 0x17, 0x41, 0x12, 0x16, 0x9E, 0x12, 0x17, 0x87, + 0xC2, 0xAF, 0x75, 0xEF, 0x12, 0x12, 0x17, 0x7F, 0xDC, 0xD1, 0x05, 0x76, 0xC3, 0xE5, 0x76, 0x94, + 0x03, 0x50, 0x02, 0x80, 0xC1, 0x12, 0x17, 0x7F, 0x05, 0x75, 0xE5, 0x74, 0x14, 0x90, 0x00, 0x9F, + 0x93, 0xF8, 0x08, 0xC3, 0xE5, 0x75, 0x98, 0x50, 0x02, 0x80, 0xA8, 0x12, 0x17, 0x7F, 0x12, 0x17, + 0x7F, 0x05, 0x74, 0xC3, 0xE5, 0x74, 0x94, 0x0C, 0x50, 0x02, 0x80, 0x94, 0x12, 0x0F, 0xB8, 0x12, + 0x16, 0x9E, 0xC2, 0xAF, 0x75, 0xEF, 0x12, 0x12, 0x17, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x02, 0x11, 0x0E, + 0x0E, 0x06, 0x15, 0x09, 0x09, 0x04, 0xFF, 0x03, 0xFF, 0x09, 0x03, 0x01, 0x01, 0x55, 0xAA, 0x01, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x03, 0xFF, 0xFF, 0xFF, 0x25, 0xD0, 0x28, 0x50, 0x04, 0xFF, 0x02, + 0x00, 0x7A, 0xFF, 0x01, 0x01, 0x00, 0x03, 0x00, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0x23, 0x46, 0x56, 0x54, 0x4C, 0x69, 0x62, 0x65, 0x65, 0x33, 0x30, 0x41, 0x23, 0x20, 0x20, 0x20, + 0x23, 0x42, 0x4C, 0x48, 0x45, 0x4C, 0x49, 0x23, 0x46, 0x33, 0x39, 0x30, 0x23, 0x20, 0x20, 0x20, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, + 0xC2, 0xAF, 0xC2, 0xD3, 0x53, 0xD9, 0xBF, 0x75, 0x81, 0xC0, 0x43, 0xB2, 0x03, 0x43, 0xFF, 0x80, + 0x75, 0x24, 0x38, 0x75, 0x25, 0x03, 0xB1, 0xC9, 0x75, 0xEF, 0x02, 0x75, 0xE2, 0x40, 0x43, 0xF1, + 0x20, 0x53, 0xA4, 0xDF, 0x75, 0xD4, 0xFF, 0xD2, 0x85, 0x7D, 0xFA, 0x78, 0x03, 0x79, 0xE6, 0x7C, + 0x10, 0x75, 0x22, 0x00, 0x75, 0x23, 0x00, 0x90, 0x1D, 0xD6, 0x7B, 0x06, 0x75, 0x24, 0x98, 0x75, + 0x25, 0x01, 0x30, 0x85, 0x02, 0x81, 0x42, 0x20, 0x85, 0x08, 0xD8, 0xFB, 0xD9, 0xF9, 0xDC, 0xF7, + 0x81, 0xB4, 0x30, 0x85, 0x08, 0xD8, 0xFB, 0xD9, 0xF9, 0xDC, 0xF7, 0x81, 0xB4, 0xB1, 0x89, 0xE4, + 0x93, 0xA3, 0xC3, 0x9A, 0x60, 0x04, 0xDD, 0xC3, 0x81, 0xB4, 0xDB, 0xDB, 0xB1, 0x7B, 0x60, 0x02, + 0x81, 0x2B, 0x7B, 0x08, 0xE4, 0x93, 0xFA, 0xA3, 0xB1, 0x5C, 0xDB, 0xF8, 0x7A, 0x30, 0xB1, 0x5C, + 0x75, 0x22, 0x00, 0x75, 0x23, 0x00, 0xB1, 0x7B, 0xEA, 0xFC, 0xEB, 0xFD, 0xC3, 0xED, 0x94, 0xFE, + 0x40, 0x0E, 0xB1, 0x7B, 0x8A, 0x27, 0x8B, 0x28, 0xED, 0x30, 0xE0, 0x04, 0x8A, 0x82, 0x8B, 0x83, + 0xB1, 0x7B, 0x7A, 0xC2, 0x70, 0xD8, 0xC3, 0xED, 0x94, 0xFE, 0x60, 0x13, 0x50, 0xCE, 0xBD, 0x00, + 0x25, 0xEC, 0x60, 0x09, 0x75, 0x20, 0x00, 0x75, 0x21, 0xFF, 0x02, 0x00, 0x00, 0x81, 0x00, 0xA8, + 0x27, 0xA9, 0x28, 0x08, 0x09, 0xD8, 0x04, 0xD9, 0x02, 0x81, 0xD3, 0xB1, 0x7F, 0xEA, 0x89, 0xAA, + 0xF2, 0x81, 0xC5, 0x0D, 0x81, 0xA0, 0xC3, 0xED, 0x94, 0x03, 0x50, 0x49, 0xED, 0xA2, 0xE0, 0x92, + 0x00, 0x7A, 0xC5, 0xC3, 0xE5, 0x82, 0x94, 0x00, 0xE5, 0x83, 0x94, 0x1C, 0x50, 0x90, 0x20, 0x00, + 0x10, 0x43, 0x8F, 0x02, 0x43, 0x8F, 0x01, 0x75, 0xB7, 0xA5, 0x75, 0xB7, 0xF1, 0xF0, 0x30, 0x00, + 0x1F, 0xA8, 0x27, 0xA9, 0x28, 0x08, 0x09, 0x43, 0x8F, 0x01, 0x53, 0x8F, 0xFD, 0xD8, 0x04, 0xD9, + 0x02, 0xA1, 0x20, 0x89, 0xAA, 0xE2, 0x75, 0xB7, 0xA5, 0x75, 0xB7, 0xF1, 0xF0, 0xA3, 0xA1, 0x0D, + 0x53, 0x8F, 0xFC, 0x81, 0x7C, 0x7A, 0xC1, 0xBD, 0x03, 0x0C, 0xE4, 0x93, 0xA3, 0xFA, 0xB1, 0x40, + 0xDC, 0xF8, 0xB1, 0x38, 0x81, 0x7C, 0x81, 0x7E, 0xAA, 0x22, 0xAB, 0x23, 0xB1, 0x5C, 0xEB, 0xFA, + 0xEA, 0x62, 0x22, 0x75, 0x26, 0x08, 0xC3, 0xE5, 0x23, 0x13, 0xF5, 0x23, 0xE5, 0x22, 0x13, 0xF5, + 0x22, 0x50, 0x06, 0x63, 0x23, 0xA0, 0x63, 0x22, 0x01, 0xD5, 0x26, 0xEA, 0xB1, 0xC9, 0xB1, 0xC9, + 0x75, 0x26, 0x0A, 0xEA, 0xF4, 0x20, 0x01, 0x02, 0xD2, 0x85, 0x30, 0x01, 0x02, 0xC2, 0x85, 0xB1, + 0xC9, 0xC3, 0x13, 0x40, 0x02, 0xC2, 0x01, 0xD5, 0x26, 0xEB, 0x22, 0xB1, 0x7F, 0xEA, 0xFB, 0x20, + 0x85, 0x02, 0xA1, 0x7F, 0x30, 0x85, 0x02, 0xA1, 0x84, 0x75, 0x26, 0x08, 0xAE, 0x24, 0xAF, 0x25, + 0xC3, 0xEF, 0x13, 0xFF, 0xEE, 0x13, 0xFE, 0xB1, 0xCD, 0xB1, 0xC9, 0xC3, 0xEA, 0x13, 0x30, 0x85, + 0x02, 0x44, 0x80, 0xFA, 0x30, 0xE7, 0x03, 0x63, 0x22, 0x01, 0xC3, 0xE5, 0x23, 0x13, 0xF5, 0x23, + 0xE5, 0x22, 0x13, 0xF5, 0x22, 0x50, 0x06, 0x63, 0x23, 0xA0, 0x63, 0x22, 0x01, 0xD5, 0x26, 0xD9, + 0xE5, 0x22, 0x65, 0x23, 0x65, 0x23, 0xF5, 0x22, 0x22, 0xAE, 0x24, 0xAF, 0x25, 0x0E, 0x0F, 0xDE, + 0xFE, 0xDF, 0xFC, 0xD2, 0x01, 0x22, 0x42, 0x4C, 0x48, 0x65, 0x6C, 0x69, 0x34, 0x37, 0x31, 0x63, + 0xF3, 0x90, 0x06, 0x01 + }; + +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo) +{ + //only 2 bytes used $1E9307 -> 0x9307 + pDeviceInfo->dword = 0x163F390; + return true; +} + +uint8_t BL_SendCMDKeepAlive(void) +{ + return true; +} + +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo) +{ + pDeviceInfo->bytes[0] = 1; + return; +} + +static uint8_t BL_ReadA(uint8_t cmd, ioMem_t *pMem) +{ + UNUSED(cmd); + uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; + + uint16_t bytes = pMem->D_NUM_BYTES; + if (bytes == 0) bytes = 256; + + memcpy(pMem->D_PTR_I, &virtualFlash[address], bytes); + return true; +} + +static uint8_t BL_WriteA(uint8_t cmd, ioMem_t *pMem, uint32_t timeout) +{ + UNUSED(cmd); + UNUSED(timeout); + uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; + + uint16_t bytes = pMem->D_NUM_BYTES; + if (bytes == 0) bytes = 256; + memcpy(&virtualFlash[address], pMem->D_PTR_I, bytes); + return true; +} + +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem) +{ + UNUSED(interface_mode); + return BL_ReadA(0, pMem); +} + +uint8_t BL_ReadEEprom(ioMem_t *pMem) +{ + return BL_ReadA(0, pMem); +} + +uint8_t BL_PageErase(ioMem_t *pMem) +{ + uint16_t address = pMem->D_FLASH_ADDR_H << 8 | pMem->D_FLASH_ADDR_L; + if (address + VIRTUAL_PAGE_SIZE > VIRTUAL_FLASH_SIZE) + return false; + memset(&virtualFlash[address], 0xFF, VIRTUAL_PAGE_SIZE); + return true; +} + +uint8_t BL_WriteEEprom(ioMem_t *pMem) +{ + return BL_WriteA(0, pMem, 0); +} + +uint8_t BL_WriteFlash(ioMem_t *pMem) +{ + return BL_WriteA(0, pMem, 0); +} + +#endif +#endif diff --git a/lib/betaflight/src/io/serial_4way_avrootloader.h b/lib/betaflight/src/io/serial_4way_avrootloader.h new file mode 100644 index 00000000..e86a2dd4 --- /dev/null +++ b/lib/betaflight/src/io/serial_4way_avrootloader.h @@ -0,0 +1,44 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 + * for info about Hagens AVRootloader: + * http://www.mikrocontroller.net/topic/avr-bootloader-mit-verschluesselung +*/ + +#pragma once +// Bootloader result codes +#define brSUCCESS 0x30 +#define brERRORVERIFY 0xC0 +#define brERRORCOMMAND 0xC1 +#define brERRORCRC 0xC2 +#define brNONE 0xFF + +void BL_SendBootInit(void); +uint8_t BL_ConnectEx(uint8_32_u *pDeviceInfo); +uint8_t BL_SendCMDKeepAlive(void); +uint8_t BL_PageErase(ioMem_t *pMem); +uint8_t BL_ReadEEprom(ioMem_t *pMem); +uint8_t BL_WriteEEprom(ioMem_t *pMem); +uint8_t BL_WriteFlash(ioMem_t *pMem); +uint8_t BL_ReadFlash(uint8_t interface_mode, ioMem_t *pMem); +uint8_t BL_VerifyFlash(ioMem_t *pMem); +void BL_SendCMDRunRestartBootloader(uint8_32_u *pDeviceInfo); diff --git a/lib/betaflight/src/io/serial_4way_impl.h b/lib/betaflight/src/io/serial_4way_impl.h new file mode 100644 index 00000000..0fdb434d --- /dev/null +++ b/lib/betaflight/src/io/serial_4way_impl.h @@ -0,0 +1,53 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 +*/ +#pragma once + +#include "drivers/io_types.h" + +typedef struct { + IO_t io; +} escHardware_t; + +extern uint8_t selected_esc; + +bool isEscHi(uint8_t selEsc); +bool isEscLo(uint8_t selEsc); +void setEscHi(uint8_t selEsc); +void setEscLo(uint8_t selEsc); +void setEscInput(uint8_t selEsc); +void setEscOutput(uint8_t selEsc); + +#define ESC_IS_HI isEscHi(selected_esc) +#define ESC_IS_LO isEscLo(selected_esc) +#define ESC_SET_HI setEscHi(selected_esc) +#define ESC_SET_LO setEscLo(selected_esc) +#define ESC_INPUT setEscInput(selected_esc) +#define ESC_OUTPUT setEscOutput(selected_esc) + +typedef struct ioMem_s { + uint8_t D_NUM_BYTES; + uint8_t D_FLASH_ADDR_H; + uint8_t D_FLASH_ADDR_L; + uint8_t *D_PTR_I; +} ioMem_t; diff --git a/lib/betaflight/src/io/serial_4way_stk500v2.c b/lib/betaflight/src/io/serial_4way_stk500v2.c new file mode 100644 index 00000000..3cadd514 --- /dev/null +++ b/lib/betaflight/src/io/serial_4way_stk500v2.c @@ -0,0 +1,427 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 + * have a look at https://github.com/sim-/tgy/blob/master/boot.inc + * for info about the stk500v2 implementation + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_SERIAL_4WAY_BLHELI_INTERFACE) && defined(USE_SERIAL_4WAY_SK_BOOTLOADER) + +#include "drivers/io.h" +#include "drivers/serial.h" +#include "drivers/time.h" + +#include "io/serial.h" +#include "io/serial_4way.h" +#include "io/serial_4way_impl.h" +#include "io/serial_4way_stk500v2.h" + + +#define BIT_LO_US (32) //32uS +#define BIT_HI_US (2*BIT_LO_US) + +static uint8_t StkInBuf[16]; + +#define STK_BIT_TIMEOUT 250 // micro seconds +#define STK_WAIT_TICKS (1000 / STK_BIT_TIMEOUT) // per ms +#define STK_WAITCYLCES (STK_WAIT_TICKS * 35) // 35ms +#define STK_WAITCYLCES_START (STK_WAIT_TICKS / 2) // 0.5 ms +#define STK_WAITCYLCES_EXT (STK_WAIT_TICKS * 5000) //5 s + +#define WaitPinLo while (ESC_IS_HI) {if (micros() > timeout_timer) goto timeout;} +#define WaitPinHi while (ESC_IS_LO) {if (micros() > timeout_timer) goto timeout;} + +static uint32_t LastBitTime; +static uint32_t HiLoTsh; + +static uint8_t SeqNumber; +static uint8_t StkCmd; +static uint8_t ckSumIn; +static uint8_t ckSumOut; + +// used STK message constants from ATMEL AVR - Application note +#define MESSAGE_START 0x1B +#define TOKEN 0x0E + +#define CMD_SIGN_ON 0x01 +#define CMD_LOAD_ADDRESS 0x06 +#define CMD_CHIP_ERASE_ISP 0x12 +#define CMD_PROGRAM_FLASH_ISP 0x13 +#define CMD_READ_FLASH_ISP 0x14 +#define CMD_PROGRAM_EEPROM_ISP 0x15 +#define CMD_READ_EEPROM_ISP 0x16 +#define CMD_READ_SIGNATURE_ISP 0x1B +#define CMD_SPI_MULTI 0x1D + +#define STATUS_CMD_OK 0x00 + +#define CmdFlashEepromRead 0xA0 +#define EnterIspCmd1 0xAC +#define EnterIspCmd2 0x53 +#define signature_r 0x30 + +#define delay_us(x) delayMicroseconds(x) +#define IRQ_OFF // dummy +#define IRQ_ON // dummy + +static void StkSendByte(uint8_t dat) +{ + ckSumOut ^= dat; + for (uint8_t i = 0; i < 8; i++) { + if (dat & 0x01) { + // 1-bits are encoded as 64.0us high, 72.8us low (135.8us total). + ESC_SET_HI; + delay_us(BIT_HI_US); + ESC_SET_LO; + delay_us(BIT_HI_US); + } else { + // 0-bits are encoded as 27.8us high, 34.5us low, 34.4us high, 37.9 low (134.6us total) + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_SET_LO; + delay_us(BIT_LO_US); + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_SET_LO; + delay_us(BIT_LO_US); + } + dat >>= 1; + } +} + +static void StkSendPacketHeader(void) +{ + IRQ_OFF; + ESC_OUTPUT; + StkSendByte(0xFF); + StkSendByte(0xFF); + StkSendByte(0x7F); + ckSumOut = 0; + StkSendByte(MESSAGE_START); + StkSendByte(++SeqNumber); +} + +static void StkSendPacketFooter(void) +{ + StkSendByte(ckSumOut); + ESC_SET_HI; + delay_us(BIT_LO_US); + ESC_INPUT; + IRQ_ON; +} + + + +static int8_t ReadBit(void) +{ + uint32_t btimer = micros(); + uint32_t timeout_timer = btimer + STK_BIT_TIMEOUT; + WaitPinLo; + WaitPinHi; + LastBitTime = micros() - btimer; + if (LastBitTime <= HiLoTsh) { + timeout_timer = timeout_timer + STK_BIT_TIMEOUT; + WaitPinLo; + WaitPinHi; + //lo-bit + return 0; + } else { + return 1; + } +timeout: + return -1; +} + +static uint8_t ReadByte(uint8_t *bt) +{ + *bt = 0; + for (uint8_t i = 0; i < 8; i++) { + int8_t bit = ReadBit(); + if (bit == -1) goto timeout; + if (bit == 1) { + *bt |= (1 << i); + } + } + ckSumIn ^=*bt; + return 1; +timeout: + return 0; +} + +static uint8_t StkReadLeader(void) +{ + + // Reset learned timing + HiLoTsh = BIT_HI_US + BIT_LO_US; + + // Wait for the first bit + uint32_t waitcycl; //250uS each + + if ((StkCmd == CMD_PROGRAM_EEPROM_ISP) || (StkCmd == CMD_CHIP_ERASE_ISP)) { + waitcycl = STK_WAITCYLCES_EXT; + } else if (StkCmd == CMD_SIGN_ON) { + waitcycl = STK_WAITCYLCES_START; + } else { + waitcycl= STK_WAITCYLCES; + } + for ( ; waitcycl >0 ; waitcycl--) { + //check is not timeout + if (ReadBit() >- 1) break; + } + + //Skip the first bits + if (waitcycl == 0) { + goto timeout; + } + + for (uint8_t i = 0; i < 10; i++) { + if (ReadBit() == -1) goto timeout; + } + + // learn timing + HiLoTsh = (LastBitTime >> 1) + (LastBitTime >> 2); + + // Read until we get a 0 bit + int8_t bit; + do { + bit = ReadBit(); + if (bit == -1) goto timeout; + } while (bit > 0); + return 1; +timeout: + return 0; +} + +static uint8_t StkRcvPacket(uint8_t *pstring) +{ + uint8_t bt = 0; + uint8_16_u Len; + + IRQ_OFF; + if (!StkReadLeader()) goto Err; + ckSumIn=0; + if (!ReadByte(&bt) || (bt != MESSAGE_START)) goto Err; + if (!ReadByte(&bt) || (bt != SeqNumber)) goto Err; + ReadByte(&Len.bytes[1]); + if (Len.bytes[1] > 1) goto Err; + ReadByte(&Len.bytes[0]); + if (Len.bytes[0] < 1) goto Err; + if (!ReadByte(&bt) || (bt != TOKEN)) goto Err; + if (!ReadByte(&bt) || (bt != StkCmd)) goto Err; + if (!ReadByte(&bt) || (bt != STATUS_CMD_OK)) goto Err; + for (uint16_t i = 0; i < (Len.word - 2); i++) + { + if (!ReadByte(pstring)) goto Err; + pstring++; + } + ReadByte(&bt); + if (ckSumIn != 0) goto Err; + IRQ_ON; + return 1; +Err: + IRQ_ON; + return 0; +} + +static uint8_t _CMD_SPI_MULTI_EX(volatile uint8_t * ResByte,uint8_t Cmd,uint8_t AdrHi,uint8_t AdrLo) +{ + StkCmd= CMD_SPI_MULTI; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(8); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_SPI_MULTI); + StkSendByte(4); // NumTX + StkSendByte(4); // NumRX + StkSendByte(0); // RxStartAdr + StkSendByte(Cmd); // {TxData} Cmd + StkSendByte(AdrHi); // {TxData} AdrHi + StkSendByte(AdrLo); // {TxData} AdrLoch + StkSendByte(0); // {TxData} 0 + StkSendPacketFooter(); + if (StkRcvPacket((void *)StkInBuf)) { // NumRX + 3 + if ((StkInBuf[0] == 0x00) && ((StkInBuf[1] == Cmd)||(StkInBuf[1] == 0x00)/* ignore zero returns */) &&(StkInBuf[2] == 0x00)) { + *ResByte = StkInBuf[3]; + } + return 1; + } + return 0; +} + +static uint8_t _CMD_LOAD_ADDRESS(ioMem_t *pMem) +{ + // ignore 0xFFFF + // assume address is set before and we read or write the immediately following package + if ((pMem->D_FLASH_ADDR_H == 0xFF) && (pMem->D_FLASH_ADDR_L == 0xFF)) return 1; + StkCmd = CMD_LOAD_ADDRESS; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(5); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_LOAD_ADDRESS); + StkSendByte(0); + StkSendByte(0); + StkSendByte(pMem->D_FLASH_ADDR_H); + StkSendByte(pMem->D_FLASH_ADDR_L); + StkSendPacketFooter(); + return (StkRcvPacket((void *)StkInBuf)); +} + +static uint8_t _CMD_READ_MEM_ISP(ioMem_t *pMem) +{ + uint8_t LenHi; + if (pMem->D_NUM_BYTES>0) { + LenHi=0; + } else { + LenHi=1; + } + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(4); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(StkCmd); + StkSendByte(LenHi); + StkSendByte(pMem->D_NUM_BYTES); + StkSendByte(CmdFlashEepromRead); + StkSendPacketFooter(); + return (StkRcvPacket(pMem->D_PTR_I)); +} + +static uint8_t _CMD_PROGRAM_MEM_ISP(ioMem_t *pMem) +{ + uint8_16_u Len; + uint8_t LenLo = pMem->D_NUM_BYTES; + uint8_t LenHi; + if (LenLo) { + LenHi = 0; + Len.word = LenLo + 10; + } else { + LenHi = 1; + Len.word = 256 + 10; + } + StkSendPacketHeader(); + StkSendByte(Len.bytes[1]); // high byte Msg len + StkSendByte(Len.bytes[0]); // low byte Msg len + StkSendByte(TOKEN); + StkSendByte(StkCmd); + StkSendByte(LenHi); + StkSendByte(LenLo); + StkSendByte(0); // mode + StkSendByte(0); // delay + StkSendByte(0); // cmd1 + StkSendByte(0); // cmd2 + StkSendByte(0); // cmd3 + StkSendByte(0); // poll1 + StkSendByte(0); // poll2 + do { + StkSendByte(*pMem->D_PTR_I); + pMem->D_PTR_I++; + LenLo--; + } while (LenLo); + StkSendPacketFooter(); + return StkRcvPacket((void *)StkInBuf); +} + +uint8_t Stk_SignOn(void) +{ + StkCmd=CMD_SIGN_ON; + StkSendPacketHeader(); + StkSendByte(0); // hi byte Msg len + StkSendByte(1); // lo byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_SIGN_ON); + StkSendPacketFooter(); + return (StkRcvPacket((void *) StkInBuf)); +} + +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo) +{ + if (Stk_SignOn()) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[1], signature_r,0,1)) { + if (_CMD_SPI_MULTI_EX(&pDeviceInfo->bytes[0], signature_r,0,2)) { + return 1; + } + } + } + return 0; +} + +uint8_t Stk_Chip_Erase(void) +{ + StkCmd = CMD_CHIP_ERASE_ISP; + StkSendPacketHeader(); + StkSendByte(0); // high byte Msg len + StkSendByte(7); // low byte Msg len + StkSendByte(TOKEN); + StkSendByte(CMD_CHIP_ERASE_ISP); + StkSendByte(20); // ChipErase_eraseDelay atmega8 + StkSendByte(0); // ChipErase_pollMethod atmega8 + StkSendByte(0xAC); + StkSendByte(0x88); + StkSendByte(0x13); + StkSendByte(0x76); + StkSendPacketFooter(); + return (StkRcvPacket(StkInBuf)); +} + +uint8_t Stk_ReadFlash(ioMem_t *pMem) +{ + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_FLASH_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; +} + + +uint8_t Stk_ReadEEprom(ioMem_t *pMem) +{ + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_READ_EEPROM_ISP; + return (_CMD_READ_MEM_ISP(pMem)); + } + return 0; +} + +uint8_t Stk_WriteFlash(ioMem_t *pMem) +{ + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_FLASH_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; +} + +uint8_t Stk_WriteEEprom(ioMem_t *pMem) +{ + if (_CMD_LOAD_ADDRESS(pMem)) { + StkCmd = CMD_PROGRAM_EEPROM_ISP; + return (_CMD_PROGRAM_MEM_ISP(pMem)); + } + return 0; +} +#endif diff --git a/lib/betaflight/src/io/serial_4way_stk500v2.h b/lib/betaflight/src/io/serial_4way_stk500v2.h new file mode 100644 index 00000000..fd1fdc4a --- /dev/null +++ b/lib/betaflight/src/io/serial_4way_stk500v2.h @@ -0,0 +1,32 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* + * Author: 4712 +*/ +#pragma once + +uint8_t Stk_SignOn(void); +uint8_t Stk_ConnectEx(uint8_32_u *pDeviceInfo); +uint8_t Stk_ReadEEprom(ioMem_t *pMem); +uint8_t Stk_WriteEEprom(ioMem_t *pMem); +uint8_t Stk_ReadFlash(ioMem_t *pMem); +uint8_t Stk_WriteFlash(ioMem_t *pMem); +uint8_t Stk_Chip_Erase(void); From 14296177a01c410a11813a23a70121c5665bd892 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 17 Dec 2023 23:43:53 +0100 Subject: [PATCH 02/28] missing headers --- lib/betaflight/src/drivers/buf_writer.h | 0 lib/betaflight/src/drivers/dshot.h | 0 lib/betaflight/src/drivers/io.h | 0 lib/betaflight/src/drivers/io_types.h | 0 lib/betaflight/src/drivers/light_led.h | 0 lib/betaflight/src/drivers/serial.h | 0 lib/betaflight/src/drivers/timer.h | 0 lib/betaflight/src/extract_headers.sh | 2 +- 8 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 lib/betaflight/src/drivers/buf_writer.h create mode 100644 lib/betaflight/src/drivers/dshot.h create mode 100644 lib/betaflight/src/drivers/io.h create mode 100644 lib/betaflight/src/drivers/io_types.h create mode 100644 lib/betaflight/src/drivers/light_led.h create mode 100644 lib/betaflight/src/drivers/serial.h create mode 100644 lib/betaflight/src/drivers/timer.h diff --git a/lib/betaflight/src/drivers/buf_writer.h b/lib/betaflight/src/drivers/buf_writer.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/dshot.h b/lib/betaflight/src/drivers/dshot.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/io.h b/lib/betaflight/src/drivers/io.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/io_types.h b/lib/betaflight/src/drivers/io_types.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/light_led.h b/lib/betaflight/src/drivers/light_led.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/serial.h b/lib/betaflight/src/drivers/serial.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/drivers/timer.h b/lib/betaflight/src/drivers/timer.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/extract_headers.sh b/lib/betaflight/src/extract_headers.sh index 8f1884ea..ffd6eafd 100755 --- a/lib/betaflight/src/extract_headers.sh +++ b/lib/betaflight/src/extract_headers.sh @@ -1,6 +1,6 @@ #!/bin/bash -FILES=$(grep -R '#include "' blackbox | sed -e 's/.*"\(.*\)"/\1/' | uniq) +FILES=$(grep -R '#include "' . | sed -e 's/.*"\(.*\)"/\1/' | uniq) for f in $FILES do From dc4161e457432c748146b05e6e1e1561e6cae361 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 18 Dec 2023 01:35:18 +0100 Subject: [PATCH 03/28] initial adaptation --- lib/Espfc/src/Blackbox.h | 38 ++------- lib/Espfc/src/Msp/MspProcessor.h | 23 ++++++ lib/betaflight/src/platform.c | 59 ++++---------- lib/betaflight/src/platform.h | 52 ++++++++++++- lib/betaflight/src/platform_cpp.cpp | 116 ++++++++++++++++++++++++++++ 5 files changed, 213 insertions(+), 75 deletions(-) create mode 100644 lib/betaflight/src/platform_cpp.cpp diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index 08d10083..2d036ef7 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -52,34 +52,8 @@ class BlackboxBuffer uint8_t _data[SIZE]; }; -static BlackboxBuffer * blackboxSerial = nullptr; static Espfc::Model * _model_ptr = nullptr; -void serialWrite(serialPort_t * instance, uint8_t ch) -{ - UNUSED(instance); - if(blackboxSerial) blackboxSerial->write(ch); -} - -void serialWriteInit(BlackboxBuffer * serial) -{ - blackboxSerial = serial; -} - -uint32_t serialTxBytesFree(const serialPort_t * instance) -{ - UNUSED(instance); - if(!blackboxSerial) return 0; - return blackboxSerial->availableForWrite(); -} - -bool isSerialTransmitBufferEmpty(const serialPort_t * instance) -{ - UNUSED(instance); - if(!blackboxSerial) return false; - return blackboxSerial->isTxFifoEmpty(); -} - void initBlackboxModel(Espfc::Model * m) { _model_ptr = m; @@ -174,11 +148,12 @@ class Blackbox if(!_model.blackboxEnabled()) return 0; - Device::SerialDevice * serial = _model.getSerialStream(SERIAL_FUNCTION_BLACKBOX); - if(!serial) return 0; + _serial = _model.getSerialStream(SERIAL_FUNCTION_BLACKBOX); + if(!_serial) return 0; - _buffer.begin(serial); - serialWriteInit(&_buffer); + //_buffer.begin(_serial); + //serialBlackboxInit(&_buffer); + serialDeviceInit(_serial, 0); systemConfigMutable()->activeRateProfile = 0; systemConfigMutable()->debug_mode = debugMode = _model.config.debugMode; @@ -322,7 +297,7 @@ class Blackbox int update() { if(!_model.blackboxEnabled()) return 0; - if(!blackboxSerial) return 0; + if(!_serial) return 0; Stats::Measure measure(_model.state.stats, COUNTER_BLACKBOX); uint32_t startTime = micros(); @@ -432,6 +407,7 @@ class Blackbox Model& _model; pidProfile_s _pidProfile; + Device::SerialDevice * _serial; BlackboxBuffer _buffer; }; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 7df43558..34c3ac2d 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -8,6 +8,7 @@ #include "platform.h" extern "C" { + #include "io/serial_4way.h" int blackboxCalculatePDenom(int rateNum, int rateDenom); uint8_t blackboxCalculateSampleRate(uint16_t pRatio); uint8_t blackboxGetRateDenom(void); @@ -146,6 +147,8 @@ static uint16_t toIbatCurrent(float current) } +#define MSP_PASSTHROUGH_ESC_4WAY 0xff + namespace Espfc { namespace Msp { @@ -1357,6 +1360,26 @@ class MspProcessor } break; + case MSP_SET_PASSTHROUGH: + { + uint8_t ptMode = MSP_PASSTHROUGH_ESC_4WAY; + uint8_t ptArg = 0; + if(m.remain() >= 2) { + ptMode = m.readU8(); + ptArg = m.readU8(); + } + switch (ptMode) + { + case MSP_PASSTHROUGH_ESC_4WAY: + r.writeU8(esc4wayInit()); + break; + default: + r.writeU8(0); + break; + } + (void)ptArg; + } + case MSP_DEBUG: for (int i = 0; i < 4; i++) { r.writeU16(_model.state.debug[i]); diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 1adb1d49..9ffb12b6 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -68,14 +68,6 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS]; uint32_t targetPidLooptime; float rcCommand[4]; -static serialPort_t _sp = { - .txBufferSize = 128 -}; -static serialPortConfig_t _spc = { - .blackbox_baudrateIndex = 5, - .identifier = SERIAL_PORT_USART1 -}; - int gcd(int num, int denom) { if (denom == 0) return num; @@ -102,40 +94,6 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId) return bitArrayGet(&rcModeActivationMask, boxId); } -serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) -{ - return NULL; -} - -void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort) -{ - UNUSED(serialPort); -} - -serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) -{ - return &_spc; -} - -serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudrate, portMode_e mode, portOptions_e options) -{ - return &_sp; -} - -void closeSerialPort(serialPort_t *serialPort) -{ - UNUSED(serialPort); -} - -void mspSerialAllocatePorts(void) -{ -} - -portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) -{ - return PORTSHARING_UNUSED; -} - bool rxAreFlightChannelsValid(void) { return true; @@ -184,3 +142,20 @@ uint32_t zigzagEncode(int32_t value) { return (uint32_t)((value << 1) ^ (value >> 31)); } + +pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; + +pwmOutputPort_t * pwmGetMotors(void) +{ + return motors; +} + +void motorDisable(void) +{ + +} + +void motorEnable(void) +{ + +} \ No newline at end of file diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index d21a3ed2..d5cceb6a 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -241,6 +241,7 @@ typedef struct serialPort_s { uint32_t txBufferTail; serialReceiveCallbackPtr rxCallback; + void * espfcDevice; } serialPort_t; typedef enum { @@ -320,17 +321,23 @@ PG_DECLARE(serialConfig_t, serialConfig); extern const uint32_t baudRates[]; +void mspSerialAllocatePorts(void); +void mspSerialReleasePortIfAllocated(serialPort_t *serialPort); serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); -void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudrate, portMode_e mode, portOptions_e options); void closeSerialPort(serialPort_t *serialPort); -void mspSerialAllocatePorts(void); +uint32_t serialRxBytesWaiting(serialPort_t * instance); uint32_t serialTxBytesFree(const serialPort_t *instance); bool isSerialTransmitBufferEmpty(const serialPort_t *instance); portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function); +void serialDeviceInit(void * serial, size_t index); + +void serialBeginWrite(serialPort_t * instance); +void serialEndWrite(serialPort_t * instance); void serialWrite(serialPort_t *instance, uint8_t ch); +int serialRead(serialPort_t *instance); /* SERIAL END */ /* MIXER START */ @@ -1119,6 +1126,47 @@ int16_t getMotorOutputHigh(); #endif // USE_GPS_LAP_TIMER #endif +// ESC 4-Way IF + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define USE_SERIAL_4WAY_BLHELI_BOOTLOADER +#define USE_SERIAL_4WAY_SK_BOOTLOADER + +typedef int8_t IO_t; + +typedef struct { + bool enabled; + IO_t io; +} pwmOutputPort_t; + +pwmOutputPort_t * pwmGetMotors(void); +void motorDisable(void); +void motorEnable(void); + +#if defined(UNIT_TEST) || defined(ESP8266) || defined(ARCH_RP2040) +void delay(unsigned long ms); +void delayMicroseconds(unsigned int us); +#else +void delay(uint32_t ms); +void delayMicroseconds(uint32_t us); +#endif + +#define IOCFG_IPU 0x05 // INPUT_PULLUP +#define IOCFG_OUT_PP 0x03 // OUTPUT +#define IOCFG_AF_PP 0x03 // OUTPUT +#define Bit_RESET 0x00 // LOW +#define IO_NONE -1 + +int IORead(IO_t pin); +void IOConfigGPIO(IO_t pin, uint8_t mode); +void IOHi(IO_t pin); +void IOLo(IO_t pin); + +#define LED0_ON do {} while(false) +#define LED0_OFF do {} while(false) + +// end ESC 4-Way IF + #ifdef __cplusplus } #endif diff --git a/lib/betaflight/src/platform_cpp.cpp b/lib/betaflight/src/platform_cpp.cpp new file mode 100644 index 00000000..69591d71 --- /dev/null +++ b/lib/betaflight/src/platform_cpp.cpp @@ -0,0 +1,116 @@ +#include +#include "platform.h" +#include "Device/SerialDevice.h" + +int IORead(IO_t pin) +{ + return digitalRead(pin); +} + +void IOConfigGPIO(IO_t pin, uint8_t mode) +{ + pinMode(pin, mode); +} + +void IOHi(IO_t pin) +{ + digitalWrite(pin, HIGH); +} + +void IOLo(IO_t pin) +{ + digitalWrite(pin, LOW); +} + +static serialPort_t _sp[2] = {{ + .txBufferSize = 128 +}, { + .txBufferSize = 128 +}}; + +static serialPortConfig_t _spc = { + .identifier = SERIAL_PORT_USART1, + .blackbox_baudrateIndex = 5, +}; + +void serialDeviceInit(void * serial, size_t index) +{ + if(index > 1) return; + _sp[index].espfcDevice = serial; +} + +serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction) +{ + return NULL; +} + +void mspSerialReleasePortIfAllocated(serialPort_t *serialPort) +{ + UNUSED(serialPort); +} + +serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function) +{ + return &_spc; +} + +serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudrate, portMode_e mode, portOptions_e options) +{ + return &_sp[0]; +} + +void closeSerialPort(serialPort_t *serialPort) +{ + UNUSED(serialPort); +} + +void mspSerialAllocatePorts(void) +{ +} + +portSharing_e determinePortSharing(const serialPortConfig_t *portConfig, serialPortFunction_e function) +{ + return PORTSHARING_UNUSED; +} + +void serialBeginWrite(serialPort_t * instance) +{ +} + +void serialEndWrite(serialPort_t * instance) +{ +} + +void serialWrite(serialPort_t * instance, uint8_t ch) +{ + Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; + if(dev) dev->write(ch); +} + +uint32_t serialRxBytesWaiting(serialPort_t * instance) +{ + Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; + if(!dev) return 0; + return dev->available(); +} + +int serialRead(serialPort_t * instance) +{ + Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; + if(dev) return dev->read(); + return -1; +} + +uint32_t serialTxBytesFree(const serialPort_t * instance) +{ + Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; + if(!dev) return 0; + return dev->availableForWrite(); +} + +bool isSerialTransmitBufferEmpty(const serialPort_t * instance) +{ + Espfc::Device::SerialDevice * dev = (Espfc::Device::SerialDevice *)instance->espfcDevice; + if(!dev) return 0; + return dev->isTxFifoEmpty(); +} From b4b1c3048dcc581e45e7b9488f7e934ed030100a Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 18 Dec 2023 23:15:48 +0100 Subject: [PATCH 04/28] esp32 s3 support --- docs/connections.md | 59 ++++++++ lib/EscDriver/src/EscDriver.h | 9 ++ lib/EscDriver/src/EscDriverEsp32.h | 4 + lib/Espfc/src/Hardware.h | 2 +- lib/Espfc/src/Target/Target.h | 4 +- lib/Espfc/src/Target/TargetESP32s3.h | 204 +++++++++++++++++++++++++++ platformio.ini | 29 +++- 7 files changed, 306 insertions(+), 5 deletions(-) create mode 100644 lib/Espfc/src/Target/TargetESP32s3.h diff --git a/docs/connections.md b/docs/connections.md index f1af4aed..57fec585 100644 --- a/docs/connections.md +++ b/docs/connections.md @@ -138,4 +138,63 @@ https://github.com/espressif/esptool/wiki/ESP32-Boot-Mode-Selection CMD(11) SD3(10) | | SD0(07) CLK(06) +----|---|----+ USB +``` + +# ESP32-S3 + +https://docs.espressif.com/projects/esp-idf/en/latest/esp32s3/api-reference/peripherals/gpio.html + +``` +Pin | Function | ESPFC external device +----+------+---------------+---------------------------------------------- + 0 | Strapping | + 1 | ADC1_CH0 | ADC_VOLTAGE + 2 | ADC1_CH1 | DEBUG + 3 | ADC1_CH2, Strapping | + 4 | ADC1_CH3 | ADC_CURRENT + 5 | ADC1_CH4 | BUZZER + 6 | ADC1_CH5 | PPM + 7 | ADC1_CH6 | SPI_CS_BARO + 8 | ADC1_CH7 | SPI_CS_GYRO + 9 | ADC1_CH8 | I2C_0_SDA +10 | ADC1_CH9 | I2C_0_SCK +11 | ADC2_CH0 | SPI_0_MOSI +12 | ADC2_CH1 | SPI_0_SCK +13 | ADC2_CH2 | SPI_0_MISO +14 | ADC2_CH3 | ? +15 | ADC2_CH4 | RX1 +16 | ADC2_CH5 | TX1 +17 | ADC2_CH6 | RX2 +18 | ADC2_CH7 | TX2 +19 | ADC2_CH8, USB | USB +20 | ADC2_CH9, USB | USB +21 | RTC_21 | ? +38 | RGB | LED_RGB +39 | | M0 +40 | | M1 +41 | | M2 +42 | | M3 +43 | U1TX | TX0 +44 | U1RX | RX0 +45 | | ? +46 | Strapping | SPI_CS_SD +47 | Strapping | ? +48 | | ? + +RESERVED +26 | FLASH | +27 | FLASH | +28 | FLASH | +29 | FLASH | +30 | FLASH | +31 | FLASH | +32 | FLASH | +33 | FLASH | +34 | FLASH | +35 | FLASH | +36 | FLASH | +37 | FLASH | + +UNAVAILABLE +22, 23, 24, 25 ``` \ No newline at end of file diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 4613f504..3f38b841 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -69,6 +69,15 @@ class EscDriverBase #define ESC_DRIVER_MOTOR_TIMER ESC_DRIVER_TIMER0 #define ESC_DRIVER_SERVO_TIMER ESC_DRIVER_TIMER1 +#elif defined(ESP32S3) + + #define ESC_CHANNEL_COUNT 4 + #include "EscDriverEsp32.h" + #define EscDriver EscDriverEsp32 + + #define ESC_DRIVER_MOTOR_TIMER 0 + #define ESC_DRIVER_SERVO_TIMER 1 + #elif defined(ESP32) #define ESC_CHANNEL_COUNT RMT_CHANNEL_MAX diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 96076d6c..f47eb8c6 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -16,7 +16,11 @@ static const int32_t DURATION_MAX = 0x7fff; // max in 15 bits // faster esc response, but unsafe (no task synchronisation) // set to 0 in case of issues +#if defined(ESP32S3) +#define ESPFC_RMT_BYPASS_WRITE_SYNC 0 +#else #define ESPFC_RMT_BYPASS_WRITE_SYNC 1 +#endif #if ESPFC_RMT_BYPASS_WRITE_SYNC IRAM_ATTR static esp_err_t _rmt_fill_tx_items(rmt_channel_t channel, const rmt_item32_t* item, uint16_t item_num, uint16_t mem_offset) diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index 596fcf94..cb437c7d 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -27,7 +27,7 @@ namespace { #if defined(ESPFC_SPI_0) - #if defined(ESP32C3) + #if defined(ESP32C3) || defined(ESP32S3) static SPIClass SPI1(HSPI); #elif defined(ESP32) //where is this defined??? static SPIClass SPI1(VSPI); diff --git a/lib/Espfc/src/Target/Target.h b/lib/Espfc/src/Target/Target.h index 80cdfebf..44dc3f60 100644 --- a/lib/Espfc/src/Target/Target.h +++ b/lib/Espfc/src/Target/Target.h @@ -2,7 +2,9 @@ #if defined(ESP32C3) #include "TargetESP32c3.h" -#elif defined(ESP32) //where is this defined??? +#elif defined(ESP32S3) + #include "TargetESP32s3.h" +#elif defined(ESP32) #include "TargetESP32.h" #elif defined(ESP8266) #include "TargetESP8266.h" diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h new file mode 100644 index 00000000..421b6158 --- /dev/null +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -0,0 +1,204 @@ +#pragma once + +#include "Esp.h" +#include "Debug_Espfc.h" + +#define ESPFC_INPUT +#define ESPFC_INPUT_PIN 6 // ppm + +#define ESPFC_OUTPUT_COUNT 4 +#define ESPFC_OUTPUT_0 39 +#define ESPFC_OUTPUT_1 40 +#define ESPFC_OUTPUT_2 41 +#define ESPFC_OUTPUT_3 42 + +#define ESPFC_SERIAL_0 +#define ESPFC_SERIAL_0_DEV Serial0 +#define ESPFC_SERIAL_0_DEV_T HardwareSerial +#define ESPFC_SERIAL_0_TX 43 +#define ESPFC_SERIAL_0_RX 44 +#define ESPFC_SERIAL_0_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_0_BAUD (SERIAL_SPEED_115200) +#define ESPFC_SERIAL_0_BBAUD (SERIAL_SPEED_NONE) + +#define ESPFC_SERIAL_1 +#define ESPFC_SERIAL_1_DEV Serial1 +#define ESPFC_SERIAL_1_DEV_T HardwareSerial +#define ESPFC_SERIAL_1_TX 16 +#define ESPFC_SERIAL_1_RX 15 +#define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200) +#define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) + +#define ESPFC_SERIAL_2 +#define ESPFC_SERIAL_2_DEV Serial2 +#define ESPFC_SERIAL_2_DEV_T HardwareSerial +#define ESPFC_SERIAL_2_TX 18 +#define ESPFC_SERIAL_2_RX 17 +#define ESPFC_SERIAL_2_FN (SERIAL_FUNCTION_RX_SERIAL) +#define ESPFC_SERIAL_2_BAUD (SERIAL_SPEED_115200) +#define ESPFC_SERIAL_2_BBAUD (SERIAL_SPEED_NONE) + +#define ESPFC_SERIAL_USB +#define ESPFC_SERIAL_USB_DEV Serial +#define ESPFC_SERIAL_USB_DEV_T HWCDC +#define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) + +#define ESPFC_SERIAL_SOFT_0 +#define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_SOFT_0_WIFI + +#define ESPFC_SERIAL_REMAP_PINS +#define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 +#define SERIAL_TX_FIFO_SIZE 0xFF + +#define ESPFC_SPI_0 +#define ESPFC_SPI_0_DEV SPI1 +#define ESPFC_SPI_0_SCK 12 +#define ESPFC_SPI_0_MOSI 11 +#define ESPFC_SPI_0_MISO 13 + +#define ESPFC_SPI_CS_GYRO 8 +#define ESPFC_SPI_CS_BARO 7 + +#define ESPFC_I2C_0 +#define ESPFC_I2C_0_SCL 10 +#define ESPFC_I2C_0_SDA 9 +#define ESPFC_I2C_0_SOFT + +#define ESPFC_BUZZER +#define ESPFC_BUZZER_PIN 5 + +#define ESPFC_ADC_0 +#define ESPFC_ADC_0_PIN 1 + +#define ESPFC_ADC_1 +#define ESPFC_ADC_1_PIN 4 + +#define ESPFC_ADC_SCALE (3.3f / 4096) + +#define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) + +#define ESPFC_GUARD 0 + +#define ESPFC_GYRO_I2C_RATE_MAX 2000 +#define ESPFC_GYRO_SPI_RATE_MAX 8000 + +//#define ESPFC_LOGGER_FS // doesn't compile on ESP32 + +#define ESPFC_FREE_RTOS +#ifndef CONFIG_FREERTOS_UNICORE + #define ESPFC_MULTI_CORE +#endif + +//#define ESPFC_FREE_RTOS_QUEUE +#define ESPFC_ATOMIC_QUEUE + + +#define ESPFC_DSP + +#include "Device/SerialDevice.h" + +#define SERIAL_UART_PARITY_NONE 0B00000000 +#define SERIAL_UART_PARITY_EVEN 0B00000010 +#define SERIAL_UART_PARITY_ODD 0B00000011 + +#define SERIAL_UART_NB_BIT_5 0B00000000 +#define SERIAL_UART_NB_BIT_6 0B00000100 +#define SERIAL_UART_NB_BIT_7 0B00001000 +#define SERIAL_UART_NB_BIT_8 0B00001100 + +#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 +#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 +#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 +#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 + +namespace Espfc { + +template +inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) +{ + uint32_t sc = 0x8000000; + switch(conf.data_bits) + { + case 8: sc |= SERIAL_UART_NB_BIT_8; break; + case 7: sc |= SERIAL_UART_NB_BIT_7; break; + case 6: sc |= SERIAL_UART_NB_BIT_6; break; + case 5: sc |= SERIAL_UART_NB_BIT_5; break; + default: sc |= SERIAL_UART_NB_BIT_8; break; + } + switch(conf.parity) + { + case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; + case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; + default: sc |= SERIAL_UART_PARITY_NONE; break; + } + switch(conf.stop_bits) + { + case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; + case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; + case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; + default: break; + } + dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); + dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); + return 1; +} + +template<> +inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) +{ + dev.begin(conf.baud); + //while(!dev) delay(10); + return 1; +} + +template +inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) +{ + dev.begin(sck, miso, mosi, ss); + return 1; +} + +template +inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) +{ + dev.begin(sda, scl, speed); + dev.setTimeout(50); + return 1; +} + +inline uint32_t getBoardId0() +{ + const int64_t mac = ESP.getEfuseMac(); + return (uint32_t)mac; +} + +inline uint32_t getBoardId1() +{ + const int64_t mac = ESP.getEfuseMac(); + return (uint32_t)(mac >> 32); +} + +inline uint32_t getBoardId2() +{ + return 0; +} + +inline void targetReset() +{ + ESP.restart(); + while(1) {} +} + +inline uint32_t targetCpuFreq() +{ + return ESP.getCpuFreqMHz(); +} + +inline uint32_t targetFreeHeap() +{ + return ESP.getFreeHeap(); +} + +}; diff --git a/platformio.ini b/platformio.ini index 5f63a83a..8a625212 100644 --- a/platformio.ini +++ b/platformio.ini @@ -58,6 +58,29 @@ monitor_speed = ${common.esp32_monitor_speed} monitor_port = ${common.esp32_monitor_port} ;extra_scripts = merge_firmware.py +[env:esp32s3] +board = lolin_s3 +platform = espressif32 +framework = arduino +board_build.f_cpu = 240000000L +board_build.f_flash = 80000000L +board_build.flash_mode = qio +lib_deps = +build_flags = + ${common.build_flags} + ${common.esp32_build_flags} + -DESP32S3 + -DARDUINO_USB_MODE=1 + -DARDUINO_USB_CDC_ON_BOOT=1 +#upload_speed = ${common.esp32_upload_speed} +#upload_port = ${common.esp32_upload_port} +monitor_speed = ${common.esp32_monitor_speed} +monitor_port = ${common.esp32_monitor_port} +;extra_scripts = merge_firmware.py +board_upload.use_1200bps_touch = yes +board_upload.wait_for_upload_port = yes +board_upload.require_upload_port = yes + [env:esp32c3] board = esp32-c3-devkitm-1 platform = espressif32 @@ -69,10 +92,10 @@ board_build.flash_mode = qio lib_deps = build_flags = ${common.build_flags} - -DESP32C3 - -DARDUINO_USB_MODE - -DARDUINO_USB_CDC_ON_BOOT ${common.esp32_build_flags} + -DESP32C3 + -DARDUINO_USB_MODE=1 + -DARDUINO_USB_CDC_ON_BOOT=1 #upload_speed = ${common.esp32_upload_speed} #upload_port = ${common.esp32_upload_port} monitor_speed = ${common.esp32_monitor_speed} From 281da100ef2c8aa998f14cdeba6458885fdfd4a6 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 18 Dec 2023 23:19:22 +0100 Subject: [PATCH 05/28] esp32 s3 - CI --- .github/workflows/platformio.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index a5389598..62151201 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -40,7 +40,7 @@ jobs: runs-on: ubuntu-22.04 strategy: matrix: - target: ['esp32', 'esp32c3', 'esp8266', 'rp2040'] + target: ['esp32', 'esp32s3', 'esp32c3', 'esp8266', 'rp2040'] steps: - uses: actions/checkout@v3 - name: Extract Version From 15b982d6aabc1c551a2475840f2efdc5ee897b2f Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 20 Dec 2023 21:11:40 +0100 Subject: [PATCH 06/28] ESP32-S2 support --- .github/workflows/platformio.yml | 2 +- lib/EscDriver/src/EscDriver.h | 2 +- lib/EscDriver/src/EscDriverEsp32.cpp | 2 +- lib/EscDriver/src/EscDriverEsp32.h | 4 +- lib/EspWire/src/esp_twi.cpp | 5 +- lib/Espfc/src/Hardware.h | 4 +- lib/Espfc/src/Target/Target.h | 2 + lib/Espfc/src/Target/TargetESP32s2.h | 194 +++++++++++++++++++++++++++ lib/betaflight/src/platform.h | 6 +- platformio.ini | 69 ++++------ 10 files changed, 237 insertions(+), 53 deletions(-) create mode 100644 lib/Espfc/src/Target/TargetESP32s2.h diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 62151201..43186115 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -40,7 +40,7 @@ jobs: runs-on: ubuntu-22.04 strategy: matrix: - target: ['esp32', 'esp32s3', 'esp32c3', 'esp8266', 'rp2040'] + target: ['esp32', 'esp32s2', 'esp32s3', 'esp32c3', 'esp8266', 'rp2040'] steps: - uses: actions/checkout@v3 - name: Extract Version diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 3f38b841..dfe58a7f 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -69,7 +69,7 @@ class EscDriverBase #define ESC_DRIVER_MOTOR_TIMER ESC_DRIVER_TIMER0 #define ESC_DRIVER_SERVO_TIMER ESC_DRIVER_TIMER1 -#elif defined(ESP32S3) +#elif defined(ESP32S3) || defined(ESP32S2) #define ESC_CHANNEL_COUNT 4 #include "EscDriverEsp32.h" diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 55b96446..ab380e5c 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -1,4 +1,4 @@ -#if defined(ESP32) and not defined(ESP32C3) //where is this defined??? +#if defined(ESP32) && !defined(ESP32C3) #include "EscDriverEsp32.h" diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index f47eb8c6..0fe1b049 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -1,7 +1,7 @@ #ifndef _ESC_DRIVER_ESP32_H_ #define _ESC_DRIVER_ESP32_H_ -#if defined(ESP32) and not defined(ESP32C3) //where is this defined??? +#if defined(ESP32) && !defined(ESP32C3) #include "EscDriver.h" #include @@ -16,7 +16,7 @@ static const int32_t DURATION_MAX = 0x7fff; // max in 15 bits // faster esc response, but unsafe (no task synchronisation) // set to 0 in case of issues -#if defined(ESP32S3) +#if defined(ESP32S3) || defined(ESP32S2) #define ESPFC_RMT_BYPASS_WRITE_SYNC 0 #else #define ESPFC_RMT_BYPASS_WRITE_SYNC 1 diff --git a/lib/EspWire/src/esp_twi.cpp b/lib/EspWire/src/esp_twi.cpp index c8b3c254..4740dfcb 100644 --- a/lib/EspWire/src/esp_twi.cpp +++ b/lib/EspWire/src/esp_twi.cpp @@ -39,7 +39,7 @@ static uint32_t esp_twi_clockStretchLimit; #define SCL_LOW() (GPES = (1 << esp_twi_scl)) #define SCL_HIGH() (GPEC = (1 << esp_twi_scl)) #define SCL_READ() ((GPI & (1 << esp_twi_scl)) != 0) -#elif defined(ESP32C3) //where is this defined??? +#elif defined(ESP32C3) //#define SDA_LOW() (GPIO.enable_w1ts.enable_w1ts = ((uint32_t) 1 << esp_twi_sda)) //Enable SDA (becomes output and since GPO is 0 for the pin, it will pull the line low) static inline void SDA_LOW() { GPIO.enable_w1ts.enable_w1ts = (uint32_t) 1 << esp_twi_sda; GPIO.out_w1tc.out_w1tc = (uint32_t) 1 << esp_twi_sda; } #define SDA_HIGH() (GPIO.enable_w1tc.enable_w1tc = ((uint32_t) 1 << esp_twi_sda)) //Disable SDA (becomes input and since it has pullup it will go high) @@ -184,8 +184,7 @@ void esp_twi_stop(void){ static inline IRAM_ATTR unsigned int _getCycleCount() { - -#if defined(ESP32C3) //where is this defined??? +#if defined(ESP32C3) return esp_cpu_get_ccount(); #elif defined(ESP32) || defined(ESP8266) unsigned int ccount = 0; diff --git a/lib/Espfc/src/Hardware.h b/lib/Espfc/src/Hardware.h index cb437c7d..116e107b 100644 --- a/lib/Espfc/src/Hardware.h +++ b/lib/Espfc/src/Hardware.h @@ -27,9 +27,9 @@ namespace { #if defined(ESPFC_SPI_0) - #if defined(ESP32C3) || defined(ESP32S3) + #if defined(ESP32C3) || defined(ESP32S3) || defined(ESP32S2) static SPIClass SPI1(HSPI); - #elif defined(ESP32) //where is this defined??? + #elif defined(ESP32) static SPIClass SPI1(VSPI); #endif static Espfc::Device::BusSPI spiBus(ESPFC_SPI_0_DEV); diff --git a/lib/Espfc/src/Target/Target.h b/lib/Espfc/src/Target/Target.h index 44dc3f60..4b6343fe 100644 --- a/lib/Espfc/src/Target/Target.h +++ b/lib/Espfc/src/Target/Target.h @@ -2,6 +2,8 @@ #if defined(ESP32C3) #include "TargetESP32c3.h" +#elif defined(ESP32S2) + #include "TargetESP32s2.h" #elif defined(ESP32S3) #include "TargetESP32s3.h" #elif defined(ESP32) diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h new file mode 100644 index 00000000..0a6b19d8 --- /dev/null +++ b/lib/Espfc/src/Target/TargetESP32s2.h @@ -0,0 +1,194 @@ +#pragma once + +#include "Esp.h" +#include "Debug_Espfc.h" + +#define ESPFC_INPUT +#define ESPFC_INPUT_PIN 6 // ppm + +#define ESPFC_OUTPUT_COUNT 4 +#define ESPFC_OUTPUT_0 39 +#define ESPFC_OUTPUT_1 40 +#define ESPFC_OUTPUT_2 41 +#define ESPFC_OUTPUT_3 42 + +#define ESPFC_SERIAL_0 +#define ESPFC_SERIAL_0_DEV Serial0 +#define ESPFC_SERIAL_0_DEV_T HardwareSerial +#define ESPFC_SERIAL_0_TX 43 +#define ESPFC_SERIAL_0_RX 44 +#define ESPFC_SERIAL_0_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_0_BAUD (SERIAL_SPEED_115200) +#define ESPFC_SERIAL_0_BBAUD (SERIAL_SPEED_NONE) + +#define ESPFC_SERIAL_1 +#define ESPFC_SERIAL_1_DEV Serial1 +#define ESPFC_SERIAL_1_DEV_T HardwareSerial +#define ESPFC_SERIAL_1_TX 16 +#define ESPFC_SERIAL_1_RX 15 +#define ESPFC_SERIAL_1_FN (SERIAL_FUNCTION_RX_SERIAL) +#define ESPFC_SERIAL_1_BAUD (SERIAL_SPEED_115200) +#define ESPFC_SERIAL_1_BBAUD (SERIAL_SPEED_NONE) + +#define ESPFC_SERIAL_USB +#define ESPFC_SERIAL_USB_DEV Serial +#define ESPFC_SERIAL_USB_DEV_T USBCDC +#define ESPFC_SERIAL_USB_FN (SERIAL_FUNCTION_MSP) + +#define ESPFC_SERIAL_SOFT_0 +#define ESPFC_SERIAL_SOFT_0_FN (SERIAL_FUNCTION_MSP) +#define ESPFC_SERIAL_SOFT_0_WIFI + +#define ESPFC_SERIAL_REMAP_PINS +#define ESPFC_SERIAL_DEBUG_PORT SERIAL_UART_0 +#define SERIAL_TX_FIFO_SIZE 0xFF + +#define ESPFC_SPI_0 +#define ESPFC_SPI_0_DEV SPI1 +#define ESPFC_SPI_0_SCK 12 +#define ESPFC_SPI_0_MOSI 11 +#define ESPFC_SPI_0_MISO 13 + +#define ESPFC_SPI_CS_GYRO 8 +#define ESPFC_SPI_CS_BARO 7 + +#define ESPFC_I2C_0 +#define ESPFC_I2C_0_SCL 10 +#define ESPFC_I2C_0_SDA 9 +#define ESPFC_I2C_0_SOFT + +#define ESPFC_BUZZER +#define ESPFC_BUZZER_PIN 5 + +#define ESPFC_ADC_0 +#define ESPFC_ADC_0_PIN 1 + +#define ESPFC_ADC_1 +#define ESPFC_ADC_1_PIN 4 + +#define ESPFC_ADC_SCALE (3.3f / 4096) + +#define ESPFC_FEATURE_MASK (FEATURE_RX_SERIAL | FEATURE_DYNAMIC_FILTER) + +#define ESPFC_GUARD 0 + +#define ESPFC_GYRO_I2C_RATE_MAX 2000 +#define ESPFC_GYRO_SPI_RATE_MAX 2000 + +//#define ESPFC_LOGGER_FS // doesn't compile on ESP32 + +#define ESPFC_FREE_RTOS +#ifndef CONFIG_FREERTOS_UNICORE + #define ESPFC_MULTI_CORE +#endif + +//#define ESPFC_FREE_RTOS_QUEUE +//#define ESPFC_ATOMIC_QUEUE + +#define ESPFC_DSP + +#include "Device/SerialDevice.h" + +#define SERIAL_UART_PARITY_NONE 0B00000000 +#define SERIAL_UART_PARITY_EVEN 0B00000010 +#define SERIAL_UART_PARITY_ODD 0B00000011 + +#define SERIAL_UART_NB_BIT_5 0B00000000 +#define SERIAL_UART_NB_BIT_6 0B00000100 +#define SERIAL_UART_NB_BIT_7 0B00001000 +#define SERIAL_UART_NB_BIT_8 0B00001100 + +#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 +#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 +#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 +#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 + +namespace Espfc { + +template +inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) +{ + uint32_t sc = 0x8000000; + switch(conf.data_bits) + { + case 8: sc |= SERIAL_UART_NB_BIT_8; break; + case 7: sc |= SERIAL_UART_NB_BIT_7; break; + case 6: sc |= SERIAL_UART_NB_BIT_6; break; + case 5: sc |= SERIAL_UART_NB_BIT_5; break; + default: sc |= SERIAL_UART_NB_BIT_8; break; + } + switch(conf.parity) + { + case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; + case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; + default: sc |= SERIAL_UART_PARITY_NONE; break; + } + switch(conf.stop_bits) + { + case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; + case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; + case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; + default: break; + } + dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); + dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); + return 1; +} + +template<> +inline int targetSerialInit(USBCDC& dev, const SerialDeviceConfig& conf) +{ + dev.begin(conf.baud); + //while(!dev) delay(10); + return 1; +} + +template +inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) +{ + dev.begin(sck, miso, mosi, ss); + return 1; +} + +template +inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) +{ + dev.begin(sda, scl, speed); + dev.setTimeout(50); + return 1; +} + +inline uint32_t getBoardId0() +{ + const int64_t mac = ESP.getEfuseMac(); + return (uint32_t)mac; +} + +inline uint32_t getBoardId1() +{ + const int64_t mac = ESP.getEfuseMac(); + return (uint32_t)(mac >> 32); +} + +inline uint32_t getBoardId2() +{ + return 0; +} + +inline void targetReset() +{ + ESP.restart(); + while(1) {} +} + +inline uint32_t targetCpuFreq() +{ + return ESP.getCpuFreqMHz(); +} + +inline uint32_t targetFreeHeap() +{ + return ESP.getFreeHeap(); +} + +}; diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index d21a3ed2..c36e2ba4 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -16,7 +16,11 @@ #if defined(ESP8266) #define ESPFC_TARGET "ESP8266" -#elif defined(ESP32C3) //where is this defined??? +#elif defined(ESP32S3) +#define ESPFC_TARGET "ESP32S3" +#elif defined(ESP32S2) +#define ESPFC_TARGET "ESP32S2" +#elif defined(ESP32C3) #define ESPFC_TARGET "ESP32C3" #elif defined(ESP32) #define ESPFC_TARGET "ESP32" diff --git a/platformio.ini b/platformio.ini index 8a625212..2eeefb4a 100644 --- a/platformio.ini +++ b/platformio.ini @@ -10,7 +10,7 @@ [platformio] default_envs = esp32 -[common] +[env] build_flags = -std=c++14 -Wall @@ -25,19 +25,8 @@ build_flags = ; -DESPFC_DEV_PRESET_DSHOT ; -DESPFC_DEV_PRESET_SCALER -esp8266_upload_port = /dev/ttyUSB0 -esp8266_upload_speed = 460800 -esp8266_monitor_port = /dev/ttyUSB0 -esp8266_monitor_speed = 115200 -esp8266_build_flags = - -esp32_upload_port = /dev/ttyUSB0 -esp32_upload_speed = 921600 -esp32_monitor_port = /dev/ttyUSB0 -esp32_monitor_speed = 115200 -esp32_build_flags = - -[env] +monitor_speed = 115200 +upload_speed = 921600 ; monitor_filters = esp8266_exception_decoder ; monitor_filters = esp32_exception_decoder @@ -50,12 +39,7 @@ board_build.f_flash = 80000000L board_build.flash_mode = qio lib_deps = build_flags = - ${common.build_flags} - ${common.esp32_build_flags} -upload_speed = ${common.esp32_upload_speed} -upload_port = ${common.esp32_upload_port} -monitor_speed = ${common.esp32_monitor_speed} -monitor_port = ${common.esp32_monitor_port} + ${env.build_flags} ;extra_scripts = merge_firmware.py [env:esp32s3] @@ -67,15 +51,28 @@ board_build.f_flash = 80000000L board_build.flash_mode = qio lib_deps = build_flags = - ${common.build_flags} - ${common.esp32_build_flags} + ${env.build_flags} -DESP32S3 -DARDUINO_USB_MODE=1 -DARDUINO_USB_CDC_ON_BOOT=1 -#upload_speed = ${common.esp32_upload_speed} -#upload_port = ${common.esp32_upload_port} -monitor_speed = ${common.esp32_monitor_speed} -monitor_port = ${common.esp32_monitor_port} +;extra_scripts = merge_firmware.py +board_upload.use_1200bps_touch = yes +board_upload.wait_for_upload_port = yes +board_upload.require_upload_port = yes + +[env:esp32s2] +board = esp32-s2-saola-1 +platform = espressif32 +framework = arduino +board_build.f_cpu = 240000000L +board_build.f_flash = 80000000L +board_build.flash_mode = qio +lib_deps = +build_flags = + ${env.build_flags} + -DESP32S2 + -DARDUINO_USB_MODE=0 + -DARDUINO_USB_CDC_ON_BOOT=1 ;extra_scripts = merge_firmware.py board_upload.use_1200bps_touch = yes board_upload.wait_for_upload_port = yes @@ -91,15 +88,10 @@ board_build.f_flash = 80000000L board_build.flash_mode = qio lib_deps = build_flags = - ${common.build_flags} - ${common.esp32_build_flags} + ${env.build_flags} -DESP32C3 -DARDUINO_USB_MODE=1 -DARDUINO_USB_CDC_ON_BOOT=1 -#upload_speed = ${common.esp32_upload_speed} -#upload_port = ${common.esp32_upload_port} -monitor_speed = ${common.esp32_monitor_speed} -monitor_port = ${common.esp32_monitor_port} [env:esp8266] board = d1_mini @@ -113,12 +105,7 @@ board_build.ldscript = eagle.flash.4m3m.ld ;board_build.ldscript = eagle.flash.16m15m.ld ; d1_mini_pro lib_deps = build_flags = - ${common.build_flags} - ${common.esp8266_build_flags} -upload_port = ${common.esp8266_upload_port} -upload_speed = ${common.esp8266_upload_speed} -monitor_port = ${common.esp8266_monitor_port} -monitor_speed = ${common.esp8266_monitor_speed} + ${env.build_flags} [env:rp2040] board = pico @@ -126,19 +113,17 @@ platform = https://github.com/maxgerhardt/platform-raspberrypi.git framework = arduino board_build.core = earlephilhower build_flags = - ${common.build_flags} + ${env.build_flags} -DARCH_RP2040 -DIRAM_ATTR="" ; -DIRAM_ATTR='__attribute__ ((section(".time_critical.iram_attr")))' -monitor_speed = 115200 -upload_port = /dev/ttyACM0 [env:nrf52840] platform = https://github.com/maxgerhardt/platform-nordicnrf52 board = xiaoblesense framework = arduino build_flags = - ${common.build_flags} + ${env.build_flags} -DARCH_NRF52840 -DIRAM_ATTR="" From 4e24a83cb21dcd5dc969648604377ec70cd3989e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 27 Jan 2024 23:46:16 +0100 Subject: [PATCH 07/28] stats improvement --- lib/Espfc/src/Cli.h | 6 ++++++ lib/Espfc/src/Stats.h | 9 ++++++++- lib/Espfc/src/Target/TargetESP32s2.h | 1 - 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 2be914d7..1ac72eaa 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1206,6 +1206,7 @@ class Cli int time = lrintf(_model.state.stats.getTime(c)); float load = _model.state.stats.getLoad(c); int freq = lrintf(_model.state.stats.getFreq(c)); + int real = lrintf(_model.state.stats.getReal(c)); s.print(FPSTR(_model.state.stats.getName(c))); s.print(": "); @@ -1214,6 +1215,11 @@ class Cli s.print(time); s.print("us, "); + if(real < 100) s.print(' '); + if(real < 10) s.print(' '); + s.print(real); + s.print("us/i, "); + if(load < 10) s.print(' '); s.print(load, 1); s.print("%, "); diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 33dc25fa..e6bb4408 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -98,8 +98,9 @@ class Stats for(size_t i = 0; i < COUNTER_COUNT; i++) { _avg[i] = (float)_sum[i] / timer.delta; - _sum[i] = 0; _freq[i] = (float)_count[i] * 1e6 / timer.delta; + _real[i] = _count[i] > 0 ? ((float)_sum[i] / _count[i]) : 0.0f; + _sum[i] = 0; _count[i] = 0; } } @@ -117,6 +118,11 @@ class Stats return _avg[c] * timer.interval * 0.001f; } + float getReal(StatCounter c) const + { + return _real[c]; + } + float getFreq(StatCounter c) const { return _freq[c]; @@ -196,6 +202,7 @@ class Stats uint32_t _count[COUNTER_COUNT]; float _avg[COUNTER_COUNT]; float _freq[COUNTER_COUNT]; + float _real[COUNTER_COUNT]; uint32_t _loop_last; int32_t _loop_time; }; diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h index 0a6b19d8..07f690d3 100644 --- a/lib/Espfc/src/Target/TargetESP32s2.h +++ b/lib/Espfc/src/Target/TargetESP32s2.h @@ -44,7 +44,6 @@ #define SERIAL_TX_FIFO_SIZE 0xFF #define ESPFC_SPI_0 -#define ESPFC_SPI_0_DEV SPI1 #define ESPFC_SPI_0_SCK 12 #define ESPFC_SPI_0_MOSI 11 #define ESPFC_SPI_0_MISO 13 From b6887cd31b393f0667f4cdcadd784e74e8fa8daf Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 28 Jan 2024 01:06:00 +0100 Subject: [PATCH 08/28] log buffer --- lib/Espfc/src/Cli.h | 40 ++----- lib/Espfc/src/Debug_Espfc.h | 4 +- lib/Espfc/src/Logger.h | 230 +++++++++--------------------------- 3 files changed, 68 insertions(+), 206 deletions(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 1ac72eaa..1d3e7056 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -833,9 +833,9 @@ class Cli PSTR("available commands:"), PSTR(" help"), PSTR(" dump"), PSTR(" get param"), PSTR(" set param value ..."), PSTR(" cal [gyro]"), PSTR(" defaults"), PSTR(" save"), PSTR(" reboot"), PSTR(" scaler"), PSTR(" mixer"), - PSTR(" stats"), PSTR(" status"), PSTR(" devinfo"), PSTR(" version"), + PSTR(" stats"), PSTR(" status"), PSTR(" devinfo"), PSTR(" version"), PSTR(" logs"), //PSTR(" load"), PSTR(" eeprom"), - //PSTR(" fsinfo"), PSTR(" fsformat"), PSTR(" logs"), PSTR(" log"), + //PSTR(" fsinfo"), PSTR(" fsformat"), PSTR(" log"), NULL }; for(const char ** ptr = helps; *ptr; ptr++) @@ -1238,36 +1238,6 @@ class Cli s.print(F("%")); s.println(); } - else if(strcmp_P(cmd.args[0], PSTR("fsinfo")) == 0) - { - _model.logger.info(&s); - } - else if(strcmp_P(cmd.args[0], PSTR("fsformat")) == 0) - { - s.print(F("wait... ")); - _model.logger.format(); - s.println(F("OK")); - } - else if(strcmp_P(cmd.args[0], PSTR("logs")) == 0) - { - _model.logger.list(&s); - } - else if(strcmp_P(cmd.args[0], PSTR("log")) == 0) - { - if(!cmd.args[1]) - { - _model.logger.show(&s); - return; - } - int id = String(cmd.args[1]).toInt(); - if(!id) - { - s.println(F("invalid log id")); - s.println(); - return; - } - _model.logger.show(&s, id); - } else if(strcmp_P(cmd.args[0], PSTR("reboot")) == 0) { Hardware::restart(_model); @@ -1280,6 +1250,12 @@ class Cli { _active = false; } + else if(strcmp_P(cmd.args[0], PSTR("logs")) == 0) + { + s.print(_model.logger.c_str()); + s.print(PSTR("total: ")); + s.println(_model.logger.length()); + } else { s.print(F("unknown command: ")); diff --git a/lib/Espfc/src/Debug_Espfc.h b/lib/Espfc/src/Debug_Espfc.h index afc1401c..12772148 100644 --- a/lib/Espfc/src/Debug_Espfc.h +++ b/lib/Espfc/src/Debug_Espfc.h @@ -21,7 +21,8 @@ extern Stream * _debugStream; static inline void initDebugStream(Stream * p) { _debugStream = p; } #define LOG_SERIAL_INIT(p) _debugStream = p; -#define LOG_SERIAL_DEBUG(v) if(_debugStream) { _debugStream->print(' '); _debugStream->print(v); } +#define LOG_SERIAL_DEBUG(v) if(_debugStream) { _debugStream->print(v); } +#define LOG_SERIAL_DEBUG_HEX(v) if(_debugStream) { _debugStream->print(v, HEX); } template void D(T t) @@ -47,6 +48,7 @@ static inline void initDebugStream(Stream * p) {} #define LOG_SERIAL_INIT(p) #define LOG_SERIAL_DEBUG(v) +#define LOG_SERIAL_DEBUG_HEX(v) #define D(...) #endif diff --git a/lib/Espfc/src/Logger.h b/lib/Espfc/src/Logger.h index f34c5846..af58e5e6 100644 --- a/lib/Espfc/src/Logger.h +++ b/lib/Espfc/src/Logger.h @@ -4,226 +4,110 @@ #include #include "Debug_Espfc.h" -#if defined(ESPFC_LOGGER_FS) - #if defined(ESPFC_LOGGER_FS_ALT) - #include "FS.h" - #else - #include "SPIFFS.h" - #endif -#endif - namespace Espfc { class Logger { public: - int begin() + int begin(size_t size = 1024) { -#if defined(ESPFC_LOGGER_FS) - _valid = false; - SPIFFS.begin(); - int count = 0; - int first = 0; - int last = 0; - - Dir dir = SPIFFS.openDir(""); - while(dir.next()) - { - String fn = dir.fileName(); - int id = String(fn).toInt(); - if(!id) continue; - - last = max(last, id); - if(!first) first = id; - first = min(first, id); - count++; - } - int next = last + 1; - int remove = count > 20 && first; - - String name; - if(remove) - { - _mkname(name, first); - SPIFFS.remove(name); - } - - _mkname(_name, next); - _valid = true; - info().logln(F("LOG INIT")); - - if(remove) - { - info().log(F("LOG RM")).logln(name); - } -#endif + _size = size; + _tail = 0; + _buff = new char[size]; + _buff[0] = '\0'; return 1; } - void list(Stream * s) - { -#if defined(ESPFC_LOGGER_FS) - if(!s) return; - Dir dir = SPIFFS.openDir(""); - while(dir.next()) - { - s->print(dir.fileName()); - s->print(' '); - File f = dir.openFile("r"); - s->println(f.size()); - } -#endif - } - - void show(Stream * s, int i) + Logger& info() { - String name; - _mkname(name, i); - show(s, name); + LOG_SERIAL_DEBUG("I") + append('I'); + return *this; } - void show(Stream * s) + Logger& err() { - show(s, _name); + LOG_SERIAL_DEBUG("E") + append('E'); + return *this; } - void show(Stream * s, const String& name) + template + Logger& log(const T& v) { - if(!s) return; -#if defined(ESPFC_LOGGER_FS) - File f = SPIFFS.open(name, "r"); - if(!f) - { - s->println(F("Error reading file")); - return; - } - while(f.available()) - { - String line = f.readStringUntil('\n'); - s->println(line); - } -#endif + LOG_SERIAL_DEBUG(' ') + LOG_SERIAL_DEBUG(v) + append(' '); + append(String(v)); + return *this; } - bool format() + template + Logger& loghex(const T& v) { -#if defined(ESPFC_LOGGER_FS) - return SPIFFS.format(); -#else - return false; -#endif + LOG_SERIAL_DEBUG(' ') + LOG_SERIAL_DEBUG_HEX(v) + append(' '); + append(String(v, HEX)); + return *this; } - void info(Stream * s) + template + Logger& logln(const T& v) { -#if defined(ESPFC_LOGGER_FS) - if(!s) return; - FSInfo i; - SPIFFS.info(i); - s->print(F("total: ")); s->print(i.totalBytes / 1024); s->println(F(" kB")); - s->print(F(" used: ")); s->print(i.usedBytes / 1024); s->println(F(" kB")); - s->print(F("avail: ")); s->print((i.totalBytes - i.usedBytes) / 1024); s->println(F(" kB")); - s->print(F("block: ")); s->println(i.blockSize); - s->print(F(" page: ")); s->println(i.pageSize); - s->print(F("files: ")); s->println(i.maxOpenFiles); - s->print(F(" path: ")); s->println(i.maxPathLength); -#endif + LOG_SERIAL_DEBUG(' ') + LOG_SERIAL_DEBUG(v) + append(' '); + append(String(v)); + return endl(); } - Logger& info() + Logger& endl() { - LOG_SERIAL_DEBUG("I") -#if defined(ESPFC_LOGGER_FS) - if(!_available()) return *this; - File f = SPIFFS.open(_name, "a"); - if(f) - { - f.print(millis()); - f.print(F(" I")); - f.close(); - } -#endif + LOG_SERIAL_DEBUG('\r') + LOG_SERIAL_DEBUG('\n') + append('\r'); + append('\n'); return *this; } - Logger& err() + void append(const String& s) { - LOG_SERIAL_DEBUG("E") -#if defined(ESPFC_LOGGER_FS) - if(!_available()) return *this; - File f = SPIFFS.open(_name, "a"); - if(f) - { - f.print(millis()); - f.print(F(" E")); - f.close(); - } -#endif - return *this; + append(s.c_str(), s.length()); } - template - Logger& log(const T& v) + void append(const char * s, size_t len) { - LOG_SERIAL_DEBUG(v) -#if defined(ESPFC_LOGGER_FS) - if(!_available()) return *this; - File f = SPIFFS.open(_name, "a"); - if(f) + for(size_t i = 0; i < len; i++) { - f.print(' '); - f.print(v); - f.close(); + append(s[i]); } -#endif - return *this; } - template - Logger& logln(const T& v) + void append(char c) { - LOG_SERIAL_DEBUG(v) - LOG_SERIAL_DEBUG('\r') - LOG_SERIAL_DEBUG('\n') -#if defined(ESPFC_LOGGER_FS) - if(!_available()) return *this; - File f = SPIFFS.open(_name, "a"); - if(f) + if(_size > 0 && _tail < _size - 1) { - f.print(' '); - f.println(v); - f.close(); + _buff[_tail] = c; + _tail++; + _buff[_tail] = '\0'; } -#endif - return *this; } - bool _available() + const char * c_str() const { -#if defined(ESPFC_LOGGER_FS) - if(_valid) - { - FSInfo i; - SPIFFS.info(i); - _valid = i.totalBytes - i.usedBytes > 1024; // keep 1kB free space margin - } - return _valid; -#else - return false; -#endif + return _buff; } - void _mkname(String& name, int i) + const size_t length() const { - name = ""; - if(i < 10) name += "000"; - else if(i < 100) name += "00"; - else if(i < 1000) name += "0"; - name += i; + return _tail; } - String _name; - bool _valid; + private: + char * _buff; + size_t _size; + size_t _tail; }; } From 1f7163989c57089a4c73d9b3fe45c8704369d25d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 30 Jan 2024 15:07:19 +0100 Subject: [PATCH 09/28] reorder initialization flow --- lib/Espfc/src/Espfc.h | 9 +++++---- lib/Espfc/src/Model.h | 2 +- src/main.cpp | 19 +++++++++---------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/lib/Espfc/src/Espfc.h b/lib/Espfc/src/Espfc.h index 192f1b80..4ce7a28d 100644 --- a/lib/Espfc/src/Espfc.h +++ b/lib/Espfc/src/Espfc.h @@ -35,24 +35,25 @@ class Espfc int begin() { + _serial.begin(); + _model.logStorageResult(); _hardware.begin(); _model.begin(); - _mixer.begin(); + //_mixer.begin(); _sensor.begin(); _input.begin(); _actuator.begin(); _controller.begin(); _blackbox.begin(); - _model.state.buzzer.push(BEEPER_SYSTEM_INIT); return 1; } int beginOther() { - _serial.begin(); - _model.logStorageResult(); + _mixer.begin(); _buzzer.begin(); + _model.state.buzzer.push(BEEPER_SYSTEM_INIT); return 1; } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 23ffb852..9f6d6f06 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -153,7 +153,7 @@ class Model { if(state.gyroCalibrationState == CALIBRATION_SAVE) { - save(); + //save(); state.buzzer.push(BEEPER_GYRO_CALIBRATED); logger.info().log(F("GYRO BIAS")).log(degrees(state.gyroBias.x)).log(degrees(state.gyroBias.y)).logln(degrees(state.gyroBias.z)); } diff --git a/src/main.cpp b/src/main.cpp index 59ef21a4..c94ea18d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -37,7 +37,6 @@ Espfc::Espfc espfc; void otherTask(void *pvParameters) { - espfc.load(); espfc.beginOther(); xTaskNotifyGive(loopTaskHandle); while(true) @@ -47,19 +46,19 @@ Espfc::Espfc espfc; } void setup() { + espfc.load(); + espfc.begin(); disableCore0WDT(); - //xTaskCreatePinnedToCore(otherTask, "otherTask", 8192, NULL, 1, &otherTaskHandle, 0); // run on PRO(0) core, loopTask is on APP(1) xTaskCreateUniversal(otherTask, "otherTask", 8192, NULL, 1, &otherTaskHandle, 0); // run on PRO(0) core, loopTask is on APP(1) ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // wait for `otherTask` initialization - espfc.begin(); } void loop() { - const uint32_t timeout = millis() + 200; - while(millis() < timeout) - { + //const uint32_t timeout = millis() + 200; + //while(millis() < timeout) + //{ espfc.update(); - } + //} } #elif defined(ESPFC_MULTI_CORE_RP2040) @@ -68,14 +67,14 @@ Espfc::Espfc espfc; volatile static bool setup1Done = false; void setup1() { - espfc.load(); espfc.beginOther(); setup1Done = true; } void setup() { - while(!setup1Done); //wait for setup1() + espfc.load(); espfc.begin(); + while(!setup1Done); //wait for setup1() } void loop() { @@ -96,8 +95,8 @@ Espfc::Espfc espfc; void setup() { espfc.load(); - espfc.beginOther(); espfc.begin(); + espfc.beginOther(); } void loop() { From 2738d677826d8d6de9ba69581ef9578bfe44eda9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 30 Jan 2024 22:09:26 +0100 Subject: [PATCH 10/28] prevent arming if wifi active + adming disable text flags --- lib/Espfc/src/Cli.h | 22 ++++++++++++++++++++-- lib/Espfc/src/Wireless.h | 3 ++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 1d3e7056..6d7492ef 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1190,9 +1190,27 @@ class Cli s.print(F(", ")); s.println(_model.state.inputAutoFactor); + static const char* armingDisableNames[] = { + PSTR("NO_GYRO"), PSTR("FAILSAFE"), PSTR("RX_FAILSAFE"), PSTR("BAD_RX_RECOVERY"), + PSTR("BOXFAILSAFE"), PSTR("RUNAWAY_TAKEOFF"), PSTR("CRASH_DETECTED"), PSTR("THROTTLE"), + PSTR("ANGLE"), PSTR("BOOT_GRACE_TIME"), PSTR("NOPREARM"), PSTR("LOAD"), + PSTR("CALIBRATING"), PSTR("CLI"), PSTR("CMS_MENU"), PSTR("BST"), + PSTR("MSP"), PSTR("PARALYZE"), PSTR("GPS"), PSTR("RESC"), + PSTR("RPMFILTER"), PSTR("REBOOT_REQUIRED"), PSTR("DSHOT_BITBANG"), PSTR("ACC_CALIBRATION"), + PSTR("MOTOR_PROTOCOL"), PSTR("ARM_SWITCH") + }; + const size_t armingDisableNamesLength = sizeof(armingDisableNames) / sizeof(armingDisableNames[0]); + + s.println(); + s.print(F(" arming disabled:")); + for(size_t i = 0; i < armingDisableNamesLength; i++) + { + if(_model.state.armingDisabledFlags & (1 << i)) { + s.print(' '); + s.print(armingDisableNames[i]); + } + } s.println(); - s.print(F(" arming disabled: ")); - s.println(_model.state.armingDisabledFlags); } else if(strcmp_P(cmd.args[0], PSTR("stats")) == 0) { diff --git a/lib/Espfc/src/Wireless.h b/lib/Espfc/src/Wireless.h index 910ec46c..b233c066 100644 --- a/lib/Espfc/src/Wireless.h +++ b/lib/Espfc/src/Wireless.h @@ -25,7 +25,7 @@ class Wireless { #ifdef ESPFC_SERIAL_SOFT_0_WIFI int status = -1; - if(_model.config.wireless.mode != WIRELESS_MODE_NULL) + if(_model.config.wireless.mode == WIRELESS_MODE_STA) { using std::placeholders::_1; using std::placeholders::_2; @@ -33,6 +33,7 @@ class Wireless WiFi.persistent(false); WiFi.mode((WiFiMode_t)_model.config.wireless.mode); status = WiFi.begin(_model.config.wireless.ssid, _model.config.wireless.pass); + _model.setArmingDisabled(ARMING_DISABLED_PARALYZE, true); } else { From 84fbe9755923a4908c41e9703b03d7fd75a2ef77 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 30 Jan 2024 22:48:06 +0100 Subject: [PATCH 11/28] esp32s3 CI update --- .github/workflows/platformio.yml | 6 +++++- README.md | 8 ++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index d40bed95..8ca1584e 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -98,10 +98,14 @@ jobs: PLATFORMIO_BUILD_FLAGS: -DESPFC_REVISION=${{ env.build_sha }} -DESPFC_VERSION=${{ env.build_tag }} - name: Merge ESP32 Target - if: ${{ startsWith(matrix.target, 'esp32') }} + if: ${{ matrix.target == 'esp32' }} run: | python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ${{ matrix.target }} merge_bin -o .pio/build/${{ matrix.target }}/firmware_merged.bin --target-offset 0x0 --flash_mode keep --flash_freq keep --flash_size 4MB 0x1000 .pio/build/${{ matrix.target }}/bootloader.bin 0x8000 .pio/build/${{ matrix.target }}/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/${{ matrix.target }}/firmware.bin + - name: Merge ESP32-S3 Target + if: ${{ matrix.target == 'esp32s3' }} + run: | + python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ${{ matrix.target }} merge_bin -o .pio/build/${{ matrix.target }}/firmware_merged.bin --target-offset 0x0 --flash_mode keep --flash_freq keep --flash_size 16MB 0x0000 .pio/build/${{ matrix.target }}/bootloader.bin 0x8000 .pio/build/${{ matrix.target }}/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/${{ matrix.target }}/firmware.bin - name: Create Development Artifact uses: actions/upload-artifact@v4 diff --git a/README.md b/README.md index d9fa3ba0..1b3d127b 100644 --- a/README.md +++ b/README.md @@ -56,6 +56,14 @@ Here are more details about [how to setup](/docs/setup.md). [![ESP-FC example wiring diagrams](/docs/images/espfc_wiring_combined.png)](/docs/wiring.md) +## Supported chips + + - **ESP32** - recommended + - **ESP32-S2** - experimantal + - **ESP32-S3** - experimantal + - **ESP32-C3** - experimantal + - **RP2040** - experimantal + - **ESP8266** - deprecated, may stop being developed ## Supported protocols From 585e0c01f302954fb15ea6580ddf15450c343c12 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 30 Jan 2024 23:15:02 +0100 Subject: [PATCH 12/28] esp32 CI update --- .github/workflows/platformio.yml | 20 ++++++++++---------- merge_firmware.py | 2 +- platformio.ini | 7 ++++--- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/.github/workflows/platformio.yml b/.github/workflows/platformio.yml index 8ca1584e..23a1d63d 100644 --- a/.github/workflows/platformio.yml +++ b/.github/workflows/platformio.yml @@ -97,15 +97,15 @@ jobs: env: PLATFORMIO_BUILD_FLAGS: -DESPFC_REVISION=${{ env.build_sha }} -DESPFC_VERSION=${{ env.build_tag }} - - name: Merge ESP32 Target - if: ${{ matrix.target == 'esp32' }} - run: | - python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ${{ matrix.target }} merge_bin -o .pio/build/${{ matrix.target }}/firmware_merged.bin --target-offset 0x0 --flash_mode keep --flash_freq keep --flash_size 4MB 0x1000 .pio/build/${{ matrix.target }}/bootloader.bin 0x8000 .pio/build/${{ matrix.target }}/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/${{ matrix.target }}/firmware.bin +# - name: Merge ESP32 Target +# if: ${{ matrix.target == 'esp32' }} +# run: | +# python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ${{ matrix.target }} merge_bin -o .pio/build/${{ matrix.target }}/firmware_merged.bin --target-offset 0x0 --flash_mode keep --flash_freq keep --flash_size 4MB 0x1000 .pio/build/${{ matrix.target }}/bootloader.bin 0x8000 .pio/build/${{ matrix.target }}/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/${{ matrix.target }}/firmware.bin - - name: Merge ESP32-S3 Target - if: ${{ matrix.target == 'esp32s3' }} - run: | - python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ${{ matrix.target }} merge_bin -o .pio/build/${{ matrix.target }}/firmware_merged.bin --target-offset 0x0 --flash_mode keep --flash_freq keep --flash_size 16MB 0x0000 .pio/build/${{ matrix.target }}/bootloader.bin 0x8000 .pio/build/${{ matrix.target }}/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/${{ matrix.target }}/firmware.bin +# - name: Merge ESP32-S3 Target +# if: ${{ matrix.target == 'esp32s3' }} +# run: | +# python3 ~/.platformio/packages/tool-esptoolpy/esptool.py --chip ${{ matrix.target }} merge_bin -o .pio/build/${{ matrix.target }}/firmware_merged.bin --target-offset 0x0 --flash_mode keep --flash_freq keep --flash_size 16MB 0x0000 .pio/build/${{ matrix.target }}/bootloader.bin 0x8000 .pio/build/${{ matrix.target }}/partitions.bin 0xe000 ~/.platformio/packages/framework-arduinoespressif32/tools/partitions/boot_app0.bin 0x10000 .pio/build/${{ matrix.target }}/firmware.bin - name: Create Development Artifact uses: actions/upload-artifact@v4 @@ -119,7 +119,7 @@ jobs: if: ${{ !startsWith(github.ref, 'refs/tags/') && startsWith(matrix.target, 'esp32') }} with: name: "${{ env.build_file_devel }}_0x00.bin" - path: .pio/build/${{ matrix.target }}/firmware_merged.bin + path: .pio/build/${{ matrix.target }}/firmware_0x00.bin - name: Create Release Artifact @@ -134,4 +134,4 @@ jobs: if: ${{ startsWith(github.ref, 'refs/tags/') && startsWith(matrix.target, 'esp32') }} with: name: ${{ env.build_file_release }}_0x00.bin - path: .pio/build/${{ matrix.target }}/firmware_merged.bin + path: .pio/build/${{ matrix.target }}/firmware_0x00.bin diff --git a/merge_firmware.py b/merge_firmware.py index b405cbc5..acc3893b 100644 --- a/merge_firmware.py +++ b/merge_firmware.py @@ -1,7 +1,7 @@ Import("env") APP_BIN = "$BUILD_DIR/${PROGNAME}.bin" -MERGED_BIN = "$BUILD_DIR/${PROGNAME}_esp32_0x00.bin" +MERGED_BIN = "$BUILD_DIR/${PROGNAME}_0x00.bin" BOARD_CONFIG = env.BoardConfig() diff --git a/platformio.ini b/platformio.ini index 2eeefb4a..d6c6ccd6 100644 --- a/platformio.ini +++ b/platformio.ini @@ -40,7 +40,7 @@ board_build.flash_mode = qio lib_deps = build_flags = ${env.build_flags} -;extra_scripts = merge_firmware.py +extra_scripts = merge_firmware.py [env:esp32s3] board = lolin_s3 @@ -55,7 +55,7 @@ build_flags = -DESP32S3 -DARDUINO_USB_MODE=1 -DARDUINO_USB_CDC_ON_BOOT=1 -;extra_scripts = merge_firmware.py +extra_scripts = merge_firmware.py board_upload.use_1200bps_touch = yes board_upload.wait_for_upload_port = yes board_upload.require_upload_port = yes @@ -73,7 +73,7 @@ build_flags = -DESP32S2 -DARDUINO_USB_MODE=0 -DARDUINO_USB_CDC_ON_BOOT=1 -;extra_scripts = merge_firmware.py +extra_scripts = merge_firmware.py board_upload.use_1200bps_touch = yes board_upload.wait_for_upload_port = yes board_upload.require_upload_port = yes @@ -92,6 +92,7 @@ build_flags = -DESP32C3 -DARDUINO_USB_MODE=1 -DARDUINO_USB_CDC_ON_BOOT=1 +extra_scripts = merge_firmware.py [env:esp8266] board = d1_mini From a2934c09e147aa09c6240cdb7eb3bc0a1c9898df Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 3 Feb 2024 00:59:58 +0100 Subject: [PATCH 13/28] bb buffer + esc off + 4way process command --- lib/Espfc/src/Blackbox.h | 64 ++++++++++++++++++++++------- lib/Espfc/src/Msp/MspProcessor.h | 29 +++++++++---- lib/Espfc/src/Output/Mixer.h | 1 + lib/betaflight/src/platform.c | 10 ----- lib/betaflight/src/platform.h | 2 + lib/betaflight/src/platform_cpp.cpp | 22 ++++++++++ 6 files changed, 95 insertions(+), 33 deletions(-) diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index 2d036ef7..cdb57264 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -5,49 +5,83 @@ #include "Hardware.h" #include "EscDriver.h" #include "Math/Utils.h" +#include "Device/SerialDevice.h" extern "C" { #include "blackbox/blackbox.h" #include "blackbox/blackbox_fielddefs.h" } -class BlackboxBuffer +class BlackboxBuffer: public Espfc::Device::SerialDevice { public: - BlackboxBuffer(): _stream(nullptr), _idx(0) {} + BlackboxBuffer(): _dev(nullptr), _idx(0) {} - void begin(Espfc::Device::SerialDevice * s) + virtual void wrap(Espfc::Device::SerialDevice * s) { - _stream = s; + _dev = s; } - void write(uint8_t c) + virtual void begin(const Espfc::SerialDeviceConfig& conf) + { + //_dev->begin(conf); + } + + virtual size_t write(uint8_t c) { _data[_idx++] = c; if(_idx >= SIZE) flush(); + return 1; } - void flush() + virtual void flush() { - if(_stream) _stream->write(_data, _idx); + if(_dev) _dev->write(_data, _idx); _idx = 0; } - size_t availableForWrite() const + virtual int availableForWrite() { - //return _stream->availableForWrite(); + //return _dev->availableForWrite(); return SIZE - _idx; } - size_t isTxFifoEmpty() const + virtual bool isTxFifoEmpty() { - //return _stream->isTxFifoEmpty(); + //return _dev->isTxFifoEmpty(); return _idx == 0; } + virtual int available() { return _dev->available(); } + virtual int read() { return _dev->read(); } + virtual size_t readMany(uint8_t * c, size_t l) { +#ifdef TARGET_RP2040 + size_t count = std::min(l, (size_t)available()); + for(size_t i = 0; i < count; i++) + { + c[i] = read(); + } + return count; +#else + return _dev->readMany(c, l); +#endif + } + virtual int peek() { return _dev->peek(); } + + virtual size_t write(const uint8_t * c, size_t l) + { + for(size_t i = 0; i < l; i++) + { + write(c[i]); + } + return l; + } + virtual bool isSoft() const { return false; }; + virtual operator bool() const { return (bool)(*_dev); } + static const size_t SIZE = SERIAL_TX_FIFO_SIZE;//128; - Espfc::Device::SerialDevice * _stream; + Espfc::Device::SerialDevice * _dev; size_t _idx; uint8_t _data[SIZE]; }; @@ -151,9 +185,9 @@ class Blackbox _serial = _model.getSerialStream(SERIAL_FUNCTION_BLACKBOX); if(!_serial) return 0; - //_buffer.begin(_serial); - //serialBlackboxInit(&_buffer); - serialDeviceInit(_serial, 0); + _buffer.wrap(_serial); + serialDeviceInit(&_buffer, 0); + //serialDeviceInit(_serial, 0); systemConfigMutable()->activeRateProfile = 0; systemConfigMutable()->debug_mode = debugMode = _model.config.debugMode; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 34c3ac2d..fbdc3baa 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -168,7 +168,7 @@ class MspProcessor switch(msg.dir) { case MSP_TYPE_CMD: - processCommand(msg, res); + processCommand(msg, res, s); sendResponse(res, s); msg = MspMessage(); res = MspResponse(); @@ -183,7 +183,7 @@ class MspProcessor return msg.state != MSP_STATE_IDLE; } - void processCommand(MspMessage& m, MspResponse& r) + void processCommand(MspMessage& m, MspResponse& r, Device::SerialDevice& s) { r.cmd = m.cmd; r.version = m.version; @@ -1372,6 +1372,8 @@ class MspProcessor { case MSP_PASSTHROUGH_ESC_4WAY: r.writeU8(esc4wayInit()); + serialDeviceInit(&s, 0); + _postCommand = std::bind(&MspProcessor::processEsc4way, this); break; default: r.writeU8(0); @@ -1398,7 +1400,7 @@ class MspProcessor break; case MSP_REBOOT: - _reboot = true; + _postCommand = std::bind(&MspProcessor::processRestart, this); break; default: @@ -1407,6 +1409,17 @@ class MspProcessor } } + void processEsc4way() + { + esc4wayProcess(getSerialPort()); + processRestart(); + } + + void processRestart() + { + Hardware::restart(_model); + } + void sendResponse(MspResponse& r, Device::SerialDevice& s) { debugResponse(r); @@ -1465,10 +1478,10 @@ class MspProcessor void postCommand() { - if(_reboot) - { - Hardware::restart(_model); - } + if(!_postCommand) return; + std::function cb = _postCommand; + _postCommand = {}; + cb(); } bool debugSkip(uint8_t cmd) @@ -1526,7 +1539,7 @@ class MspProcessor private: Model& _model; MspParser _parser; - bool _reboot; + std::function _postCommand; }; } diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index b4a3b4e0..ebab03bf 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -19,6 +19,7 @@ class Mixer escMotor.begin((EscProtocol)_model.config.output.protocol, _model.config.output.async, _model.config.output.rate, ESC_DRIVER_MOTOR_TIMER); _model.state.escMotor = _motor = &escMotor; _model.logger.info().log(F("MOTOR CONF")).log(_model.config.output.protocol).log(_model.config.output.async).log(_model.config.output.rate).logln(ESC_DRIVER_MOTOR_TIMER); + motorInitEscDevice(_motor); if(_model.config.output.servoRate) { diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 9ffb12b6..6274873a 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -149,13 +149,3 @@ pwmOutputPort_t * pwmGetMotors(void) { return motors; } - -void motorDisable(void) -{ - -} - -void motorEnable(void) -{ - -} \ No newline at end of file diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index 8fbe4a1b..d6df38db 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -330,6 +330,7 @@ void mspSerialReleasePortIfAllocated(serialPort_t *serialPort); serialPort_t *findSharedSerialPort(uint16_t functionMask, serialPortFunction_e sharedWithFunction); serialPortConfig_t *findSerialPortConfig(serialPortFunction_e function); serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFunction_e function, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudrate, portMode_e mode, portOptions_e options); +serialPort_t *getSerialPort(); void closeSerialPort(serialPort_t *serialPort); uint32_t serialRxBytesWaiting(serialPort_t * instance); uint32_t serialTxBytesFree(const serialPort_t *instance); @@ -1146,6 +1147,7 @@ typedef struct { pwmOutputPort_t * pwmGetMotors(void); void motorDisable(void); void motorEnable(void); +void motorInitEscDevice(void * driver); #if defined(UNIT_TEST) || defined(ESP8266) || defined(ARCH_RP2040) void delay(unsigned long ms); diff --git a/lib/betaflight/src/platform_cpp.cpp b/lib/betaflight/src/platform_cpp.cpp index 69591d71..3778bc7b 100644 --- a/lib/betaflight/src/platform_cpp.cpp +++ b/lib/betaflight/src/platform_cpp.cpp @@ -1,6 +1,7 @@ #include #include "platform.h" #include "Device/SerialDevice.h" +#include "EscDriver.h" int IORead(IO_t pin) { @@ -59,6 +60,11 @@ serialPort_t *openSerialPort(serialPortIdentifier_e identifier, serialPortFuncti return &_sp[0]; } +serialPort_t *getSerialPort() +{ + return &_sp[0]; +} + void closeSerialPort(serialPort_t *serialPort) { UNUSED(serialPort); @@ -114,3 +120,19 @@ bool isSerialTransmitBufferEmpty(const serialPort_t * instance) if(!dev) return 0; return dev->isTxFifoEmpty(); } + +static EscDriver * escDriver; + +void motorInitEscDevice(void * driver) +{ + escDriver = (EscDriver *)driver; +} + +void motorDisable(void) +{ + if(escDriver) escDriver->end(); +} + +void motorEnable(void) +{ +} \ No newline at end of file From aa324cddedfe9e0d6ff95151f8de17bee39de7eb Mon Sep 17 00:00:00 2001 From: rtlopez Date: Tue, 6 Feb 2024 00:03:50 +0100 Subject: [PATCH 14/28] fix conflicts + fix tests + 4way motor detection + 4way serial link --- lib/EscDriver/src/EscDriver.h | 1 + lib/EscDriver/src/EscDriverEsp32.h | 6 ++ lib/EscDriver/src/EscDriverEsp32c3.cpp | 6 ++ lib/EscDriver/src/EscDriverEsp32c3.h | 1 + lib/EscDriver/src/EscDriverEsp8266.cpp | 6 ++ lib/EscDriver/src/EscDriverEsp8266.h | 1 + lib/EscDriver/src/EscDriverRP2040.cpp | 6 ++ lib/EscDriver/src/EscDriverRP2040.h | 1 + lib/EspGpio/src/EspGpio.h | 4 ++ lib/Espfc/src/Actuator.h | 6 +- lib/Espfc/src/Blackbox.h | 2 +- lib/Espfc/src/Cli.h | 50 ++++++++++++----- lib/Espfc/src/Controller.h | 4 +- lib/Espfc/src/Input.h | 8 +-- lib/Espfc/src/Model.h | 4 +- lib/Espfc/src/ModelConfig.h | 58 ++++++++++---------- lib/Espfc/src/ModelState.h | 12 ++-- lib/Espfc/src/Msp/MspProcessor.h | 15 ++--- lib/Espfc/src/Output/Mixer.h | 3 +- lib/Espfc/src/Output/Mixers.h | 66 +++++++++++----------- lib/Espfc/src/Stats.h | 4 +- lib/betaflight/src/platform.c | 12 ---- lib/betaflight/src/platform.h | 2 +- lib/betaflight/src/platform_cpp.cpp | 35 ++++++++++-- merge_firmware.py | 18 +++--- test/test_fc/test_fc.cpp | 76 +++++++++++++------------- test/test_msp/test_msp.cpp | 1 + 27 files changed, 239 insertions(+), 169 deletions(-) diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index dfe58a7f..c2a02783 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -29,6 +29,7 @@ class EscDriverBase int attach(size_t channel, int pin, int pulse) { return 1; } int write(size_t channel, int pulse) { return 1; } void apply() {} + int pin(size_t channel) const { return -1; } #endif uint16_t dshotEncode(uint16_t value) diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 0fe1b049..a316a3ce 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -169,6 +169,12 @@ class EscDriverEsp32: public EscDriverBase transmitAll(); } + int pin(size_t channel) const + { + if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; + return _channel[channel].dev.gpio_num; + } + private: void initChannel(int i, gpio_num_t pin, int pulse) { diff --git a/lib/EscDriver/src/EscDriverEsp32c3.cpp b/lib/EscDriver/src/EscDriverEsp32c3.cpp index 5a4710b4..e1e8f832 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.cpp +++ b/lib/EscDriver/src/EscDriverEsp32c3.cpp @@ -82,6 +82,12 @@ int EscDriverEsp32c3::write(size_t channel, int pulse) return 1; } +int EscDriverEsp32c3::pin(size_t channel) const +{ + if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; + return _slots[channel].pin; +} + void EscDriverEsp32c3::apply() { if(_protocol == ESC_PROTOCOL_DISABLED) return; diff --git a/lib/EscDriver/src/EscDriverEsp32c3.h b/lib/EscDriver/src/EscDriverEsp32c3.h index 4571c043..eb07bae9 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.h +++ b/lib/EscDriver/src/EscDriverEsp32c3.h @@ -48,6 +48,7 @@ class EscDriverEsp32c3: public EscDriverBase int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; void apply() IRAM_ATTR; + int pin(size_t channel) const; static bool handle(void * p) IRAM_ATTR; private: diff --git a/lib/EscDriver/src/EscDriverEsp8266.cpp b/lib/EscDriver/src/EscDriverEsp8266.cpp index 687d9c45..adffd920 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.cpp +++ b/lib/EscDriver/src/EscDriverEsp8266.cpp @@ -162,6 +162,12 @@ int EscDriverEsp8266::attach(size_t channel, int pin, int pulse) return 1; } +int EscDriverEsp8266::pin(size_t channel) const +{ + if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; + return _slots[channel].pin; +} + int EscDriverEsp8266::write(size_t channel, int pulse) { if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; diff --git a/lib/EscDriver/src/EscDriverEsp8266.h b/lib/EscDriver/src/EscDriverEsp8266.h index 9b5132f3..4fbbfc6f 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.h +++ b/lib/EscDriver/src/EscDriverEsp8266.h @@ -55,6 +55,7 @@ class EscDriverEsp8266: public EscDriverBase int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; void apply() IRAM_ATTR; + int pin(size_t channel) const; static void handle(void * p, void * x) IRAM_ATTR; private: diff --git a/lib/EscDriver/src/EscDriverRP2040.cpp b/lib/EscDriver/src/EscDriverRP2040.cpp index 3cd93974..3bcf1b82 100644 --- a/lib/EscDriver/src/EscDriverRP2040.cpp +++ b/lib/EscDriver/src/EscDriverRP2040.cpp @@ -77,6 +77,12 @@ int EscDriverRP2040::attach(size_t channel, int pin, int pulse) return 1; } +int EscDriverRP2040::pin(size_t channel) const +{ + if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; + return _slots[channel].pin; +} + bool EscDriverRP2040::isSliceDriven(int slice) { for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i) { diff --git a/lib/EscDriver/src/EscDriverRP2040.h b/lib/EscDriver/src/EscDriverRP2040.h index cb4c3918..26c445aa 100644 --- a/lib/EscDriver/src/EscDriverRP2040.h +++ b/lib/EscDriver/src/EscDriverRP2040.h @@ -42,6 +42,7 @@ class EscDriverRP2040: public EscDriverBase void end(); int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; + int pin(size_t channel) const; void apply() IRAM_ATTR; private: diff --git a/lib/EspGpio/src/EspGpio.h b/lib/EspGpio/src/EspGpio.h index 6048514f..82701b22 100644 --- a/lib/EspGpio/src/EspGpio.h +++ b/lib/EspGpio/src/EspGpio.h @@ -25,6 +25,8 @@ class EspGpio if(val) GP16O |= 1; else GP16O &= ~1; } +#elif defined(ESP32) + ::gpio_set_level((gpio_num_t)pin, val); #elif defined(UNIT_TEST) // do nothing #else @@ -44,6 +46,8 @@ class EspGpio return GP16I & 0x01; } return 0; +#elif defined(ESP32) + return ::gpio_get_level((gpio_num_t)pin); #elif defined(UNIT_TEST) return 0; // do nothing diff --git a/lib/Espfc/src/Actuator.h b/lib/Espfc/src/Actuator.h index 525d02fa..26945cd3 100644 --- a/lib/Espfc/src/Actuator.h +++ b/lib/Espfc/src/Actuator.h @@ -87,7 +87,7 @@ class Actuator _model.state.i2cErrorDelta = 0; _model.setArmingDisabled(ARMING_DISABLED_NO_GYRO, !_model.state.gyroPresent || errors); - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); _model.setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE, _model.state.inputRxLoss || _model.state.inputRxFailSafe); _model.setArmingDisabled(ARMING_DISABLED_THROTTLE, !_model.isThrottleLow()); _model.setArmingDisabled(ARMING_DISABLED_CALIBRATING, _model.calibrationActive()); @@ -116,11 +116,11 @@ class Actuator _model.updateSwitchActive(newMask); - _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FAILSAFE_IDLE); + _model.setArmingDisabled(ARMING_DISABLED_FAILSAFE, _model.state.failsafe.phase != FC_FAILSAFE_IDLE); _model.setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE, _model.isSwitchActive(MODE_FAILSAFE)); _model.setArmingDisabled(ARMING_DISABLED_ARM_SWITCH, _model.armingDisabled() && _model.isSwitchActive(MODE_ARMED)); - if(_model.state.failsafe.phase != FAILSAFE_IDLE) + if(_model.state.failsafe.phase != FC_FAILSAFE_IDLE) { newMask |= (1 << MODE_FAILSAFE); } diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index cdb57264..be91e6bc 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -207,7 +207,7 @@ class Blackbox rp->rates_type = _model.config.input.rateType; pidProfile_s * cp = currentPidProfile = &_pidProfile; - for(size_t i = 0; i < PID_ITEM_COUNT; i++) + for(size_t i = 0; i < FC_PID_ITEM_COUNT; i++) { cp->pid[i].P = _model.config.pid[i].P; cp->pid[i].I = _model.config.pid[i].I; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 6d7492ef..4d49e549 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -534,24 +534,24 @@ class Cli Param(PSTR("pid_sync"), &c.loopSync), - Param(PSTR("pid_roll_p"), &c.pid[PID_ROLL].P), - Param(PSTR("pid_roll_i"), &c.pid[PID_ROLL].I), - Param(PSTR("pid_roll_d"), &c.pid[PID_ROLL].D), - Param(PSTR("pid_roll_f"), &c.pid[PID_ROLL].F), + Param(PSTR("pid_roll_p"), &c.pid[FC_PID_ROLL].P), + Param(PSTR("pid_roll_i"), &c.pid[FC_PID_ROLL].I), + Param(PSTR("pid_roll_d"), &c.pid[FC_PID_ROLL].D), + Param(PSTR("pid_roll_f"), &c.pid[FC_PID_ROLL].F), - Param(PSTR("pid_pitch_p"), &c.pid[PID_PITCH].P), - Param(PSTR("pid_pitch_i"), &c.pid[PID_PITCH].I), - Param(PSTR("pid_pitch_d"), &c.pid[PID_PITCH].D), - Param(PSTR("pid_pitch_f"), &c.pid[PID_PITCH].F), + Param(PSTR("pid_pitch_p"), &c.pid[FC_PID_PITCH].P), + Param(PSTR("pid_pitch_i"), &c.pid[FC_PID_PITCH].I), + Param(PSTR("pid_pitch_d"), &c.pid[FC_PID_PITCH].D), + Param(PSTR("pid_pitch_f"), &c.pid[FC_PID_PITCH].F), - Param(PSTR("pid_yaw_p"), &c.pid[PID_YAW].P), - Param(PSTR("pid_yaw_i"), &c.pid[PID_YAW].I), - Param(PSTR("pid_yaw_d"), &c.pid[PID_YAW].D), - Param(PSTR("pid_yaw_f"), &c.pid[PID_YAW].F), + Param(PSTR("pid_yaw_p"), &c.pid[FC_PID_YAW].P), + Param(PSTR("pid_yaw_i"), &c.pid[FC_PID_YAW].I), + Param(PSTR("pid_yaw_d"), &c.pid[FC_PID_YAW].D), + Param(PSTR("pid_yaw_f"), &c.pid[FC_PID_YAW].F), - Param(PSTR("pid_level_p"), &c.pid[PID_LEVEL].P), - Param(PSTR("pid_level_i"), &c.pid[PID_LEVEL].I), - Param(PSTR("pid_level_d"), &c.pid[PID_LEVEL].D), + Param(PSTR("pid_level_p"), &c.pid[FC_PID_LEVEL].P), + Param(PSTR("pid_level_i"), &c.pid[FC_PID_LEVEL].I), + Param(PSTR("pid_level_d"), &c.pid[FC_PID_LEVEL].D), Param(PSTR("pid_level_angle_limit"), &c.angleLimit), Param(PSTR("pid_level_rate_limit"), &c.angleRateLimit), @@ -1268,6 +1268,26 @@ class Cli { _active = false; } + else if(strcmp_P(cmd.args[0], PSTR("motors")) == 0) + { + s.print(PSTR("count: ")); + s.println(getMotorCount()); + for (size_t i = 0; i < 8; i++) + { + s.print(i); + s.print(PSTR(": ")); + if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) + { + s.print(-1); + s.print(' '); + s.println(0); + } else { + s.print(_model.config.pin[i + PIN_OUTPUT_0]); + s.print(' '); + s.println(_model.state.outputUs[i]); + } + } + } else if(strcmp_P(cmd.args[0], PSTR("logs")) == 0) { s.print(_model.logger.c_str()); diff --git a/lib/Espfc/src/Controller.h b/lib/Espfc/src/Controller.h index ccb3a3d8..9f557989 100644 --- a/lib/Espfc/src/Controller.h +++ b/lib/Espfc/src/Controller.h @@ -31,7 +31,7 @@ class Controller { Stats::Measure(_model.state.stats, COUNTER_OUTER_PID); resetIterm(); - if(_model.config.mixerType == MIXER_GIMBAL) + if(_model.config.mixerType == FC_MIXER_GIMBAL) { outerLoopRobot(); } @@ -43,7 +43,7 @@ class Controller { Stats::Measure(_model.state.stats, COUNTER_INNER_PID); - if(_model.config.mixerType == MIXER_GIMBAL) + if(_model.config.mixerType == FC_MIXER_GIMBAL) { innerLoopRobot(); } diff --git a/lib/Espfc/src/Input.h b/lib/Espfc/src/Input.h index a590e945..82e25082 100644 --- a/lib/Espfc/src/Input.h +++ b/lib/Espfc/src/Input.h @@ -244,12 +244,12 @@ class Input void failsafeIdle() { - _model.state.failsafe.phase = FAILSAFE_IDLE; + _model.state.failsafe.phase = FC_FAILSAFE_IDLE; } void failsafeStage1() { - _model.state.failsafe.phase = FAILSAFE_RX_LOSS_DETECTED; + _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; _model.state.inputRxLoss = true; for(size_t i = 0; i < _model.state.inputChannelCount; i++) { @@ -259,12 +259,12 @@ class Input void failsafeStage2() { - _model.state.failsafe.phase = FAILSAFE_RX_LOSS_DETECTED; + _model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; _model.state.inputRxLoss = true; _model.state.inputRxFailSafe = true; if(_model.isModeActive(MODE_ARMED)) { - _model.state.failsafe.phase = FAILSAFE_LANDED; + _model.state.failsafe.phase = FC_FAILSAFE_LANDED; _model.disarm(DISARM_REASON_FAILSAFE); } } diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 9f6d6f06..1fe2ef78 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -503,7 +503,7 @@ class Model // configure PIDs float pidScale[] = { 1.f, 1.f, 1.f }; - if(config.mixerType == MIXER_GIMBAL) + if(config.mixerType == FC_MIXER_GIMBAL) { pidScale[AXIS_YAW] = 0.2f; // ROBOT pidScale[AXIS_PITCH] = 20.f; // ROBOT @@ -540,7 +540,7 @@ class Model for(size_t i = 0; i < AXIS_YAW; i++) { - PidConfig& pc = config.pid[PID_LEVEL]; + PidConfig& pc = config.pid[FC_PID_LEVEL]; Control::Pid& pid = state.outerPid[i]; pid.Kp = (float)pc.P * LEVEL_PTERM_SCALE; pid.Ki = (float)pc.I * LEVEL_ITERM_SCALE; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index ef1e5968..f8be4ff0 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -349,17 +349,17 @@ class BuzzerConfig }; enum PidIndex { - PID_ROLL, - PID_PITCH, - PID_YAW, - PID_ALT, - PID_POS, - PID_POSR, - PID_NAVR, - PID_LEVEL, - PID_MAG, - PID_VEL, - PID_ITEM_COUNT + FC_PID_ROLL, + FC_PID_PITCH, + FC_PID_YAW, + FC_PID_ALT, + FC_PID_POS, + FC_PID_POSR, + FC_PID_NAVR, + FC_PID_LEVEL, + FC_PID_MAG, + FC_PID_VEL, + FC_PID_ITEM_COUNT }; class PidConfig @@ -568,7 +568,7 @@ class ModelConfig int8_t mixerType; bool yawReverse; - PidConfig pid[PID_ITEM_COUNT]; + PidConfig pid[FC_PID_ITEM_COUNT]; FilterConfig yawFilter; @@ -805,7 +805,7 @@ class ModelConfig output.channel[i].neutral = 1500; } - mixerType = MIXER_QUADX; + mixerType = FC_MIXER_QUADX; yawReverse = 0; output.protocol = ESC_PROTOCOL_DISABLED; @@ -885,23 +885,23 @@ class ModelConfig failsafe.killSwitch = 0; // PID controller config (BF default) - //pid[PID_ROLL] = { .P = 42, .I = 85, .D = 30, .F = 90 }; - //pid[PID_PITCH] = { .P = 46, .I = 90, .D = 32, .F = 95 }; - //pid[PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 90 }; - //pid[PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; + //pid[FC_PID_ROLL] = { .P = 42, .I = 85, .D = 30, .F = 90 }; + //pid[FC_PID_PITCH] = { .P = 46, .I = 90, .D = 32, .F = 95 }; + //pid[FC_PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 90 }; + //pid[FC_PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; // PID controller config (ESPFC default) - pid[PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }; - pid[PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }; - pid[PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 72 }; - pid[PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; - - pid[PID_ALT] = { .P = 0, .I = 0, .D = 0, .F = 0 }; - pid[PID_POS] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // POSHOLD_P * 100, POSHOLD_I * 100, - pid[PID_POSR] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, - pid[PID_NAVR] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // NAV_P * 10, NAV_I * 100, NAV_D * 1000 - pid[PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }; - pid[PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }; + pid[FC_PID_ROLL] = { .P = 42, .I = 85, .D = 24, .F = 72 }; + pid[FC_PID_PITCH] = { .P = 46, .I = 90, .D = 26, .F = 76 }; + pid[FC_PID_YAW] = { .P = 45, .I = 90, .D = 0, .F = 72 }; + pid[FC_PID_LEVEL] = { .P = 55, .I = 0, .D = 0, .F = 0 }; + + pid[FC_PID_ALT] = { .P = 0, .I = 0, .D = 0, .F = 0 }; + pid[FC_PID_POS] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // POSHOLD_P * 100, POSHOLD_I * 100, + pid[FC_PID_POSR] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000, + pid[FC_PID_NAVR] = { .P = 0, .I = 0, .D = 0, .F = 0 }; // NAV_P * 10, NAV_I * 100, NAV_D * 1000 + pid[FC_PID_MAG] = { .P = 0, .I = 0, .D = 0, .F = 0 }; + pid[FC_PID_VEL] = { .P = 0, .I = 0, .D = 0, .F = 0 }; itermWindupPointPercent = 30; itermRelax = ITERM_RELAX_RP; @@ -1041,7 +1041,7 @@ class ModelConfig void brobot() { - mixerType = MIXER_GIMBAL; + mixerType = FC_MIXER_GIMBAL; pin[PIN_OUTPUT_0] = 14; // D5 // ROBOT pin[PIN_OUTPUT_1] = 12; // D6 // ROBOT diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 6abff5c4..b486275e 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -114,12 +114,12 @@ enum CalibrationState { }; enum FailsafePhase { - FAILSAFE_IDLE = 0, - FAILSAFE_RX_LOSS_DETECTED, - FAILSAFE_LANDING, - FAILSAFE_LANDED, - FAILSAFE_RX_LOSS_MONITORING, - FAILSAFE_RX_LOSS_RECOVERED + FC_FAILSAFE_IDLE = 0, + FC_FAILSAFE_RX_LOSS_DETECTED, + FC_FAILSAFE_LANDING, + FC_FAILSAFE_LANDED, + FC_FAILSAFE_RX_LOSS_MONITORING, + FC_FAILSAFE_RX_LOSS_RECOVERED }; class FailsafeState diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index fbdc3baa..e6c9ca8b 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1155,9 +1155,9 @@ class MspProcessor r.writeU8(0); // abs control gain r.writeU8(0); // throttle boost r.writeU8(0); // acro trainer max angle - r.writeU16(_model.config.pid[PID_ROLL].F); //pid roll f - r.writeU16(_model.config.pid[PID_PITCH].F); //pid pitch f - r.writeU16(_model.config.pid[PID_YAW].F); //pid yaw f + r.writeU16(_model.config.pid[FC_PID_ROLL].F); //pid roll f + r.writeU16(_model.config.pid[FC_PID_PITCH].F); //pid pitch f + r.writeU16(_model.config.pid[FC_PID_YAW].F); //pid yaw f r.writeU8(0); // antigravity mode // 1.41+ r.writeU8(0); // d min roll @@ -1208,9 +1208,9 @@ class MspProcessor m.readU8(); //abs control gain m.readU8(); //throttle boost m.readU8(); //acro trainer max angle - _model.config.pid[PID_ROLL].F = m.readU16(); // pid roll f - _model.config.pid[PID_PITCH].F = m.readU16(); // pid pitch f - _model.config.pid[PID_YAW].F = m.readU16(); // pid yaw f + _model.config.pid[FC_PID_ROLL].F = m.readU16(); // pid roll f + _model.config.pid[FC_PID_PITCH].F = m.readU16(); // pid pitch f + _model.config.pid[FC_PID_YAW].F = m.readU16(); // pid yaw f m.readU8(); //antigravity mode } // 1.41+ @@ -1252,7 +1252,7 @@ class MspProcessor break; case MSP_MOTOR: - for (size_t i = 0; i < OUTPUT_CHANNELS; i++) + for (size_t i = 0; i < 8; i++) { if (i >= OUTPUT_CHANNELS || _model.config.pin[i + PIN_OUTPUT_0] == -1) { @@ -1381,6 +1381,7 @@ class MspProcessor } (void)ptArg; } + break; case MSP_DEBUG: for (int i = 0; i < 4; i++) { diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index ebab03bf..05e5339a 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -4,6 +4,7 @@ #include "Model.h" #include "Output/Mixers.h" #include "EscDriver.h" +#include "platform.h" namespace Espfc { @@ -19,7 +20,6 @@ class Mixer escMotor.begin((EscProtocol)_model.config.output.protocol, _model.config.output.async, _model.config.output.rate, ESC_DRIVER_MOTOR_TIMER); _model.state.escMotor = _motor = &escMotor; _model.logger.info().log(F("MOTOR CONF")).log(_model.config.output.protocol).log(_model.config.output.async).log(_model.config.output.rate).logln(ESC_DRIVER_MOTOR_TIMER); - motorInitEscDevice(_motor); if(_model.config.output.servoRate) { @@ -45,6 +45,7 @@ class Mixer _model.logger.info().log(F("MOTOR PIN")).log(i).logln(_model.config.pin[PIN_OUTPUT_0 + i]); } } + motorInitEscDevice(_motor); _model.state.minThrottle = _model.config.output.minThrottle; _model.state.maxThrottle = _model.config.output.maxThrottle; diff --git a/lib/Espfc/src/Output/Mixers.h b/lib/Espfc/src/Output/Mixers.h index 409f2219..17c0fd0b 100644 --- a/lib/Espfc/src/Output/Mixers.h +++ b/lib/Espfc/src/Output/Mixers.h @@ -4,32 +4,32 @@ namespace Espfc { enum MixerType { - MIXER_TRI = 1, - MIXER_QUADP = 2, - MIXER_QUADX = 3, - MIXER_BICOPTER = 4, - MIXER_GIMBAL = 5, - MIXER_Y6 = 6, - MIXER_HEX6 = 7, - MIXER_FLYING_WING = 8, - MIXER_Y4 = 9, - MIXER_HEX6X = 10, - MIXER_OCTOX8 = 11, - MIXER_OCTOFLATP = 12, - MIXER_OCTOFLATX = 13, - MIXER_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported) - MIXER_HELI_120_CCPM = 15, - MIXER_HELI_90_DEG = 16, - MIXER_VTAIL4 = 17, - MIXER_HEX6H = 18, - MIXER_PPM_TO_SERVO = 19, // PPM -> servo relay - MIXER_DUALCOPTER = 20, - MIXER_SINGLECOPTER = 21, - MIXER_ATAIL4 = 22, - MIXER_CUSTOM = 23, - MIXER_CUSTOM_AIRPLANE = 24, - MIXER_CUSTOM_TRI = 25, - MIXER_QUADX_1234 = 26, + FC_MIXER_TRI = 1, + FC_MIXER_QUADP = 2, + FC_MIXER_QUADX = 3, + FC_MIXER_BICOPTER = 4, + FC_MIXER_GIMBAL = 5, + FC_MIXER_Y6 = 6, + FC_MIXER_HEX6 = 7, + FC_MIXER_FLYING_WING = 8, + FC_MIXER_Y4 = 9, + FC_MIXER_HEX6X = 10, + FC_MIXER_OCTOX8 = 11, + FC_MIXER_OCTOFLATP = 12, + FC_MIXER_OCTOFLATX = 13, + FC_MIXER_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported) + FC_MIXER_HELI_120_CCPM = 15, + FC_MIXER_HELI_90_DEG = 16, + FC_MIXER_VTAIL4 = 17, + FC_MIXER_HEX6H = 18, + FC_MIXER_PPM_TO_SERVO = 19, // PPM -> servo relay + FC_MIXER_DUALCOPTER = 20, + FC_MIXER_SINGLECOPTER = 21, + FC_MIXER_ATAIL4 = 22, + FC_MIXER_CUSTOM = 23, + FC_MIXER_CUSTOM_AIRPLANE = 24, + FC_MIXER_CUSTOM_TRI = 25, + FC_MIXER_QUADX_1234 = 26, }; enum MixerSource { @@ -128,21 +128,21 @@ class Mixers switch(mixer) { - case MIXER_QUADX: + case FC_MIXER_QUADX: return MixerConfig(4, mixesQuadX); - case MIXER_QUADX_1234: + case FC_MIXER_QUADX_1234: return MixerConfig(4, mixesQuadX1234); - case MIXER_TRI: + case FC_MIXER_TRI: return MixerConfig(4, mixesTricopter); - case MIXER_GIMBAL: + case FC_MIXER_GIMBAL: return MixerConfig(2, mixesGimbal); - case MIXER_CUSTOM: - case MIXER_CUSTOM_TRI: - case MIXER_CUSTOM_AIRPLANE: + case FC_MIXER_CUSTOM: + case FC_MIXER_CUSTOM_TRI: + case FC_MIXER_CUSTOM_AIRPLANE: return custom; default: diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index e6bb4408..21a5bc97 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -82,8 +82,8 @@ class Stats { uint32_t now = micros(); uint32_t diff = now - _loop_last; - _loop_time = diff; - //_loop_time += (((int32_t)diff - _loop_time + 8) >> 4); + //_loop_time = diff; + _loop_time += (((int32_t)diff - _loop_time + 8) >> 4); _loop_last = now; } diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 6274873a..6042a88d 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -99,11 +99,6 @@ bool rxAreFlightChannelsValid(void) return true; } -uint8_t getMotorCount() -{ - return MAX_SUPPORTED_MOTORS; -} - bool isModeActivationConditionPresent(boxId_e modeId) { return false; @@ -142,10 +137,3 @@ uint32_t zigzagEncode(int32_t value) { return (uint32_t)((value << 1) ^ (value >> 31)); } - -pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; - -pwmOutputPort_t * pwmGetMotors(void) -{ - return motors; -} diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index d6df38db..7bb8ed53 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -32,7 +32,7 @@ #error "Unsupported platform" #endif -#define MAX_SUPPORTED_MOTORS 4 +#define MAX_SUPPORTED_MOTORS 8 #define MAX_SUPPORTED_SERVOS 8 #define PID_PROCESS_DENOM_DEFAULT 1 diff --git a/lib/betaflight/src/platform_cpp.cpp b/lib/betaflight/src/platform_cpp.cpp index 3778bc7b..e9754934 100644 --- a/lib/betaflight/src/platform_cpp.cpp +++ b/lib/betaflight/src/platform_cpp.cpp @@ -2,10 +2,11 @@ #include "platform.h" #include "Device/SerialDevice.h" #include "EscDriver.h" +#include "EspGpio.h" int IORead(IO_t pin) { - return digitalRead(pin); + return EspGpio::digitalRead(pin); } void IOConfigGPIO(IO_t pin, uint8_t mode) @@ -15,12 +16,12 @@ void IOConfigGPIO(IO_t pin, uint8_t mode) void IOHi(IO_t pin) { - digitalWrite(pin, HIGH); + EspGpio::digitalWrite(pin, HIGH); } void IOLo(IO_t pin) { - digitalWrite(pin, LOW); + EspGpio::digitalWrite(pin, LOW); } static serialPort_t _sp[2] = {{ @@ -121,11 +122,30 @@ bool isSerialTransmitBufferEmpty(const serialPort_t * instance) return dev->isTxFifoEmpty(); } +static size_t _motorCount = 0; +pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; + +pwmOutputPort_t * pwmGetMotors(void) +{ + return motors; +} + static EscDriver * escDriver; void motorInitEscDevice(void * driver) { - escDriver = (EscDriver *)driver; + escDriver = static_cast(driver); + memset(&motors, 0, sizeof(pwmOutputPort_t)); + if(!driver) return; + + for(size_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) + { + int pin = escDriver->pin(i); + if(pin == -1) continue; + motors[i].enabled = true; + motors[i].io = pin; + _motorCount++; + } } void motorDisable(void) @@ -135,4 +155,9 @@ void motorDisable(void) void motorEnable(void) { -} \ No newline at end of file +} + +uint8_t getMotorCount() +{ + return _motorCount; +} diff --git a/merge_firmware.py b/merge_firmware.py index acc3893b..8148757e 100644 --- a/merge_firmware.py +++ b/merge_firmware.py @@ -32,12 +32,12 @@ def merge_bin(source, target, env): env.AddPostAction(APP_BIN , merge_bin) # Patch the upload command to flash the merged binary at address 0x0 -env.Replace( - UPLOADERFLAGS=[ - f - for f in env.get("UPLOADERFLAGS") - if f not in env.Flatten(env.get("FLASH_EXTRA_IMAGES")) - ] - + ["0x0", MERGED_BIN], - UPLOADCMD='"$PYTHONEXE" "$UPLOADER" $UPLOADERFLAGS', -) \ No newline at end of file +#env.Replace( +# UPLOADERFLAGS=[ +# f +# for f in env.get("UPLOADERFLAGS") +# if f not in env.Flatten(env.get("FLASH_EXTRA_IMAGES")) +# ] +# + ["0x0", MERGED_BIN], +# UPLOADCMD='"$PYTHONEXE" "$UPLOADER" $UPLOADERFLAGS', +#) \ No newline at end of file diff --git a/test/test_fc/test_fc.cpp b/test/test_fc/test_fc.cpp index 021e6ea9..ccde43c3 100644 --- a/test/test_fc/test_fc.cpp +++ b/test/test_fc/test_fc.cpp @@ -1,10 +1,12 @@ #include #include +#include #include "Timer.h" #include "Model.h" #include "Controller.h" #include "Actuator.h" #include "Output/Mixer.h" + using namespace fakeit; using namespace Espfc; @@ -141,29 +143,29 @@ void test_model_inner_pid_init() model.config.gyroDlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; - model.config.mixerType = MIXER_QUADX; - model.config.pid[PID_ROLL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; - model.config.pid[PID_PITCH] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; - model.config.pid[PID_YAW] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; + model.config.mixerType = FC_MIXER_QUADX; + model.config.pid[FC_PID_ROLL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; + model.config.pid[FC_PID_PITCH] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; + model.config.pid[FC_PID_YAW] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.begin(); - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[PID_ROLL].rate); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[PID_ROLL].Kp); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[PID_ROLL].Ki); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[PID_ROLL].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[PID_ROLL].Kf); - - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[PID_PITCH].rate); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[PID_PITCH].Kp); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[PID_PITCH].Ki); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[PID_PITCH].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[PID_PITCH].Kf); - - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[PID_YAW].rate); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[PID_YAW].Kp); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[PID_YAW].Ki); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[PID_YAW].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[PID_YAW].Kf); + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[FC_PID_ROLL].rate); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[FC_PID_ROLL].Kp); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[FC_PID_ROLL].Ki); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[FC_PID_ROLL].Kd); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[FC_PID_ROLL].Kf); + + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[FC_PID_PITCH].rate); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[FC_PID_PITCH].Kp); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[FC_PID_PITCH].Ki); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[FC_PID_PITCH].Kd); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[FC_PID_PITCH].Kf); + + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 1000.0f, model.state.innerPid[FC_PID_YAW].rate); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1835f, model.state.innerPid[FC_PID_YAW].Kp); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 1.4002f, model.state.innerPid[FC_PID_YAW].Ki); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.0030f, model.state.innerPid[FC_PID_YAW].Kd); + TEST_ASSERT_FLOAT_WITHIN(0.000001f, 0.000788f, model.state.innerPid[FC_PID_YAW].Kf); } void test_model_outer_pid_init() @@ -173,21 +175,21 @@ void test_model_outer_pid_init() model.config.gyroDlpf = GYRO_DLPF_256; model.config.loopSync = 1; model.config.mixerSync = 1; - model.config.mixerType = MIXER_QUADX; - model.config.pid[PID_LEVEL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; + model.config.mixerType = FC_MIXER_QUADX; + model.config.pid[FC_PID_LEVEL] = { .P = 100u, .I = 100u, .D = 100u, .F = 100 }; model.begin(); - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 2000.0f, model.state.outerPid[PID_ROLL].rate); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_ROLL].Kp); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_ROLL].Ki); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_ROLL].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_ROLL].Kf); - - TEST_ASSERT_FLOAT_WITHIN( 0.1f, 2000.0f, model.state.outerPid[PID_PITCH].rate); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_PITCH].Kp); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[PID_PITCH].Ki); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_PITCH].Kd); - TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[PID_PITCH].Kf); + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 2000.0f, model.state.outerPid[FC_PID_ROLL].rate); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[FC_PID_ROLL].Kp); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[FC_PID_ROLL].Ki); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[FC_PID_ROLL].Kd); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[FC_PID_ROLL].Kf); + + TEST_ASSERT_FLOAT_WITHIN( 0.1f, 2000.0f, model.state.outerPid[FC_PID_PITCH].rate); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[FC_PID_PITCH].Kp); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 10.0f, model.state.outerPid[FC_PID_PITCH].Ki); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[FC_PID_PITCH].Kd); + TEST_ASSERT_FLOAT_WITHIN(0.0001f, 0.1f, model.state.outerPid[FC_PID_PITCH].Kf); } void test_controller_rates() @@ -197,7 +199,7 @@ void test_controller_rates() model.config.gyroDlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; - model.config.mixerType = MIXER_QUADX; + model.config.mixerType = FC_MIXER_QUADX; model.config.input.rateType = RATES_TYPE_BETAFLIGHT; model.config.input.rate[AXIS_ROLL] = 70; @@ -247,7 +249,7 @@ void test_controller_rates_limit() model.config.gyroDlpf = GYRO_DLPF_256; model.config.loopSync = 8; model.config.mixerSync = 1; - model.config.mixerType = MIXER_QUADX; + model.config.mixerType = FC_MIXER_QUADX; model.config.input.rateType = RATES_TYPE_BETAFLIGHT; model.config.input.rate[AXIS_ROLL] = 70; @@ -464,7 +466,7 @@ void test_actuator_arming_failsafe() Model model; model.state.gyroPresent = true; model.config.output.protocol = ESC_PROTOCOL_DSHOT150; - model.state.failsafe.phase = FAILSAFE_RX_LOSS_DETECTED; + model.state.failsafe.phase = FC_FAILSAFE_RX_LOSS_DETECTED; model.state.gyroCalibrationState = CALIBRATION_UPDATE; model.state.inputRxFailSafe = true; model.state.inputRxLoss = true; diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index ea513cc3..3cfb8d44 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include "Msp/Msp.h" #include "Msp/MspParser.h" From 732c0b3c0624accf35ad115215ca3ddbec48c7cc Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 11 Feb 2024 20:29:29 +0100 Subject: [PATCH 15/28] wornikng esc-4way --- docs/connections.md | 6 +++--- lib/EscDriver/src/EscDriverEsp32.h | 1 + lib/EscDriver/src/EscDriverEsp32c3.cpp | 6 +++++- lib/EscDriver/src/EscDriverEsp8266.cpp | 1 + lib/betaflight/src/io/serial_4way.c | 2 +- lib/betaflight/src/platform.h | 6 +++--- lib/betaflight/src/platform_cpp.cpp | 10 +++++++++- 7 files changed, 23 insertions(+), 9 deletions(-) diff --git a/docs/connections.md b/docs/connections.md index 57fec585..23dd29cc 100644 --- a/docs/connections.md +++ b/docs/connections.md @@ -2,10 +2,10 @@ ``` Pin | Function | ESP-8266 Pin | ESPFC external device ----+------------------+--------------+---------------------------------------------- -TX | TXD0 | TXD, GPIO1 | (UART0 TX) | CLI | MSP | OSD | GPS -RX | RXD0 | RXD, GPIO3 | (UART0 RX) | CLI | MSP | OSD | GPS | SBUS +TX | TXD0 | TXD, GPIO1 | (UART0 TX) | CLI | MSP | OSD +RX | RXD0 | RXD, GPIO3 | (UART0 RX) | CLI | MSP | OSD A0 | Analog In, 3.3V | A0 | Batery voltage or current sensor, analog only -D0 | IO | GPIO16 | LED | BUZZER | PWM (output recommended, no interrupts) +D0 | IO | GPIO16 | LED | BUZZER (output recommended, no interrupts) D1 | IO, SCL | GPIO5 | I2C Gyro, Compas, Pressure and other sensors D2 | IO, SDA | GPIO4 | I2C Gyro, Compas, Pressure and other sensors D3 | IO, PU | GPIO0 | PWM 1 (servo/motor) diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index a316a3ce..a68de8bf 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -133,6 +133,7 @@ class EscDriverEsp32: public EscDriverBase if(!_channel[i].attached()) continue; rmt_driver_uninstall(_channel[i].dev.channel); } + _protocol = ESC_PROTOCOL_DISABLED; } int begin(EscProtocol protocol, bool async, int32_t rate, int timer = 0) diff --git a/lib/EscDriver/src/EscDriverEsp32c3.cpp b/lib/EscDriver/src/EscDriverEsp32c3.cpp index e1e8f832..f2964ea2 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.cpp +++ b/lib/EscDriver/src/EscDriverEsp32c3.cpp @@ -290,12 +290,16 @@ int EscDriverEsp32c3::begin(EscProtocol protocol, bool async, int16_t rate, EscD void EscDriverEsp32c3::end() { - //_isr_end(_timer, this); + if(_protocol < ESC_PROTOCOL_DSHOT150) // analog + { + _isr_end(_timer, this); + } for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i) { if(_slots[i].pin == -1) continue; digitalWrite(_slots[i].pin, LOW); } + _protocol = ESC_PROTOCOL_DISABLED; } static inline void dshotDelay(int delay) diff --git a/lib/EscDriver/src/EscDriverEsp8266.cpp b/lib/EscDriver/src/EscDriverEsp8266.cpp index adffd920..71a1162c 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.cpp +++ b/lib/EscDriver/src/EscDriverEsp8266.cpp @@ -401,6 +401,7 @@ void EscDriverEsp8266::end() { _isr_end(_timer, this); } + _protocol = ESC_PROTOCOL_DISABLED; for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i) { if(_slots[i].pin == -1) continue; diff --git a/lib/betaflight/src/io/serial_4way.c b/lib/betaflight/src/io/serial_4way.c index 091c7e0f..225530b4 100644 --- a/lib/betaflight/src/io/serial_4way.c +++ b/lib/betaflight/src/io/serial_4way.c @@ -141,6 +141,7 @@ uint8_t esc4wayInit(void) // StopPwmAllMotors(); // XXX Review effect of motor refactor //pwmDisableMotors(); + motorDisable(); escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); pwmOutputPort_t *pwmMotors = pwmGetMotors(); @@ -154,7 +155,6 @@ uint8_t esc4wayInit(void) } } } - motorDisable(); return escCount; } diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index 7bb8ed53..b48c04ce 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -1157,9 +1157,9 @@ void delay(uint32_t ms); void delayMicroseconds(uint32_t us); #endif -#define IOCFG_IPU 0x05 // INPUT_PULLUP -#define IOCFG_OUT_PP 0x03 // OUTPUT -#define IOCFG_AF_PP 0x03 // OUTPUT +#define IOCFG_IPU 0x00 // INPUT_PULLUP +#define IOCFG_OUT_PP 0x01 // OUTPUT +#define IOCFG_AF_PP 0x02 // OUTPUT #define Bit_RESET 0x00 // LOW #define IO_NONE -1 diff --git a/lib/betaflight/src/platform_cpp.cpp b/lib/betaflight/src/platform_cpp.cpp index e9754934..fd5ed641 100644 --- a/lib/betaflight/src/platform_cpp.cpp +++ b/lib/betaflight/src/platform_cpp.cpp @@ -11,7 +11,15 @@ int IORead(IO_t pin) void IOConfigGPIO(IO_t pin, uint8_t mode) { - pinMode(pin, mode); + switch(mode) { + case IOCFG_IPU: + ::pinMode(pin, INPUT_PULLUP); + break; + case IOCFG_OUT_PP: + case IOCFG_AF_PP: + ::pinMode(pin, OUTPUT); + break; + } } void IOHi(IO_t pin) From 03aabc1342f7aa3bce69a897c839c255b82953e9 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 12 Feb 2024 01:17:53 +0100 Subject: [PATCH 16/28] split esp32 esc driver --- lib/EscDriver/src/EscDriverEsp32.cpp | 361 +++++++++++++++++++++++- lib/EscDriver/src/EscDriverEsp32.h | 401 +++------------------------ 2 files changed, 398 insertions(+), 364 deletions(-) diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index ab380e5c..40f2bc6f 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -1,9 +1,368 @@ #if defined(ESP32) && !defined(ESP32C3) +#include #include "EscDriverEsp32.h" +#include "Debug_Espfc.h" + +static const size_t DURATION_CLOCK = 25; // [ns] doubled value to increase precision +#define TO_INTERVAL_US(v) (1 * 1000 * 1000 / (v)) // [us] + +// faster esc response, but unsafe (no task synchronisation) +// set to 0 in case of issues +#if defined(ESP32S3) || defined(ESP32S2) +#define ESPFC_RMT_BYPASS_WRITE_SYNC 0 +#else +#define ESPFC_RMT_BYPASS_WRITE_SYNC 1 +#endif + +#if ESPFC_RMT_BYPASS_WRITE_SYNC +IRAM_ATTR static esp_err_t _rmt_fill_tx_items(rmt_channel_t channel, const rmt_item32_t* item, uint16_t item_num, uint16_t mem_offset) +{ + //RMT_CHECK(channel < RMT_CHANNEL_MAX, RMT_CHANNEL_ERROR_STR, (0)); + //RMT_CHECK((item != NULL), RMT_ADDR_ERROR_STR, ESP_ERR_INVALID_ARG); + //RMT_CHECK((item_num > 0), RMT_DRIVER_LENGTH_ERROR_STR, ESP_ERR_INVALID_ARG); + + /*Each block has 64 x 32 bits of data*/ + //uint8_t mem_cnt = RMT.conf_ch[channel].conf0.mem_size; + //RMT_CHECK((mem_cnt * RMT_MEM_ITEM_NUM >= item_num), RMT_WR_MEM_OVF_ERROR_STR, ESP_ERR_INVALID_ARG); + + //rmt_fill_memory(channel, item, item_num, mem_offset); + //portENTER_CRITICAL(&rmt_spinlock); + RMT.apb_conf.fifo_mask = RMT_DATA_MODE_MEM; + //portEXIT_CRITICAL(&rmt_spinlock); + //for(size_t i = 0; i < item_num; i++) { + // RMTMEM.chan[channel].data32[i + mem_offset].val = item[i].val; + //} + uint32_t *from = (uint32_t *)item; + volatile uint32_t *to = (volatile uint32_t *)&RMTMEM.chan[channel].data32[0].val; + to += mem_offset; + while (item_num--) { + *to++ = *from++; + } + return ESP_OK; +} +IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) +{ + //RMT_CHECK(channel < RMT_CHANNEL_MAX, RMT_CHANNEL_ERROR_STR, ESP_ERR_INVALID_ARG); + //portENTER_CRITICAL(&rmt_spinlock); + if(tx_idx_rst) { + RMT.conf_ch[channel].conf1.mem_rd_rst = 1; + } + RMT.conf_ch[channel].conf1.mem_owner = RMT_MEM_OWNER_TX; + RMT.conf_ch[channel].conf1.tx_start = 1; + //portEXIT_CRITICAL(&rmt_spinlock); + return ESP_OK; +} +#else +#define _rmt_tx_start rmt_tx_start +#define _rmt_fill_tx_items rmt_fill_tx_items +#endif bool EscDriverEsp32::_tx_end_installed = false; -EscDriverEsp32* EscDriverEsp32::instances[] = { NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL }; +EscDriverEsp32 *EscDriverEsp32::instances[] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; + +EscDriverEsp32::EscDriverEsp32() : _protocol(ESC_PROTOCOL_PWM), _async(true), _rate(50), _digital(false) +{ + for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) + { + _channel[i].dev.gpio_num = gpio_num_t(-1); + } +} + +void EscDriverEsp32::end() +{ + for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) + { + if (!_channel[i].attached()) continue; + rmt_driver_uninstall(_channel[i].dev.channel); + } + _protocol = ESC_PROTOCOL_DISABLED; +} + +int EscDriverEsp32::begin(EscProtocol protocol, bool async, int32_t rate, int timer) +{ + (void)timer; // unused + + _protocol = protocol; + _digital = isDigital(protocol); + _async = async; + _rate = rate; + _interval = TO_INTERVAL_US(_rate); + + return 1; +} + +int EscDriverEsp32::attach(size_t channel, int pin, int pulse) +{ + if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; + initChannel(channel, (gpio_num_t)pin, pulse); + return 1; +} + +int EscDriverEsp32::write(size_t channel, int pulse) +{ + if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; + _channel[channel].pulse = pulse; + return 1; +} + +void EscDriverEsp32::apply() +{ + if (_protocol == ESC_PROTOCOL_DISABLED) return; + if (_async) return; + transmitAll(); +} + +int EscDriverEsp32::pin(size_t channel) const +{ + if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; + return _channel[channel].dev.gpio_num; +} + +void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) +{ + if (pin == -1) return; + + pinMode(pin, OUTPUT); + digitalWrite(pin, LOW); + + _channel[i].protocol = _protocol; + _channel[i].pulse = pulse; + _channel[i].divider = getClockDivider(); + _channel[i].pulse_min = getPulseMin(); + _channel[i].pulse_max = getPulseMax(); + _channel[i].pulse_space = getPulseInterval(); + + // specification 0:37%, 1:75% of 1670ns for dshot600 + //_channel[i].dshot_t0h = getDshotPulse(625); + //_channel[i].dshot_t0l = getDshotPulse(1045); + //_channel[i].dshot_t1h = getDshotPulse(1250); + //_channel[i].dshot_t1l = getDshotPulse(420); + + // betaflight 0:35%, 1:70% of 1670ns for dshot600 + _channel[i].dshot_t0h = getDshotPulse(584 - 16); + _channel[i].dshot_t0l = getDshotPulse(1086 - 32); + _channel[i].dshot_t1h = getDshotPulse(1170 - 32); + _channel[i].dshot_t1l = getDshotPulse(500 - 16); + + _channel[i].dev.gpio_num = pin; + _channel[i].dev.rmt_mode = RMT_MODE_TX; + _channel[i].dev.channel = (rmt_channel_t)i; + _channel[i].dev.clk_div = _channel[i].divider; + _channel[i].dev.mem_block_num = 1; + _channel[i].dev.tx_config.loop_en = 0; + _channel[i].dev.tx_config.idle_output_en = 1; + _channel[i].dev.tx_config.idle_level = RMT_IDLE_LEVEL_LOW; + + // unused + _channel[i].dev.tx_config.carrier_duty_percent = 50; + _channel[i].dev.tx_config.carrier_freq_hz = 1000; + _channel[i].dev.tx_config.carrier_level = RMT_CARRIER_LEVEL_HIGH; + _channel[i].dev.tx_config.carrier_en = 0; + + instances[i] = this; + + rmt_config(&_channel[i].dev); + rmt_driver_install(_channel[i].dev.channel, 0, 0); + if (/*_async && */!_tx_end_installed) + { + _tx_end_installed = true; + rmt_register_tx_end_callback(txDoneCallback, NULL); + } + if (_async) + { + rmt_set_tx_intr_en(_channel[i].dev.channel, true); + txDoneCallback((rmt_channel_t)i, NULL); // start generating pulses + } +} + +void EscDriverEsp32::txDoneCallback(rmt_channel_t channel, void *arg) +{ + PIN_DEBUG(HIGH); + if (instances[channel] && instances[channel]->_async) + { + instances[channel]->transmitOne(channel); + } + PIN_DEBUG(LOW); +} + +void EscDriverEsp32::transmitOne(uint8_t i) +{ + if (!_channel[i].attached()) return; + if (_digital) + { + writeDshotCommand(i, _channel[i].pulse); + } + else + { + writeAnalogCommand(i, _channel[i].pulse); + } + transmitCommand(i); +} + +void EscDriverEsp32::transmitAll() +{ + for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) + { + if (!_channel[i].attached()) continue; + if (_digital) + { + writeDshotCommand(i, _channel[i].pulse); + } + else + { + writeAnalogCommand(i, _channel[i].pulse); + } + } + for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) + { + if (!_channel[i].attached()) continue; + transmitCommand(i); + } +} + +void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) +{ + Slot &slot = _channel[channel]; + int minPulse = 800; + int maxPulse = 2200; + if (_protocol == ESC_PROTOCOL_BRUSHED) + { + minPulse = 1000; + maxPulse = 2000; + } + pulse = constrain(pulse, minPulse, maxPulse); + int duration = map(pulse, 1000, 2000, slot.pulse_min, slot.pulse_max); + + int count = 2; + if (_async) + { + int space = slot.pulse_space - duration; + slot.setDuration(0, duration, 1); + slot.setDuration(1, space, 0); + slot.setTerminate(2, 0); + count = 3; + } + else + { + slot.setDuration(0, duration, 1); + slot.setTerminate(1, 0); + } + + _rmt_fill_tx_items(_channel[channel].dev.channel, _channel[channel].items, count, 0); +} + +void EscDriverEsp32::writeDshotCommand(uint8_t channel, int32_t pulse) +{ + pulse = constrain(pulse, 0, 2000); + // scale to dshot commands (0 or 48-2047) + int value = pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; + uint16_t frame = dshotEncode(value); + + Slot &slot = _channel[channel]; + for (size_t i = 0; i < DSHOT_BIT_COUNT; i++) + { + int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01; + slot.setDshotBit(i, val); + } + slot.setTerminate(DSHOT_BIT_COUNT, 0); + + _rmt_fill_tx_items(slot.dev.channel, slot.items, Slot::ITEM_COUNT, 0); +} + +void EscDriverEsp32::transmitCommand(uint8_t channel) +{ + _rmt_tx_start(_channel[channel].dev.channel, true); +} + +uint32_t EscDriverEsp32::getClockDivider() const +{ + switch (_protocol) + { + case ESC_PROTOCOL_PWM: return 24; + case ESC_PROTOCOL_ONESHOT125: return 4; + case ESC_PROTOCOL_ONESHOT42: return 2; + case ESC_PROTOCOL_MULTISHOT: return 2; + case ESC_PROTOCOL_BRUSHED: return 8; + default: return 1; + } +} + +uint32_t EscDriverEsp32::getPulseMin() const +{ + switch (_protocol) + { + case ESC_PROTOCOL_PWM: return getAnalogPulse(1000 * 1000); + case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse(125 * 1000); + case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse(42 * 1000); + case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse(5 * 1000); + case ESC_PROTOCOL_BRUSHED: return 0; + case ESC_PROTOCOL_DSHOT150: + case ESC_PROTOCOL_DSHOT300: + case ESC_PROTOCOL_DSHOT600: + default: + return 0; + } +} + +uint32_t EscDriverEsp32::getPulseMax() const +{ + switch (_protocol) + { + case ESC_PROTOCOL_PWM: return getAnalogPulse(2000 * 1000); + case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse(250 * 1000); + case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse(84 * 1000); + case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse(25 * 1000); + case ESC_PROTOCOL_BRUSHED: return getAnalogPulse(_interval * 1000); + case ESC_PROTOCOL_DSHOT150: + case ESC_PROTOCOL_DSHOT300: + case ESC_PROTOCOL_DSHOT600: + default: + return 2048; + } +} + +uint32_t EscDriverEsp32::getPulseInterval() const +{ + return getAnalogPulse(_interval * 1000); +} + +uint32_t EscDriverEsp32::getAnalogPulse(int32_t ns) const +{ + int div = getClockDivider(); + return ns / ((div * DURATION_CLOCK) / 2); +} + +uint16_t EscDriverEsp32::getDshotPulse(uint32_t width) const +{ + int div = getClockDivider(); + switch (_protocol) + { + case ESC_PROTOCOL_DSHOT150: return ((width * 4) / (div * DURATION_CLOCK / 2)); + case ESC_PROTOCOL_DSHOT300: return ((width * 2) / (div * DURATION_CLOCK / 2)); + case ESC_PROTOCOL_DSHOT600: return ((width * 1) / (div * DURATION_CLOCK / 2)); + case ESC_PROTOCOL_BRUSHED: + case ESC_PROTOCOL_PWM: + case ESC_PROTOCOL_ONESHOT125: + case ESC_PROTOCOL_ONESHOT42: + case ESC_PROTOCOL_MULTISHOT: + default: + return 0; + } +} + +bool EscDriverEsp32::isDigital(EscProtocol protocol) const +{ + switch (protocol) + { + case ESC_PROTOCOL_DSHOT150: + case ESC_PROTOCOL_DSHOT300: + case ESC_PROTOCOL_DSHOT600: + return true; + default: + return false; + } +} #endif \ No newline at end of file diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index a68de8bf..6639fed2 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -4,82 +4,16 @@ #if defined(ESP32) && !defined(ESP32C3) #include "EscDriver.h" -#include #include -//#define DURATION 12.5 /* flash 80MHz => minimum time unit in ns */ -static const size_t DURATION_CLOCK = 25; // [ns] doubled value to increase precision -static const size_t ITEM_COUNT = EscDriverBase::DSHOT_BIT_COUNT + 1; -static const int32_t DURATION_MAX = 0x7fff; // max in 15 bits - -#define TO_INTERVAL(v) (1 * 1000 * 1000 / (v)) // [us] - -// faster esc response, but unsafe (no task synchronisation) -// set to 0 in case of issues -#if defined(ESP32S3) || defined(ESP32S2) -#define ESPFC_RMT_BYPASS_WRITE_SYNC 0 -#else -#define ESPFC_RMT_BYPASS_WRITE_SYNC 1 -#endif - -#if ESPFC_RMT_BYPASS_WRITE_SYNC -IRAM_ATTR static esp_err_t _rmt_fill_tx_items(rmt_channel_t channel, const rmt_item32_t* item, uint16_t item_num, uint16_t mem_offset) -{ - //RMT_CHECK(channel < RMT_CHANNEL_MAX, RMT_CHANNEL_ERROR_STR, (0)); - //RMT_CHECK((item != NULL), RMT_ADDR_ERROR_STR, ESP_ERR_INVALID_ARG); - //RMT_CHECK((item_num > 0), RMT_DRIVER_LENGTH_ERROR_STR, ESP_ERR_INVALID_ARG); - - /*Each block has 64 x 32 bits of data*/ - //uint8_t mem_cnt = RMT.conf_ch[channel].conf0.mem_size; - //RMT_CHECK((mem_cnt * RMT_MEM_ITEM_NUM >= item_num), RMT_WR_MEM_OVF_ERROR_STR, ESP_ERR_INVALID_ARG); - - //rmt_fill_memory(channel, item, item_num, mem_offset); - //portENTER_CRITICAL(&rmt_spinlock); - RMT.apb_conf.fifo_mask = RMT_DATA_MODE_MEM; - //portEXIT_CRITICAL(&rmt_spinlock); - for(size_t i = 0; i < item_num; i++) { - RMTMEM.chan[channel].data32[i + mem_offset].val = item[i].val; - } - return ESP_OK; -} -IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) -{ - //RMT_CHECK(channel < RMT_CHANNEL_MAX, RMT_CHANNEL_ERROR_STR, ESP_ERR_INVALID_ARG); - //portENTER_CRITICAL(&rmt_spinlock); - if(tx_idx_rst) { - RMT.conf_ch[channel].conf1.mem_rd_rst = 1; - } - RMT.conf_ch[channel].conf1.mem_owner = RMT_MEM_OWNER_TX; - RMT.conf_ch[channel].conf1.tx_start = 1; - //portEXIT_CRITICAL(&rmt_spinlock); - return ESP_OK; -} -#else -#define _rmt_tx_start rmt_tx_start -#define _rmt_fill_tx_items rmt_fill_tx_items -#endif - class EscDriverEsp32: public EscDriverBase { public: class Slot { public: - rmt_config_t dev; - rmt_item32_t items[ITEM_COUNT]; - EscProtocol protocol; - int32_t pulse_min; - int32_t pulse_max; - int32_t pulse_space; - int32_t pulse; - uint16_t divider; - uint16_t dshot_t0h; - uint16_t dshot_t0l; - uint16_t dshot_t1h; - uint16_t dshot_t1l; - - void setTerminate(int item) + void setTerminate(int item, bool val) { - items[item].val = 0ul; + items[item].val = val ? (1 << 15) : 0ul; } void setDshotBit(int item, bool val) @@ -116,306 +50,47 @@ class EscDriverEsp32: public EscDriverBase { return (int)dev.gpio_num != -1; } - }; - - EscDriverEsp32(): _protocol(ESC_PROTOCOL_PWM), _async(true), _rate(50), _digital(false) - { - for(size_t i = 0; i < ESC_CHANNEL_COUNT; i++) - { - _channel[i].dev.gpio_num = gpio_num_t(-1); - } - } - - void end() - { - for(size_t i = 0; i < ESC_CHANNEL_COUNT; i++) - { - if(!_channel[i].attached()) continue; - rmt_driver_uninstall(_channel[i].dev.channel); - } - _protocol = ESC_PROTOCOL_DISABLED; - } - - int begin(EscProtocol protocol, bool async, int32_t rate, int timer = 0) - { - (void)timer; // unused - _protocol = protocol; - _digital = isDigital(protocol); - _async = async; - _rate = rate; - _interval = TO_INTERVAL(_rate); - - return 1; - } + static const size_t ITEM_COUNT = DSHOT_BIT_COUNT + 1; + static const int32_t DURATION_MAX = 0x7fff; // max in 15 bits - int attach(size_t channel, int pin, int pulse) - { - if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; - initChannel(channel, (gpio_num_t)pin, pulse); - return 1; - } - - int write(size_t channel, int pulse) - { - if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; - _channel[channel].pulse = pulse; - return 1; - } - - void apply() - { - if(_protocol == ESC_PROTOCOL_DISABLED) return; - if(_async) return; - transmitAll(); - } + rmt_config_t dev; + rmt_item32_t items[ITEM_COUNT]; + EscProtocol protocol; + int32_t pulse_min; + int32_t pulse_max; + int32_t pulse_space; + int32_t pulse; + uint16_t divider; + uint16_t dshot_t0h; + uint16_t dshot_t0l; + uint16_t dshot_t1h; + uint16_t dshot_t1l; + }; - int pin(size_t channel) const - { - if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; - return _channel[channel].dev.gpio_num; - } + EscDriverEsp32(); + int begin(EscProtocol protocol, bool async, int32_t rate, int timer = 0); + void end(); + int attach(size_t channel, int pin, int pulse); + int write(size_t channel, int pulse); + void apply(); + int pin(size_t channel) const; private: - void initChannel(int i, gpio_num_t pin, int pulse) - { - if(pin == -1) return; - - pinMode(pin, OUTPUT); - digitalWrite(pin, LOW); - - _channel[i].protocol = _protocol; - _channel[i].pulse = pulse; - _channel[i].divider = getClockDivider(); - _channel[i].pulse_min = getPulseMin(); - _channel[i].pulse_max = getPulseMax(); - _channel[i].pulse_space = getPulseInterval(); - - // specification 0:37%, 1:75% of 1670ns for dshot600 - //_channel[i].dshot_t0h = getDshotPulse(625); - //_channel[i].dshot_t0l = getDshotPulse(1045); - //_channel[i].dshot_t1h = getDshotPulse(1250); - //_channel[i].dshot_t1l = getDshotPulse(420); - - // betaflight 0:35%, 1:70% of 1670ns for dshot600 - _channel[i].dshot_t0h = getDshotPulse(584 - 16); - _channel[i].dshot_t0l = getDshotPulse(1086 - 32); - _channel[i].dshot_t1h = getDshotPulse(1170 - 32); - _channel[i].dshot_t1l = getDshotPulse(500 - 16); - - _channel[i].dev.gpio_num = pin; - _channel[i].dev.rmt_mode = RMT_MODE_TX; - _channel[i].dev.channel = (rmt_channel_t)i; - _channel[i].dev.clk_div = _channel[i].divider; - _channel[i].dev.mem_block_num = 1; - _channel[i].dev.tx_config.loop_en = 0; - _channel[i].dev.tx_config.idle_output_en = 1; - _channel[i].dev.tx_config.idle_level = RMT_IDLE_LEVEL_LOW; - - // unused - _channel[i].dev.tx_config.carrier_duty_percent = 50; - _channel[i].dev.tx_config.carrier_freq_hz = 1000; - _channel[i].dev.tx_config.carrier_level = RMT_CARRIER_LEVEL_HIGH; - _channel[i].dev.tx_config.carrier_en = 0; - - instances[i] = this; - - rmt_config(&_channel[i].dev); - rmt_driver_install(_channel[i].dev.channel, 0, 0); - if (_async && !_tx_end_installed) - { - _tx_end_installed = true; - rmt_register_tx_end_callback(txDoneCallback, NULL); - } - if(_async) - { - rmt_set_tx_intr_en(_channel[i].dev.channel, true); - txDoneCallback((rmt_channel_t)i, NULL); // start generating pulses - } - } - - static void txDoneCallback(rmt_channel_t channel, void *arg) - { - if(instances[channel] && instances[channel]->_async) instances[channel]->transmitOne(channel); - } - - void transmitOne(uint8_t i) - { - if(!_channel[i].attached()) return; - if(_digital) - { - writeDshotCommand(i, _channel[i].pulse); - } - else - { - writeAnalogCommand(i, _channel[i].pulse); - } - transmitCommand(i); - } - - void transmitAll() - { - for(size_t i = 0; i < ESC_CHANNEL_COUNT; i++) - { - if(!_channel[i].attached()) continue; - if(_digital) - { - writeDshotCommand(i, _channel[i].pulse); - } - else - { - writeAnalogCommand(i, _channel[i].pulse); - } - } - for(size_t i = 0; i < ESC_CHANNEL_COUNT; i++) - { - if(!_channel[i].attached()) continue; - transmitCommand(i); - } - } - - void writeAnalogCommand(uint8_t channel, int32_t pulse) - { - Slot& slot = _channel[channel]; - int minPulse = 800; - int maxPulse = 2200; - if(_protocol == ESC_PROTOCOL_BRUSHED) - { - minPulse = 1000; - maxPulse = 2000; - } - pulse = constrain(pulse, minPulse, maxPulse); - int duration = map(pulse, 1000, 2000, slot.pulse_min, slot.pulse_max); - - int count = 2; - if(_async) - { - int space = slot.pulse_space - duration; - slot.setDuration(0, duration, 1); - slot.setDuration(1, space, 0); - slot.setTerminate(2); - count = 3; - } - else - { - slot.setDuration(0, duration, 1); - slot.setTerminate(1); - } - - _rmt_fill_tx_items(_channel[channel].dev.channel, _channel[channel].items, count, 0); - } - - void writeDshotCommand(uint8_t channel, int32_t pulse) - { - pulse = constrain(pulse, 0, 2000); - // scale to dshot commands (0 or 48-2047) - int value = pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; - uint16_t frame = dshotEncode(value); - - Slot& slot = _channel[channel]; - for(size_t i = 0; i < DSHOT_BIT_COUNT; i++) - { - int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01; - slot.setDshotBit(i, val); - } - slot.setTerminate(DSHOT_BIT_COUNT); - - _rmt_fill_tx_items(slot.dev.channel, slot.items, ITEM_COUNT, 0); - } - - void transmitCommand(uint8_t channel) - { - _rmt_tx_start(_channel[channel].dev.channel, true); - } - - uint32_t getClockDivider() const - { - switch(_protocol) - { - case ESC_PROTOCOL_PWM: return 24; - case ESC_PROTOCOL_ONESHOT125: return 4; - case ESC_PROTOCOL_ONESHOT42: return 2; - case ESC_PROTOCOL_MULTISHOT: return 2; - case ESC_PROTOCOL_BRUSHED: return 8; - default: return 1; - } - } - - uint32_t getPulseMin() const - { - switch(_protocol) - { - case ESC_PROTOCOL_PWM: return getAnalogPulse(1000 * 1000); - case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse( 125 * 1000); - case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse( 42 * 1000); - case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse( 5 * 1000); - case ESC_PROTOCOL_BRUSHED: return 0; - case ESC_PROTOCOL_DSHOT150: - case ESC_PROTOCOL_DSHOT300: - case ESC_PROTOCOL_DSHOT600: - default: - return 0; - } - } - - uint32_t getPulseMax() const - { - switch(_protocol) - { - case ESC_PROTOCOL_PWM: return getAnalogPulse(2000 * 1000); - case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse( 250 * 1000); - case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse( 84 * 1000); - case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse( 25 * 1000); - case ESC_PROTOCOL_BRUSHED: return getAnalogPulse(_interval * 1000); - case ESC_PROTOCOL_DSHOT150: - case ESC_PROTOCOL_DSHOT300: - case ESC_PROTOCOL_DSHOT600: - default: - return 2048; - } - } - - uint32_t getPulseInterval() const - { - return getAnalogPulse(_interval * 1000); - } - - uint32_t getAnalogPulse(int32_t ns) const - { - int div = getClockDivider(); - return ns / ((div * DURATION_CLOCK) / 2); - } - - uint16_t getDshotPulse(uint32_t width) const - { - int div = getClockDivider(); - switch(_protocol) - { - case ESC_PROTOCOL_DSHOT150: return ((width * 4) / (div * DURATION_CLOCK / 2)); - case ESC_PROTOCOL_DSHOT300: return ((width * 2) / (div * DURATION_CLOCK / 2)); - case ESC_PROTOCOL_DSHOT600: return ((width * 1) / (div * DURATION_CLOCK / 2)); - case ESC_PROTOCOL_BRUSHED: - case ESC_PROTOCOL_PWM: - case ESC_PROTOCOL_ONESHOT125: - case ESC_PROTOCOL_ONESHOT42: - case ESC_PROTOCOL_MULTISHOT: - default: - return 0; - } - } - - bool isDigital(EscProtocol protocol) const - { - switch(protocol) - { - case ESC_PROTOCOL_DSHOT150: - case ESC_PROTOCOL_DSHOT300: - case ESC_PROTOCOL_DSHOT600: - return true; - default: - return false; - } - } + void initChannel(int i, gpio_num_t pin, int pulse); + static void txDoneCallback(rmt_channel_t channel, void *arg) IRAM_ATTR; + void transmitOne(uint8_t i) IRAM_ATTR; + void transmitAll(); + void writeAnalogCommand(uint8_t channel, int32_t pulse) IRAM_ATTR; + void writeDshotCommand(uint8_t channel, int32_t pulse) IRAM_ATTR; + void transmitCommand(uint8_t channel) IRAM_ATTR; + uint32_t getClockDivider() const; + uint32_t getPulseMin() const; + uint32_t getPulseMax() const; + uint32_t getPulseInterval() const; + uint32_t getAnalogPulse(int32_t ns) const; + uint16_t getDshotPulse(uint32_t width) const; + bool isDigital(EscProtocol protocol) const; Slot _channel[ESC_CHANNEL_COUNT]; EscProtocol _protocol; From 9edbe97fb9bcbbb718875ce87d984eecee99ba0d Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 14 Feb 2024 10:21:16 +0100 Subject: [PATCH 17/28] inverted dshot + rx tx switching --- lib/EscDriver/src/EscDriver.h | 6 +- lib/EscDriver/src/EscDriverEsp32.cpp | 247 +++++++++++++++++++-------- lib/EscDriver/src/EscDriverEsp32.h | 37 ++-- lib/EspGpio/src/EspGpio.h | 13 +- 4 files changed, 221 insertions(+), 82 deletions(-) diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index c2a02783..85d82f18 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -32,7 +32,7 @@ class EscDriverBase int pin(size_t channel) const { return -1; } #endif - uint16_t dshotEncode(uint16_t value) + static inline uint16_t dshotEncode(uint16_t value, bool inverted = false) { value <<= 1; @@ -44,6 +44,10 @@ class EscDriverBase csum ^= csum_data; // xor csum_data >>= 4; } + if(inverted) + { + csum = ~csum; + } csum &= 0xf; return (value << 4) | csum; diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 40f2bc6f..8f0540d9 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -3,14 +3,25 @@ #include #include "EscDriverEsp32.h" #include "Debug_Espfc.h" +#include "soc/rmt_periph.h" +#include "hal/rmt_ll.h" +#include "hal/gpio_ll.h" +#include "esp_rom_gpio.h" static const size_t DURATION_CLOCK = 25; // [ns] doubled value to increase precision #define TO_INTERVAL_US(v) (1 * 1000 * 1000 / (v)) // [us] +#define RMT_RX_CHANNEL_ENCODING_START (SOC_RMT_CHANNELS_PER_GROUP - SOC_RMT_TX_CANDIDATES_PER_GROUP) +#define RMT_TX_CHANNEL_ENCODING_END (SOC_RMT_TX_CANDIDATES_PER_GROUP - 1) + +#define RMT_IS_RX_CHANNEL(channel) ((channel) >= RMT_RX_CHANNEL_ENCODING_START) +#define RMT_IS_TX_CHANNEL(channel) ((channel) <= RMT_TX_CHANNEL_ENCODING_END) +#define RMT_ENCODE_RX_CHANNEL(encode_chan) ((encode_chan + RMT_RX_CHANNEL_ENCODING_START)) + // faster esc response, but unsafe (no task synchronisation) // set to 0 in case of issues #if defined(ESP32S3) || defined(ESP32S2) -#define ESPFC_RMT_BYPASS_WRITE_SYNC 0 +#define ESPFC_RMT_BYPASS_WRITE_SYNC 1 #else #define ESPFC_RMT_BYPASS_WRITE_SYNC 1 #endif @@ -18,39 +29,20 @@ static const size_t DURATION_CLOCK = 25; // [ns] doubled value to increase preci #if ESPFC_RMT_BYPASS_WRITE_SYNC IRAM_ATTR static esp_err_t _rmt_fill_tx_items(rmt_channel_t channel, const rmt_item32_t* item, uint16_t item_num, uint16_t mem_offset) { - //RMT_CHECK(channel < RMT_CHANNEL_MAX, RMT_CHANNEL_ERROR_STR, (0)); - //RMT_CHECK((item != NULL), RMT_ADDR_ERROR_STR, ESP_ERR_INVALID_ARG); - //RMT_CHECK((item_num > 0), RMT_DRIVER_LENGTH_ERROR_STR, ESP_ERR_INVALID_ARG); - - /*Each block has 64 x 32 bits of data*/ - //uint8_t mem_cnt = RMT.conf_ch[channel].conf0.mem_size; - //RMT_CHECK((mem_cnt * RMT_MEM_ITEM_NUM >= item_num), RMT_WR_MEM_OVF_ERROR_STR, ESP_ERR_INVALID_ARG); - - //rmt_fill_memory(channel, item, item_num, mem_offset); - //portENTER_CRITICAL(&rmt_spinlock); - RMT.apb_conf.fifo_mask = RMT_DATA_MODE_MEM; - //portEXIT_CRITICAL(&rmt_spinlock); - //for(size_t i = 0; i < item_num; i++) { - // RMTMEM.chan[channel].data32[i + mem_offset].val = item[i].val; - //} - uint32_t *from = (uint32_t *)item; - volatile uint32_t *to = (volatile uint32_t *)&RMTMEM.chan[channel].data32[0].val; - to += mem_offset; - while (item_num--) { - *to++ = *from++; - } + //rmt_ll_enable_mem_access(&RMT, true); + rmt_ll_write_memory(&RMTMEM, channel, item, item_num, mem_offset); + rmt_ll_tx_reset_pointer(&RMT, channel); + rmt_ll_rx_set_mem_owner(&RMT, channel, RMT_MEM_OWNER_TX); return ESP_OK; } IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) { - //RMT_CHECK(channel < RMT_CHANNEL_MAX, RMT_CHANNEL_ERROR_STR, ESP_ERR_INVALID_ARG); - //portENTER_CRITICAL(&rmt_spinlock); if(tx_idx_rst) { - RMT.conf_ch[channel].conf1.mem_rd_rst = 1; + rmt_ll_tx_reset_pointer(&RMT, channel); // BC } - RMT.conf_ch[channel].conf1.mem_owner = RMT_MEM_OWNER_TX; - RMT.conf_ch[channel].conf1.tx_start = 1; - //portEXIT_CRITICAL(&rmt_spinlock); + rmt_ll_rx_set_mem_owner(&RMT, channel, RMT_MEM_OWNER_TX); // moved to _rmt_fill_tx_items + rmt_ll_clear_tx_end_interrupt(&RMT, channel); + rmt_ll_tx_start(&RMT, channel); return ESP_OK; } #else @@ -58,15 +50,36 @@ IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) #define _rmt_fill_tx_items rmt_fill_tx_items #endif +static int IRAM_ATTR _rmt_rx_get_mem_len_in_isr(rmt_channel_t channel) +{ + volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[RMT_ENCODE_RX_CHANNEL(channel)].data32; + for (size_t idx = 0; idx < RMT_MEM_ITEM_NUM; idx++) { + if (data[idx].duration0 == 0) { + return idx; + } else if (data[idx].duration1 == 0) { + return idx + 1; + } + } + return RMT_MEM_ITEM_NUM; +} + +static void IRAM_ATTR _rmt_zero_mem(rmt_channel_t channel, size_t len) +{ + volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[channel].data32; + for (size_t idx = 0; idx < len; idx++) { + data[idx].val = 0; + } +} + bool EscDriverEsp32::_tx_end_installed = false; EscDriverEsp32 *EscDriverEsp32::instances[] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; -EscDriverEsp32::EscDriverEsp32() : _protocol(ESC_PROTOCOL_PWM), _async(true), _rate(50), _digital(false) +EscDriverEsp32::EscDriverEsp32() : _protocol(ESC_PROTOCOL_PWM), _async(true), _rate(50), _digital(false), _invert(false), _channel_mask(0) { for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { - _channel[i].dev.gpio_num = gpio_num_t(-1); + _channel[i].pin = gpio_num_t(-1); } } @@ -75,7 +88,7 @@ void EscDriverEsp32::end() for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { if (!_channel[i].attached()) continue; - rmt_driver_uninstall(_channel[i].dev.channel); + rmt_driver_uninstall((rmt_channel_t)i); } _protocol = ESC_PROTOCOL_DISABLED; } @@ -86,6 +99,7 @@ int EscDriverEsp32::begin(EscProtocol protocol, bool async, int32_t rate, int ti _protocol = protocol; _digital = isDigital(protocol); + if(_digital) _invert = true; _async = async; _rate = rate; _interval = TO_INTERVAL_US(_rate); @@ -117,7 +131,7 @@ void EscDriverEsp32::apply() int EscDriverEsp32::pin(size_t channel) const { if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return -1; - return _channel[channel].dev.gpio_num; + return _channel[channel].pin; } void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) @@ -125,8 +139,10 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) if (pin == -1) return; pinMode(pin, OUTPUT); - digitalWrite(pin, LOW); + digitalWrite(pin, _invert ? HIGH : LOW); + if(_invert) gpio_pullup_en((gpio_num_t)pin); // ? + _channel[i].pin = pin; _channel[i].protocol = _protocol; _channel[i].pulse = pulse; _channel[i].divider = getClockDivider(); @@ -146,45 +162,125 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) _channel[i].dshot_t1h = getDshotPulse(1170 - 32); _channel[i].dshot_t1l = getDshotPulse(500 - 16); - _channel[i].dev.gpio_num = pin; - _channel[i].dev.rmt_mode = RMT_MODE_TX; - _channel[i].dev.channel = (rmt_channel_t)i; - _channel[i].dev.clk_div = _channel[i].divider; - _channel[i].dev.mem_block_num = 1; - _channel[i].dev.tx_config.loop_en = 0; - _channel[i].dev.tx_config.idle_output_en = 1; - _channel[i].dev.tx_config.idle_level = RMT_IDLE_LEVEL_LOW; - - // unused - _channel[i].dev.tx_config.carrier_duty_percent = 50; - _channel[i].dev.tx_config.carrier_freq_hz = 1000; - _channel[i].dev.tx_config.carrier_level = RMT_CARRIER_LEVEL_HIGH; - _channel[i].dev.tx_config.carrier_en = 0; - instances[i] = this; - rmt_config(&_channel[i].dev); - rmt_driver_install(_channel[i].dev.channel, 0, 0); - if (/*_async && */!_tx_end_installed) + // init as TX + rmt_config_t conf = RMT_DEFAULT_CONFIG_TX(pin, (rmt_channel_t)i); + conf.clk_div = _channel[i].divider; + conf.tx_config.idle_level = _invert ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; + rmt_config(&conf); + + // add RX specifics + int rx_ch = RMT_ENCODE_RX_CHANNEL(i); + rmt_ll_rx_set_idle_thres(&RMT, rx_ch, 1000); + rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 10); + rmt_ll_rx_enable_filter(&RMT, rx_ch, 1); + _rmt_zero_mem((rmt_channel_t)rx_ch, RMT_MEM_ITEM_NUM); + + // sync all channels + if(!_async) { - _tx_end_installed = true; - rmt_register_tx_end_callback(txDoneCallback, NULL); + rmt_ll_tx_enable_sync(&RMT, true); + _channel_mask |= (1 << i); + rmt_ll_tx_add_to_sync_group(&RMT, i); + rmt_ll_tx_reset_channels_clock_div(&RMT, _channel_mask); } - if (_async) + + // install driver + rmt_driver_install((rmt_channel_t)i, 0, ESP_INTR_FLAG_IRAM); + + // install tx_end callback if async pwm or bidir dshot + if(_async || (_digital && _invert)) { - rmt_set_tx_intr_en(_channel[i].dev.channel, true); - txDoneCallback((rmt_channel_t)i, NULL); // start generating pulses + // install only once + if(!_tx_end_installed) + { + _tx_end_installed = true; + rmt_register_tx_end_callback(txDoneCallback, NULL); + } + rmt_ll_enable_tx_end_interrupt(&RMT, i, true); + if (_async) + { + txDoneCallback((rmt_channel_t)i, NULL); // start generating pulses + } } } +void EscDriverEsp32::modeTx(rmt_channel_t channel) +{ + //PIN_DEBUG(HIGH); + disableRx(channel); + enableTx(channel); + //PIN_DEBUG(LOW); +} + +void EscDriverEsp32::modeRx(rmt_channel_t channel) +{ + //PIN_DEBUG(HIGH); + disableTx(channel); + enableRx(channel); + //PIN_DEBUG(LOW); +} + +void EscDriverEsp32::enableTx(rmt_channel_t channel) +{ + gpio_num_t gpio_num = (gpio_num_t)_channel[(size_t)channel].pin; + + //gpio_set_direction(gpio_num, GPIO_MODE_OUTPUT); + gpio_ll_input_disable(&GPIO, gpio_num); + gpio_ll_output_enable(&GPIO, gpio_num); + esp_rom_gpio_connect_out_signal(gpio_num, rmt_periph_signals.groups[0].channels[channel].tx_sig, false, 0); + rmt_ll_enable_tx_end_interrupt(&RMT, channel, true); +} + +void EscDriverEsp32::disableTx(rmt_channel_t channel) +{ + rmt_ll_tx_stop(&RMT, channel); +} + +void EscDriverEsp32::enableRx(rmt_channel_t channel) +{ + // NOTE: time critical function, execution must not exceed 5-6us + rmt_channel_t rx_ch = (rmt_channel_t)RMT_ENCODE_RX_CHANNEL(channel); + gpio_num_t gpio_num = (gpio_num_t)_channel[(size_t)channel].pin; + + //gpio_set_direction(gpio_num, GPIO_MODE_INPUT); + gpio_ll_input_enable(&GPIO, gpio_num); + gpio_ll_output_disable(&GPIO, gpio_num); + esp_rom_gpio_connect_in_signal(gpio_num, rmt_periph_signals.groups[0].channels[rx_ch].rx_sig, false); + + //rmt_rx_start((rmt_channel_t)i, true); + rmt_ll_rx_set_mem_owner(&RMT, rx_ch, RMT_MEM_OWNER_RX); + rmt_ll_rx_reset_pointer(&RMT, rx_ch); + rmt_ll_clear_rx_end_interrupt(&RMT, rx_ch); + rmt_ll_enable_rx_end_interrupt(&RMT, rx_ch, true); + _rmt_zero_mem(rx_ch, 1); // clear first item of rx buffer to avoid reading tx items + rmt_ll_rx_enable(&RMT, rx_ch, true); +} + +void EscDriverEsp32::disableRx(rmt_channel_t channel) +{ + rmt_channel_t rx_ch = (rmt_channel_t)RMT_ENCODE_RX_CHANNEL(channel); + + //rmt_rx_stop((rmt_channel_t)channel); + rmt_ll_enable_rx_end_interrupt(&RMT, rx_ch, false); + rmt_ll_rx_enable(&RMT, rx_ch, false); +} + void EscDriverEsp32::txDoneCallback(rmt_channel_t channel, void *arg) { - PIN_DEBUG(HIGH); - if (instances[channel] && instances[channel]->_async) + //PIN_DEBUG(HIGH); + auto instance = instances[channel]; + if(!instance) return; + if(instance->_async) { - instances[channel]->transmitOne(channel); + instance->transmitOne(channel); } - PIN_DEBUG(LOW); + if(instance->_digital && instance->_invert) + { + instance->modeRx(channel); + } + //PIN_DEBUG(LOW); } void EscDriverEsp32::transmitOne(uint8_t i) @@ -203,6 +299,7 @@ void EscDriverEsp32::transmitOne(uint8_t i) void EscDriverEsp32::transmitAll() { + readTelemetry(); for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { if (!_channel[i].attached()) continue; @@ -222,6 +319,17 @@ void EscDriverEsp32::transmitAll() } } +void EscDriverEsp32::readTelemetry() +{ + for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) + { + if (!_channel[i].attached()) continue; + if (!_digital) continue; + if (!_invert) continue; + _channel[i].telemetryValue = _rmt_rx_get_mem_len_in_isr((rmt_channel_t)i); + } +} + void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) { Slot &slot = _channel[channel]; @@ -250,30 +358,35 @@ void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) slot.setTerminate(1, 0); } - _rmt_fill_tx_items(_channel[channel].dev.channel, _channel[channel].items, count, 0); + _rmt_fill_tx_items((rmt_channel_t)channel, _channel[channel].items, count, 0); } void EscDriverEsp32::writeDshotCommand(uint8_t channel, int32_t pulse) { + if(_digital && _invert) + { + modeTx((rmt_channel_t)channel); + } + pulse = constrain(pulse, 0, 2000); // scale to dshot commands (0 or 48-2047) int value = pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; - uint16_t frame = dshotEncode(value); + uint16_t frame = dshotEncode(value, _invert); - Slot &slot = _channel[channel]; + Slot& slot = _channel[channel]; for (size_t i = 0; i < DSHOT_BIT_COUNT; i++) { int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01; - slot.setDshotBit(i, val); + slot.setDshotBit(i, val, _invert); } - slot.setTerminate(DSHOT_BIT_COUNT, 0); + slot.setTerminate(DSHOT_BIT_COUNT, _invert); - _rmt_fill_tx_items(slot.dev.channel, slot.items, Slot::ITEM_COUNT, 0); + _rmt_fill_tx_items((rmt_channel_t)channel, slot.items, Slot::ITEM_COUNT, 0); } void EscDriverEsp32::transmitCommand(uint8_t channel) { - _rmt_tx_start(_channel[channel].dev.channel, true); + _rmt_tx_start((rmt_channel_t)channel, true); } uint32_t EscDriverEsp32::getClockDivider() const diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 6639fed2..43a50d7e 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -11,23 +11,30 @@ class EscDriverEsp32: public EscDriverBase public: class Slot { public: - void setTerminate(int item, bool val) + void inline setTerminate(int item, bool val) { items[item].val = val ? (1 << 15) : 0ul; } - void setDshotBit(int item, bool val) + void inline setDshotBit(int item, bool val, bool inverted) { const uint32_t th = (val ? dshot_t1h : dshot_t0h) & DURATION_MAX; const uint32_t tl = (val ? dshot_t1l : dshot_t0l) & DURATION_MAX; - items[item].val = th | 1 << 15 | tl << 16; + if(!inverted) + { + items[item].val = (th | 1 << 15) | (tl << 16 | 0 << 31); + } + else + { + items[item].val = (th | 0 << 15) | (tl << 16 | 1 << 31); + } //items[item].duration0 = th; - //items[item].level0 = 1; + //items[item].level0 = inverted ? 0 : 1; //items[item].duration1 = tl; - //items[item].level1 = 0; + //items[item].level1 = inverted ? 1 : 0; } - void setDuration(int item, int duration, bool val) + void inline setDuration(int item, int duration, bool val) { if(duration >= 2) { @@ -46,16 +53,16 @@ class EscDriverEsp32: public EscDriverBase } } - bool attached() + bool inline attached() { - return (int)dev.gpio_num != -1; + return (int)pin != -1; } static const size_t ITEM_COUNT = DSHOT_BIT_COUNT + 1; static const int32_t DURATION_MAX = 0x7fff; // max in 15 bits - rmt_config_t dev; rmt_item32_t items[ITEM_COUNT]; + int pin; EscProtocol protocol; int32_t pulse_min; int32_t pulse_max; @@ -66,6 +73,7 @@ class EscDriverEsp32: public EscDriverBase uint16_t dshot_t0l; uint16_t dshot_t1h; uint16_t dshot_t1l; + int telemetryValue; }; EscDriverEsp32(); @@ -77,10 +85,17 @@ class EscDriverEsp32: public EscDriverBase int pin(size_t channel) const; private: - void initChannel(int i, gpio_num_t pin, int pulse); + void initChannel(int channel, gpio_num_t pin, int pulse); + void modeTx(rmt_channel_t channel) IRAM_ATTR; + void modeRx(rmt_channel_t channel) IRAM_ATTR; + void enableTx(rmt_channel_t channel) IRAM_ATTR; + void enableRx(rmt_channel_t channel) IRAM_ATTR; + void disableTx(rmt_channel_t channel) IRAM_ATTR; + void disableRx(rmt_channel_t channel) IRAM_ATTR; static void txDoneCallback(rmt_channel_t channel, void *arg) IRAM_ATTR; void transmitOne(uint8_t i) IRAM_ATTR; void transmitAll(); + void readTelemetry(); void writeAnalogCommand(uint8_t channel, int32_t pulse) IRAM_ATTR; void writeDshotCommand(uint8_t channel, int32_t pulse) IRAM_ATTR; void transmitCommand(uint8_t channel) IRAM_ATTR; @@ -98,6 +113,8 @@ class EscDriverEsp32: public EscDriverBase int32_t _rate; int32_t _interval; bool _digital; + bool _invert; + uint32_t _channel_mask; static bool _tx_end_installed; static EscDriverEsp32* instances[]; diff --git a/lib/EspGpio/src/EspGpio.h b/lib/EspGpio/src/EspGpio.h index 82701b22..1dc9c157 100644 --- a/lib/EspGpio/src/EspGpio.h +++ b/lib/EspGpio/src/EspGpio.h @@ -2,6 +2,9 @@ #define _ESP_GPIO_H_ #include +#if defined(ESP32) +#include "hal/gpio_ll.h" +#endif #if defined(ARCH_RP2040) typedef PinStatus pin_status_t; @@ -12,7 +15,7 @@ typedef uint8_t pin_status_t; class EspGpio { public: - static inline void digitalWrite(uint8_t pin, pin_status_t val) + static inline void IRAM_ATTR digitalWrite(uint8_t pin, pin_status_t val) { #if defined(ESP8266) if(pin < 16) @@ -26,7 +29,8 @@ class EspGpio else GP16O &= ~1; } #elif defined(ESP32) - ::gpio_set_level((gpio_num_t)pin, val); + //::gpio_set_level((gpio_num_t)pin, val); + gpio_ll_set_level(&GPIO, (gpio_num_t)pin, val); #elif defined(UNIT_TEST) // do nothing #else @@ -34,7 +38,7 @@ class EspGpio #endif } - static inline pin_status_t digitalRead(uint8_t pin) + static inline pin_status_t IRAM_ATTR digitalRead(uint8_t pin) { #if defined(ESP8266) if(pin < 16) @@ -47,7 +51,8 @@ class EspGpio } return 0; #elif defined(ESP32) - return ::gpio_get_level((gpio_num_t)pin); + //return ::gpio_get_level((gpio_num_t)pin); + return ::gpio_ll_get_level(&GPIO, (gpio_num_t)pin); #elif defined(UNIT_TEST) return 0; // do nothing From 0eb01d555e721f56c242c2a27b50acc9edb01263 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 14 Feb 2024 10:29:19 +0100 Subject: [PATCH 18/28] tx sync fix --- lib/EscDriver/src/EscDriverEsp32.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 8f0540d9..dad98051 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -177,6 +177,7 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) rmt_ll_rx_enable_filter(&RMT, rx_ch, 1); _rmt_zero_mem((rmt_channel_t)rx_ch, RMT_MEM_ITEM_NUM); +#if SOC_RMT_SUPPORT_TX_SYNCHRO // sync all channels if(!_async) { @@ -185,6 +186,7 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) rmt_ll_tx_add_to_sync_group(&RMT, i); rmt_ll_tx_reset_channels_clock_div(&RMT, _channel_mask); } +#endif // install driver rmt_driver_install((rmt_channel_t)i, 0, ESP_INTR_FLAG_IRAM); From d2eaf0b19291025e47e536e9f0f6af849928ef38 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 15 Feb 2024 11:36:40 +0100 Subject: [PATCH 19/28] esc config + msp + cli + rx buffer --- lib/AHRS/src/helper_3dmath.h | 1 + lib/EscDriver/src/EscDriver.h | 11 ++- lib/EscDriver/src/EscDriverEsp32.cpp | 57 ++++++++------ lib/EscDriver/src/EscDriverEsp32.h | 4 +- lib/EscDriver/src/EscDriverEsp32c3.cpp | 10 +-- lib/EscDriver/src/EscDriverEsp32c3.h | 2 +- lib/EscDriver/src/EscDriverEsp8266.cpp | 10 +-- lib/EscDriver/src/EscDriverEsp8266.h | 2 +- lib/EscDriver/src/EscDriverRP2040.cpp | 10 +-- lib/EscDriver/src/EscDriverRP2040.h | 2 +- lib/Espfc/src/Cli.h | 7 +- lib/Espfc/src/ModelConfig.h | 6 +- lib/Espfc/src/Msp/MspProcessor.h | 9 ++- lib/Espfc/src/Output/Mixer.h | 18 ++++- lib/Espfc/src/Target/TargetESP32.h | 98 +----------------------- lib/Espfc/src/Target/TargetESP32c3.h | 93 +--------------------- lib/Espfc/src/Target/TargetESP32s2.h | 94 +---------------------- lib/Espfc/src/Target/TargetESP32s3.h | 94 +---------------------- lib/Espfc/src/Target/TargetESP8266.h | 3 - lib/Espfc/src/Target/TargetEsp32Common.h | 97 +++++++++++++++++++++++ 20 files changed, 202 insertions(+), 426 deletions(-) create mode 100644 lib/Espfc/src/Target/TargetEsp32Common.h diff --git a/lib/AHRS/src/helper_3dmath.h b/lib/AHRS/src/helper_3dmath.h index 3292dcdb..7838dd8e 100644 --- a/lib/AHRS/src/helper_3dmath.h +++ b/lib/AHRS/src/helper_3dmath.h @@ -11,6 +11,7 @@ inline float invSqrt(float x) //return 1.f / sqrt(x); #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wstrict-aliasing" + #pragma GCC diagnostic ignored "-Wuninitialized" float halfx = 0.5f * x; float y = x; long i = *(long*)&y; diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 85d82f18..76b65212 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -17,6 +17,15 @@ enum EscProtocol { ESC_PROTOCOL_COUNT }; +struct EscConfig +{ + int timer; + EscProtocol protocol; + int rate; + bool async; + bool dshotTelemetry; +}; + #define PWM_TO_DSHOT(v) (((v - 1000) * 2) + 47) #define ESC_PROTOCOL_SANITIZE(p) (p > ESC_PROTOCOL_DSHOT600 && p != ESC_PROTOCOL_DISABLED ? ESC_PROTOCOL_DSHOT600 : p) @@ -24,7 +33,7 @@ class EscDriverBase { public: #if defined(UNIT_TEST) - int begin(EscProtocol protocol, bool async, int16_t rate, int timer = 0) { return 1; } + int begin(const EscConfig& conf) { return 1; } void end() {} int attach(size_t channel, int pin, int pulse) { return 1; } int write(size_t channel, int pulse) { return 1; } diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index dad98051..4cee4108 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -75,7 +75,7 @@ bool EscDriverEsp32::_tx_end_installed = false; EscDriverEsp32 *EscDriverEsp32::instances[] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL}; -EscDriverEsp32::EscDriverEsp32() : _protocol(ESC_PROTOCOL_PWM), _async(true), _rate(50), _digital(false), _invert(false), _channel_mask(0) +EscDriverEsp32::EscDriverEsp32() : _protocol(ESC_PROTOCOL_PWM), _async(true), _rate(50), _digital(false), _dshot_tlm(false), _channel_mask(0) { for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { @@ -93,15 +93,13 @@ void EscDriverEsp32::end() _protocol = ESC_PROTOCOL_DISABLED; } -int EscDriverEsp32::begin(EscProtocol protocol, bool async, int32_t rate, int timer) +int EscDriverEsp32::begin(const EscConfig& conf) { - (void)timer; // unused - - _protocol = protocol; - _digital = isDigital(protocol); - if(_digital) _invert = true; - _async = async; - _rate = rate; + _protocol = conf.protocol; + _digital = isDigital(conf.protocol); + _dshot_tlm = conf.dshotTelemetry; + _async = conf.async; + _rate = conf.rate; _interval = TO_INTERVAL_US(_rate); return 1; @@ -139,8 +137,8 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) if (pin == -1) return; pinMode(pin, OUTPUT); - digitalWrite(pin, _invert ? HIGH : LOW); - if(_invert) gpio_pullup_en((gpio_num_t)pin); // ? + digitalWrite(pin, _dshot_tlm ? HIGH : LOW); + if(_dshot_tlm) gpio_pullup_en((gpio_num_t)pin); // ? _channel[i].pin = pin; _channel[i].protocol = _protocol; @@ -167,13 +165,13 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) // init as TX rmt_config_t conf = RMT_DEFAULT_CONFIG_TX(pin, (rmt_channel_t)i); conf.clk_div = _channel[i].divider; - conf.tx_config.idle_level = _invert ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; + conf.tx_config.idle_level = _dshot_tlm ? RMT_IDLE_LEVEL_HIGH : RMT_IDLE_LEVEL_LOW; rmt_config(&conf); // add RX specifics int rx_ch = RMT_ENCODE_RX_CHANNEL(i); - rmt_ll_rx_set_idle_thres(&RMT, rx_ch, 1000); - rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 10); + rmt_ll_rx_set_idle_thres(&RMT, rx_ch, getAnalogPulse(30000)); // max bit len, (30us) + rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 40); // min bit len, 80 ticks = 1us for div = 1, max value = 255 rmt_ll_rx_enable_filter(&RMT, rx_ch, 1); _rmt_zero_mem((rmt_channel_t)rx_ch, RMT_MEM_ITEM_NUM); @@ -189,10 +187,10 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) #endif // install driver - rmt_driver_install((rmt_channel_t)i, 0, ESP_INTR_FLAG_IRAM); + rmt_driver_install((rmt_channel_t)i, 256, ESP_INTR_FLAG_IRAM); // install tx_end callback if async pwm or bidir dshot - if(_async || (_digital && _invert)) + if(_async || (_digital && _dshot_tlm)) { // install only once if(!_tx_end_installed) @@ -278,7 +276,7 @@ void EscDriverEsp32::txDoneCallback(rmt_channel_t channel, void *arg) { instance->transmitOne(channel); } - if(instance->_digital && instance->_invert) + if(instance->_digital && instance->_dshot_tlm) { instance->modeRx(channel); } @@ -323,13 +321,24 @@ void EscDriverEsp32::transmitAll() void EscDriverEsp32::readTelemetry() { + PIN_DEBUG(HIGH); for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { + if (!_digital || !_dshot_tlm) continue; if (!_channel[i].attached()) continue; - if (!_digital) continue; - if (!_invert) continue; - _channel[i].telemetryValue = _rmt_rx_get_mem_len_in_isr((rmt_channel_t)i); + //_channel[i].telemetryValue = _rmt_rx_get_mem_len_in_isr((rmt_channel_t)i); + + RingbufHandle_t rb = NULL; + rmt_get_ringbuf_handle((rmt_channel_t)i, &rb); + + size_t rmt_len = 0; + rmt_item32_t * data = (rmt_item32_t *)xRingbufferReceive(rb, &rmt_len, 0/*portMAX_DELAY*/); + if (data) { + // process data here + vRingbufferReturnItem(rb, (void *)data); + } } + PIN_DEBUG(LOW); } void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) @@ -365,7 +374,7 @@ void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) void EscDriverEsp32::writeDshotCommand(uint8_t channel, int32_t pulse) { - if(_digital && _invert) + if(_digital && _dshot_tlm) { modeTx((rmt_channel_t)channel); } @@ -373,15 +382,15 @@ void EscDriverEsp32::writeDshotCommand(uint8_t channel, int32_t pulse) pulse = constrain(pulse, 0, 2000); // scale to dshot commands (0 or 48-2047) int value = pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; - uint16_t frame = dshotEncode(value, _invert); + uint16_t frame = dshotEncode(value, _dshot_tlm); Slot& slot = _channel[channel]; for (size_t i = 0; i < DSHOT_BIT_COUNT; i++) { int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01; - slot.setDshotBit(i, val, _invert); + slot.setDshotBit(i, val, _dshot_tlm); } - slot.setTerminate(DSHOT_BIT_COUNT, _invert); + slot.setTerminate(DSHOT_BIT_COUNT, _dshot_tlm); _rmt_fill_tx_items((rmt_channel_t)channel, slot.items, Slot::ITEM_COUNT, 0); } diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 43a50d7e..5564a1dd 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -77,7 +77,7 @@ class EscDriverEsp32: public EscDriverBase }; EscDriverEsp32(); - int begin(EscProtocol protocol, bool async, int32_t rate, int timer = 0); + int begin(const EscConfig& conf); void end(); int attach(size_t channel, int pin, int pulse); int write(size_t channel, int pulse); @@ -113,7 +113,7 @@ class EscDriverEsp32: public EscDriverBase int32_t _rate; int32_t _interval; bool _digital; - bool _invert; + bool _dshot_tlm; uint32_t _channel_mask; static bool _tx_end_installed; diff --git a/lib/EscDriver/src/EscDriverEsp32c3.cpp b/lib/EscDriver/src/EscDriverEsp32c3.cpp index f2964ea2..aa4f1121 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.cpp +++ b/lib/EscDriver/src/EscDriverEsp32c3.cpp @@ -248,12 +248,12 @@ EscDriverEsp32c3::EscDriverEsp32c3(): _busy(false), _async(true), _protocol(ESC_ for(size_t i = 0; i < ESC_CHANNEL_COUNT * 2; ++i) _items[i] = Item(); } -int EscDriverEsp32c3::begin(EscProtocol protocol, bool async, int16_t rate, EscDriverTimer timer) +int EscDriverEsp32c3::begin(const EscConfig& conf) { - _protocol = ESC_PROTOCOL_SANITIZE(protocol); - _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : async; // force async for brushed - _rate = constrain(rate, 50, 8000); - _timer = timer; + _protocol = ESC_PROTOCOL_SANITIZE(conf.protocol); + _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : conf.async; // force async for brushed + _rate = constrain(conf.rate, 50, 8000); + _timer = (EscDriverTimer)conf.timer; _interval = usToTicksReal(_timer, 1000000L / _rate)/* - 400*/; // small compensation to keep frequency _intervalMin = _interval / 500; // do not generate brushed pulses if duty < ~0.2% (1002) _intervalMax = _interval - _intervalMin; // set brushed output hi if duty > ~99.8% (1998) diff --git a/lib/EscDriver/src/EscDriverEsp32c3.h b/lib/EscDriver/src/EscDriverEsp32c3.h index eb07bae9..faa2a0ba 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.h +++ b/lib/EscDriver/src/EscDriverEsp32c3.h @@ -43,7 +43,7 @@ class EscDriverEsp32c3: public EscDriverBase EscDriverEsp32c3(); - int begin(EscProtocol protocol, bool async, int16_t rate, EscDriverTimer timer); + int begin(const EscConfig& conf); void end(); int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; diff --git a/lib/EscDriver/src/EscDriverEsp8266.cpp b/lib/EscDriver/src/EscDriverEsp8266.cpp index 71a1162c..af592940 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.cpp +++ b/lib/EscDriver/src/EscDriverEsp8266.cpp @@ -352,12 +352,12 @@ EscDriverEsp8266::EscDriverEsp8266(): _busy(false), _async(true), _protocol(ESC_ for(size_t i = 0; i < ESC_CHANNEL_COUNT * 2; ++i) _items[i] = Item(); } -int EscDriverEsp8266::begin(EscProtocol protocol, bool async, int16_t rate, EscDriverTimer timer) +int EscDriverEsp8266::begin(const EscConfig& conf) { - _protocol = ESC_PROTOCOL_SANITIZE(protocol); - _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : async; // force async for brushed - _rate = constrain(rate, 50, 8000); - _timer = timer; + _protocol = ESC_PROTOCOL_SANITIZE(conf.protocol); + _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : conf.async; // force async for brushed + _rate = constrain(conf.rate, 50, 8000); + _timer = (EscDriverTimer)conf.timer; _interval = usToTicksReal(_timer, 1000000L / _rate)/* - 400*/; // small compensation to keep frequency _intervalMin = _interval / 500; // do not generate brushed pulses if duty < ~0.2% (1002) _intervalMax = _interval - _intervalMin; // set brushed output hi if duty > ~99.8% (1998) diff --git a/lib/EscDriver/src/EscDriverEsp8266.h b/lib/EscDriver/src/EscDriverEsp8266.h index 4fbbfc6f..66e9cb20 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.h +++ b/lib/EscDriver/src/EscDriverEsp8266.h @@ -50,7 +50,7 @@ class EscDriverEsp8266: public EscDriverBase EscDriverEsp8266(); - int begin(EscProtocol protocol, bool async, int16_t rate, EscDriverTimer timer); + int begin(const EscConfig& conf); void end(); int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; diff --git a/lib/EscDriver/src/EscDriverRP2040.cpp b/lib/EscDriver/src/EscDriverRP2040.cpp index 3bcf1b82..b31699a3 100644 --- a/lib/EscDriver/src/EscDriverRP2040.cpp +++ b/lib/EscDriver/src/EscDriverRP2040.cpp @@ -160,12 +160,12 @@ EscDriverRP2040::EscDriverRP2040(): _protocol(ESC_PROTOCOL_PWM), _async(true), _ for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i) _slots[i] = Slot(); } -int EscDriverRP2040::begin(EscProtocol protocol, bool async, int16_t rate, EscDriverTimer timer) +int EscDriverRP2040::begin(const EscConfig& conf) { - _timer = timer; - _protocol = ESC_PROTOCOL_SANITIZE(protocol); - _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : async; // force async for brushed - _rate = constrain(rate, 50, 8000); + _timer = (EscDriverTimer)conf.timer; + _protocol = ESC_PROTOCOL_SANITIZE(conf.protocol); + _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : conf.async; // force async for brushed + _rate = constrain(conf.rate, 50, 8000); _interval = usToTicksReal(1000000ul / _rate); _dl = nsToDshotTicks(DSHOT150_T0H); diff --git a/lib/EscDriver/src/EscDriverRP2040.h b/lib/EscDriver/src/EscDriverRP2040.h index 26c445aa..ac495f5d 100644 --- a/lib/EscDriver/src/EscDriverRP2040.h +++ b/lib/EscDriver/src/EscDriverRP2040.h @@ -38,7 +38,7 @@ class EscDriverRP2040: public EscDriverBase EscDriverRP2040(); - int begin(EscProtocol protocol, bool async, int16_t rate, EscDriverTimer timer); + int begin(const EscConfig& conf); void end(); int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 4d49e549..835ecb24 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -588,13 +588,18 @@ class Cli Param(PSTR("output_motor_protocol"), &c.output.protocol, protocolChoices), Param(PSTR("output_motor_async"), &c.output.async), Param(PSTR("output_motor_rate"), &c.output.rate), +#ifdef ESPFC_DSHOT_TELEMETRY + Param(PSTR("output_motor_poles"), &c.output.motorPoles), +#endif Param(PSTR("output_servo_rate"), &c.output.servoRate), Param(PSTR("output_min_command"), &c.output.minCommand), Param(PSTR("output_min_throttle"), &c.output.minThrottle), Param(PSTR("output_max_throttle"), &c.output.maxThrottle), Param(PSTR("output_dshot_idle"), &c.output.dshotIdle), - +#ifdef ESPFC_DSHOT_TELEMETRY + Param(PSTR("output_dshot_telemetry"), &c.output.dshotTelemetry), +#endif Param(PSTR("output_0"), &c.output.channel[0]), Param(PSTR("output_1"), &c.output.channel[1]), Param(PSTR("output_2"), &c.output.channel[2]), diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index f8be4ff0..633a11df 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -428,7 +428,9 @@ class OutputConfig { public: int8_t protocol; - int16_t async; + int8_t async; + int8_t dshotTelemetry; + int8_t motorPoles; int16_t rate; int16_t servoRate; @@ -814,6 +816,8 @@ class ModelConfig //output.async = true; output.async = false; output.servoRate = 0; // default 50, 0 to disable + output.dshotTelemetry = false; + output.motorPoles = 14; // input config input.ppmMode = PPM_MODE_NORMAL; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index e6c9ca8b..7dba113d 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -702,8 +702,8 @@ class MspProcessor r.writeU16(_model.config.output.minCommand); // mincommand r.writeU8(_model.state.currentMixer.count); // motor count // 1.42+ - r.writeU8(14); // motor pole count - r.writeU8(0); // dshot telemtery + r.writeU8(_model.config.output.motorPoles); // motor pole count + r.writeU8(_model.config.output.dshotTelemetry); // dshot telemtery r.writeU8(0); // esc sensor break; @@ -713,8 +713,13 @@ class MspProcessor _model.config.output.minCommand = m.readU16(); // mincommand if(m.remain() >= 2) { +#ifdef ESPFC_DSHOT_TELEMETRY + _model.config.output.motorPoles = m.readU8(); + _model.config.output.dshotTelemetry = m.readU8(); +#else m.readU8(); m.readU8(); +#endif } _model.reload(); break; diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 05e5339a..66115013 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -17,13 +17,27 @@ class Mixer int begin() { - escMotor.begin((EscProtocol)_model.config.output.protocol, _model.config.output.async, _model.config.output.rate, ESC_DRIVER_MOTOR_TIMER); + EscConfig motorConf = { + .timer = ESC_DRIVER_MOTOR_TIMER, + .protocol = (EscProtocol)_model.config.output.protocol, + .rate = _model.config.output.rate, + .async = !!_model.config.output.async, + .dshotTelemetry = !!_model.config.output.dshotTelemetry, + }; + escMotor.begin(motorConf); _model.state.escMotor = _motor = &escMotor; _model.logger.info().log(F("MOTOR CONF")).log(_model.config.output.protocol).log(_model.config.output.async).log(_model.config.output.rate).logln(ESC_DRIVER_MOTOR_TIMER); if(_model.config.output.servoRate) { - escServo.begin(ESC_PROTOCOL_PWM, true, _model.config.output.servoRate, ESC_DRIVER_SERVO_TIMER); + EscConfig servoConf = { + .timer = ESC_DRIVER_SERVO_TIMER, + .protocol = ESC_PROTOCOL_PWM, + .rate = _model.config.output.servoRate, + .async = true, + .dshotTelemetry = false, + }; + escServo.begin(servoConf); _model.state.escServo = _servo = &escServo; _model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); } diff --git a/lib/Espfc/src/Target/TargetESP32.h b/lib/Espfc/src/Target/TargetESP32.h index 55a30ef8..97524e3f 100644 --- a/lib/Espfc/src/Target/TargetESP32.h +++ b/lib/Espfc/src/Target/TargetESP32.h @@ -83,7 +83,7 @@ #define ESPFC_GYRO_I2C_RATE_MAX 2000 #define ESPFC_GYRO_SPI_RATE_MAX 8000 -//#define ESPFC_LOGGER_FS // doesn't compile on ESP32 +#define ESPFC_DSHOT_TELEMETRY #define ESPFC_FREE_RTOS #ifndef CONFIG_FREERTOS_UNICORE @@ -98,98 +98,4 @@ #include "Device/SerialDevice.h" -#define SERIAL_UART_PARITY_NONE 0B00000000 -#define SERIAL_UART_PARITY_EVEN 0B00000010 -#define SERIAL_UART_PARITY_ODD 0B00000011 - -#define SERIAL_UART_NB_BIT_5 0B00000000 -#define SERIAL_UART_NB_BIT_6 0B00000100 -#define SERIAL_UART_NB_BIT_7 0B00001000 -#define SERIAL_UART_NB_BIT_8 0B00001100 - -#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 -#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 -#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 -#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 - -namespace Espfc { - -template -inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) -{ - uint32_t sc = 0x8000000; - switch(conf.data_bits) - { - case 8: sc |= SERIAL_UART_NB_BIT_8; break; - case 7: sc |= SERIAL_UART_NB_BIT_7; break; - case 6: sc |= SERIAL_UART_NB_BIT_6; break; - case 5: sc |= SERIAL_UART_NB_BIT_5; break; - default: sc |= SERIAL_UART_NB_BIT_8; break; - } - switch(conf.parity) - { - case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; - case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; - default: sc |= SERIAL_UART_PARITY_NONE; break; - } - switch(conf.stop_bits) - { - case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; - case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; - case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; - default: break; - } - dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); - dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); - return 1; -} - -template -inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) -{ - dev.begin(sck, miso, mosi, ss); - return 1; -} - -template -inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) -{ - dev.begin(sda, scl, speed); - dev.setTimeout(50); - return 1; -} - -inline uint32_t getBoardId0() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)mac; -} - -inline uint32_t getBoardId1() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)(mac >> 32); -} - -inline uint32_t getBoardId2() -{ - return 0; -} - -inline void targetReset() -{ - ESP.restart(); - while(1) {} -} - -inline uint32_t targetCpuFreq() -{ - return ESP.getCpuFreqMHz(); -} - -inline uint32_t targetFreeHeap() -{ - return ESP.getFreeHeap(); -} - -}; +#include "Target/TargetEsp32Common.h" diff --git a/lib/Espfc/src/Target/TargetESP32c3.h b/lib/Espfc/src/Target/TargetESP32c3.h index 00af5719..3fe1d0fa 100644 --- a/lib/Espfc/src/Target/TargetESP32c3.h +++ b/lib/Espfc/src/Target/TargetESP32c3.h @@ -73,57 +73,14 @@ #define ESPFC_GYRO_I2C_RATE_MAX 1000 #define ESPFC_GYRO_SPI_RATE_MAX 2000 -//#define ESPFC_LOGGER_FS // doesn't compile on ESP32 #define ESPFC_DSP #include "Device/SerialDevice.h" -#define SERIAL_UART_PARITY_NONE 0B00000000 -#define SERIAL_UART_PARITY_EVEN 0B00000010 -#define SERIAL_UART_PARITY_ODD 0B00000011 - -#define SERIAL_UART_NB_BIT_5 0B00000000 -#define SERIAL_UART_NB_BIT_6 0B00000100 -#define SERIAL_UART_NB_BIT_7 0B00001000 -#define SERIAL_UART_NB_BIT_8 0B00001100 - -#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 -#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 -#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 -#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 +#include "Target/TargetEsp32Common.h" namespace Espfc { -template -inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) -{ - uint32_t sc = 0x8000000; - switch(conf.data_bits) - { - case 8: sc |= SERIAL_UART_NB_BIT_8; break; - case 7: sc |= SERIAL_UART_NB_BIT_7; break; - case 6: sc |= SERIAL_UART_NB_BIT_6; break; - case 5: sc |= SERIAL_UART_NB_BIT_5; break; - default: sc |= SERIAL_UART_NB_BIT_8; break; - } - switch(conf.parity) - { - case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; - case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; - default: sc |= SERIAL_UART_PARITY_NONE; break; - } - switch(conf.stop_bits) - { - case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; - case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; - case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; - default: break; - } - dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); - dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); - return 1; -} - template<> inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) { @@ -132,52 +89,4 @@ inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) return 1; } -template -inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) -{ - dev.begin(sck, miso, mosi, ss); - return 1; -} - -template -inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) -{ - dev.begin(sda, scl, speed); - return 1; -} - -inline uint32_t getBoardId0() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)mac; } - -inline uint32_t getBoardId1() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)(mac >> 32); -} - -inline uint32_t getBoardId2() -{ - return 0; -} - -inline void targetReset() -{ - ESPFC_SERIAL_USB_DEV.end(); - ESP.restart(); - while(1) {} -} - -inline uint32_t targetCpuFreq() -{ - return ESP.getCpuFreqMHz(); -} - -inline uint32_t targetFreeHeap() -{ - return ESP.getFreeHeap(); -} - -}; diff --git a/lib/Espfc/src/Target/TargetESP32s2.h b/lib/Espfc/src/Target/TargetESP32s2.h index 07f690d3..d379dc91 100644 --- a/lib/Espfc/src/Target/TargetESP32s2.h +++ b/lib/Espfc/src/Target/TargetESP32s2.h @@ -74,7 +74,7 @@ #define ESPFC_GYRO_I2C_RATE_MAX 2000 #define ESPFC_GYRO_SPI_RATE_MAX 2000 -//#define ESPFC_LOGGER_FS // doesn't compile on ESP32 +#define ESPFC_DSHOT_TELEMETRY #define ESPFC_FREE_RTOS #ifndef CONFIG_FREERTOS_UNICORE @@ -88,52 +88,10 @@ #include "Device/SerialDevice.h" -#define SERIAL_UART_PARITY_NONE 0B00000000 -#define SERIAL_UART_PARITY_EVEN 0B00000010 -#define SERIAL_UART_PARITY_ODD 0B00000011 - -#define SERIAL_UART_NB_BIT_5 0B00000000 -#define SERIAL_UART_NB_BIT_6 0B00000100 -#define SERIAL_UART_NB_BIT_7 0B00001000 -#define SERIAL_UART_NB_BIT_8 0B00001100 - -#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 -#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 -#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 -#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 +#include "Target/TargetEsp32Common.h" namespace Espfc { -template -inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) -{ - uint32_t sc = 0x8000000; - switch(conf.data_bits) - { - case 8: sc |= SERIAL_UART_NB_BIT_8; break; - case 7: sc |= SERIAL_UART_NB_BIT_7; break; - case 6: sc |= SERIAL_UART_NB_BIT_6; break; - case 5: sc |= SERIAL_UART_NB_BIT_5; break; - default: sc |= SERIAL_UART_NB_BIT_8; break; - } - switch(conf.parity) - { - case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; - case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; - default: sc |= SERIAL_UART_PARITY_NONE; break; - } - switch(conf.stop_bits) - { - case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; - case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; - case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; - default: break; - } - dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); - dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); - return 1; -} - template<> inline int targetSerialInit(USBCDC& dev, const SerialDeviceConfig& conf) { @@ -142,52 +100,4 @@ inline int targetSerialInit(USBCDC& dev, const SerialDeviceConfig& conf) return 1; } -template -inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) -{ - dev.begin(sck, miso, mosi, ss); - return 1; -} - -template -inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) -{ - dev.begin(sda, scl, speed); - dev.setTimeout(50); - return 1; -} - -inline uint32_t getBoardId0() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)mac; } - -inline uint32_t getBoardId1() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)(mac >> 32); -} - -inline uint32_t getBoardId2() -{ - return 0; -} - -inline void targetReset() -{ - ESP.restart(); - while(1) {} -} - -inline uint32_t targetCpuFreq() -{ - return ESP.getCpuFreqMHz(); -} - -inline uint32_t targetFreeHeap() -{ - return ESP.getFreeHeap(); -} - -}; diff --git a/lib/Espfc/src/Target/TargetESP32s3.h b/lib/Espfc/src/Target/TargetESP32s3.h index 421b6158..66e28f0a 100644 --- a/lib/Espfc/src/Target/TargetESP32s3.h +++ b/lib/Espfc/src/Target/TargetESP32s3.h @@ -84,7 +84,7 @@ #define ESPFC_GYRO_I2C_RATE_MAX 2000 #define ESPFC_GYRO_SPI_RATE_MAX 8000 -//#define ESPFC_LOGGER_FS // doesn't compile on ESP32 +#define ESPFC_DSHOT_TELEMETRY #define ESPFC_FREE_RTOS #ifndef CONFIG_FREERTOS_UNICORE @@ -99,52 +99,10 @@ #include "Device/SerialDevice.h" -#define SERIAL_UART_PARITY_NONE 0B00000000 -#define SERIAL_UART_PARITY_EVEN 0B00000010 -#define SERIAL_UART_PARITY_ODD 0B00000011 - -#define SERIAL_UART_NB_BIT_5 0B00000000 -#define SERIAL_UART_NB_BIT_6 0B00000100 -#define SERIAL_UART_NB_BIT_7 0B00001000 -#define SERIAL_UART_NB_BIT_8 0B00001100 - -#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 -#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 -#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 -#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 +#include "Target/TargetEsp32Common.h" namespace Espfc { -template -inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) -{ - uint32_t sc = 0x8000000; - switch(conf.data_bits) - { - case 8: sc |= SERIAL_UART_NB_BIT_8; break; - case 7: sc |= SERIAL_UART_NB_BIT_7; break; - case 6: sc |= SERIAL_UART_NB_BIT_6; break; - case 5: sc |= SERIAL_UART_NB_BIT_5; break; - default: sc |= SERIAL_UART_NB_BIT_8; break; - } - switch(conf.parity) - { - case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; - case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; - default: sc |= SERIAL_UART_PARITY_NONE; break; - } - switch(conf.stop_bits) - { - case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; - case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; - case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; - default: break; - } - dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); - dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); - return 1; -} - template<> inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) { @@ -153,52 +111,4 @@ inline int targetSerialInit(HWCDC& dev, const SerialDeviceConfig& conf) return 1; } -template -inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) -{ - dev.begin(sck, miso, mosi, ss); - return 1; -} - -template -inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) -{ - dev.begin(sda, scl, speed); - dev.setTimeout(50); - return 1; -} - -inline uint32_t getBoardId0() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)mac; } - -inline uint32_t getBoardId1() -{ - const int64_t mac = ESP.getEfuseMac(); - return (uint32_t)(mac >> 32); -} - -inline uint32_t getBoardId2() -{ - return 0; -} - -inline void targetReset() -{ - ESP.restart(); - while(1) {} -} - -inline uint32_t targetCpuFreq() -{ - return ESP.getCpuFreqMHz(); -} - -inline uint32_t targetFreeHeap() -{ - return ESP.getFreeHeap(); -} - -}; diff --git a/lib/Espfc/src/Target/TargetESP8266.h b/lib/Espfc/src/Target/TargetESP8266.h index 5396d3ff..4a7a11fc 100644 --- a/lib/Espfc/src/Target/TargetESP8266.h +++ b/lib/Espfc/src/Target/TargetESP8266.h @@ -58,9 +58,6 @@ #define ESPFC_WIFI_ALT -//#define ESPFC_LOGGER_FS // deprecated -#define ESPFC_LOGGER_FS_ALT - #include "Device/SerialDevice.h" namespace Espfc { diff --git a/lib/Espfc/src/Target/TargetEsp32Common.h b/lib/Espfc/src/Target/TargetEsp32Common.h new file mode 100644 index 00000000..281fd9e3 --- /dev/null +++ b/lib/Espfc/src/Target/TargetEsp32Common.h @@ -0,0 +1,97 @@ +#pragma once + +#define SERIAL_UART_PARITY_NONE 0B00000000 +#define SERIAL_UART_PARITY_EVEN 0B00000010 +#define SERIAL_UART_PARITY_ODD 0B00000011 + +#define SERIAL_UART_NB_BIT_5 0B00000000 +#define SERIAL_UART_NB_BIT_6 0B00000100 +#define SERIAL_UART_NB_BIT_7 0B00001000 +#define SERIAL_UART_NB_BIT_8 0B00001100 + +#define SERIAL_UART_NB_STOP_BIT_0 0B00000000 +#define SERIAL_UART_NB_STOP_BIT_1 0B00010000 +#define SERIAL_UART_NB_STOP_BIT_15 0B00100000 +#define SERIAL_UART_NB_STOP_BIT_2 0B00110000 + +namespace Espfc { + +template +inline int targetSerialInit(T& dev, const SerialDeviceConfig& conf) +{ + uint32_t sc = 0x8000000; + switch(conf.data_bits) + { + case 8: sc |= SERIAL_UART_NB_BIT_8; break; + case 7: sc |= SERIAL_UART_NB_BIT_7; break; + case 6: sc |= SERIAL_UART_NB_BIT_6; break; + case 5: sc |= SERIAL_UART_NB_BIT_5; break; + default: sc |= SERIAL_UART_NB_BIT_8; break; + } + switch(conf.parity) + { + case SDC_SERIAL_PARITY_EVEN: sc |= SERIAL_UART_PARITY_EVEN; break; + case SDC_SERIAL_PARITY_ODD: sc |= SERIAL_UART_PARITY_ODD; break; + default: sc |= SERIAL_UART_PARITY_NONE; break; + } + switch(conf.stop_bits) + { + case SDC_SERIAL_STOP_BITS_2: sc |= SERIAL_UART_NB_STOP_BIT_2; break; + case SDC_SERIAL_STOP_BITS_15: sc |= SERIAL_UART_NB_STOP_BIT_15; break; + case SDC_SERIAL_STOP_BITS_1: sc |= SERIAL_UART_NB_STOP_BIT_1; break; + default: break; + } + dev.setTxBufferSize(SERIAL_TX_FIFO_SIZE); + dev.begin(conf.baud, sc, conf.rx_pin, conf.tx_pin, conf.inverted); + return 1; +} + +template +inline int targetSPIInit(T& dev, int8_t sck, int8_t mosi, int8_t miso, int8_t ss) +{ + dev.begin(sck, miso, mosi, ss); + return 1; +} + +template +inline int targetI2CInit(T& dev, int8_t sda, int8_t scl, uint32_t speed) +{ + dev.begin(sda, scl, speed); + dev.setTimeout(50); + return 1; +} + +inline uint32_t getBoardId0() +{ + const int64_t mac = ESP.getEfuseMac(); + return (uint32_t)mac; +} + +inline uint32_t getBoardId1() +{ + const int64_t mac = ESP.getEfuseMac(); + return (uint32_t)(mac >> 32); +} + +inline uint32_t getBoardId2() +{ + return 0; +} + +inline void targetReset() +{ + ESP.restart(); + while(1) {} +} + +inline uint32_t targetCpuFreq() +{ + return ESP.getCpuFreqMHz(); +} + +inline uint32_t targetFreeHeap() +{ + return ESP.getFreeHeap(); +} + +}; From cf7c22304865eee49d46048d1daab6059c894b74 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 17 Feb 2024 00:07:11 +0100 Subject: [PATCH 20/28] decode gcr + erpm + extended telemetry + esc tests --- docs/dshot_gcr_dump.txt | 47 ++++++++++++ lib/EscDriver/src/EscDriver.h | 110 +++++++++++++++++++++++++++ lib/EscDriver/src/EscDriverEsp32.cpp | 69 +++++++++++------ lib/EscDriver/src/EscDriverEsp32.h | 5 +- lib/Espfc/src/Model.h | 1 + lib/Espfc/src/ModelState.h | 11 +++ lib/Espfc/src/Msp/MspProcessor.h | 29 +++++++ lib/Espfc/src/Output/Mixer.h | 69 +++++++++++++++++ test/test_esc/test_esc.cpp | 51 +++++++++++++ 9 files changed, 366 insertions(+), 26 deletions(-) create mode 100644 docs/dshot_gcr_dump.txt create mode 100644 test/test_esc/test_esc.cpp diff --git a/docs/dshot_gcr_dump.txt b/docs/dshot_gcr_dump.txt new file mode 100644 index 00000000..10c04dfb --- /dev/null +++ b/docs/dshot_gcr_dump.txt @@ -0,0 +1,47 @@ +// bluejay esc, dshot 300 + +// power off +32 0:375 1:168 0:10368 0:14 0:381 1:162 0:172 1:172 0:384 1:159 0:178 1:168 0:166 1:168 0:603 2950 2950 +32 0:381 1:165 0:172 1:172 0:377 1:176 0:165 1:171 0:381 1:169 0:168 1:172 0:175 1:172 0:593 52950 52950 +32 0:381 1:172 0:171 1:165 0:385 1:168 0:168 1:169 0:381 1:169 0:178 1:162 0:175 1:171 0:607 52950 52950 +32 0:380 1:172 0:175 1:172 0:371 1:172 0:175 1:172 0:371 1:172 0:174 1:162 0:172 1:172 0:606 52950 52950 +32 0:380 1:170 0:170 1:173 0:376 1:173 0:171 1:170 0:389 1:170 0:173 1:177 0:160 1:174 0:598 52950 52950 +32 0:380 1:173 0:173 1:174 0:369 1:174 0:173 1:173 0:370 1:173 0:174 1:163 0:170 1:173 0:606 52950 52950 +32 0:380 1:163 0:170 1:174 0:382 1:160 0:177 1:170 0:373 1:170 0:174 1:176 0:160 1:174 0:598 52950 52950 +32 0:379 1:167 0:170 1:174 0:376 1:176 0:164 1:173 0:380 1:170 0:167 1:173 0:173 1:174 0:592 52950 52950 +32 0:380 1:173 0:177 1:170 0:382 1:174 0:173 1:163 0:380 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950 + +// power on +24 0:373 1:605 0:163 1:605 0:174 1:376 0:379 1:599 0:177 1:163 0:173 1EF67A 1EF67A +24 0:379 1:599 0:170 1:654 0:170 1:170 0:184 1:605 0:173 1:379 0:380 1EF5EC 1EF5EC +24 0:383 1:605 0:163 1:602 0:177 1:163 0:383 1:170 0:170 1:167 0:605 F7A50 F7A50 +24 0:370 1:601 0:174 1:605 0:595 1:173 0:174 1:163 0:380 1:379 0:174 1EF0A6 1EF0A6 +28 0:379 1:595 0:174 1:605 0:164 1:173 0:173 1:379 0:164 1:173 0:174 1:163 0:380 F7AD4 F7AD4 +24 0:383 1:605 0:167 1:608 0:161 1:379 0:608 1:164 0:173 1:170 0:167 F7B0A F7B0A + +// armed +28 0:177 1:379 0:177 1:167 0:173 1:164 0:379 1:177 0:373 1:379 0:380 1:608 0:164 D499E D499E +24 0:173 1:379 0:167 1:173 0:167 1:605 0:386 1:164 0:379 1:376 0:602 D7930 D7930 +24 0:183 1:380 0:173 1:173 0:164 1:605 0:380 1:604 0:164 1:379 0:664 35E7B0 35E7B0 +24 0:174 1:379 0:173 1:164 0:226 1:601 0:383 1:599 0:376 1:180 0:373 D79E4 D79E4 +28 0:173 1:380 0:163 1:173 0:174 1:602 0:595 1:173 0:380 1:173 0:380 1:163 0:173 1AF092 1AF092 +28 0:167 1:379 0:170 1:173 0:164 1:602 0:602 1:173 0:173 1:380 0:163 1:173 0:380 1AF0B4 1AF0B4 +28 0:164 1:379 0:174 1:173 0:164 1:605 0:605 1:163 0:174 1:379 0:174 1:163 0:380 1AF0B4 1AF0B4 + +// disarmed +28 0:380 1:595 0:173 1:605 0:164 1:173 0:174 1:379 0:164 1:173 0:174 1:163 0:380 F7AD4 F7AD4 +24 0:379 1:602 0:173 1:599 0:170 1:386 0:386 1:608 0:164 1:173 0:174 1EF67A 1EF67A +16 0:380 1:595 0:10368 0:14 0:599 1:173 0:1 1 1 +24 0:373 1:602 0:180 1:598 0:380 1:166 0:171 1:166 0:383 1:173 0:377 7BCA4 7BCA4 +20 0:380 1:605 0:173 1:596 0:605 1:173 0:379 1:380 0:595 3DE130 3DE130 + +// power off +32 0:383 1:173 0:164 1:170 0:382 1:174 0:173 1:164 0:379 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950 +32 0:382 1:170 0:167 1:170 0:380 1:170 0:177 1:163 0:380 1:173 0:170 1:167 0:173 1:174 0:595 52950 52950 +32 0:383 1:170 0:164 1:170 0:383 1:176 0:161 1:173 0:379 1:174 0:177 1:170 0:173 1:177 0:592 52950 52950 +32 0:379 1:173 0:174 1:173 0:370 1:173 0:174 1:173 0:369 1:174 0:173 1:164 0:170 1:173 0:605 52950 52950 +32 0:382 1:170 0:164 1:170 0:383 1:176 0:161 1:173 0:380 1:173 0:177 1:170 0:173 1:177 0:592 52950 52950 +32 0:383 1:170 0:167 1:170 0:380 1:170 0:177 1:163 0:380 1:173 0:170 1:167 0:173 1:173 0:596 52950 52950 +32 0:384 1:175 0:162 1:172 0:381 1:172 0:178 1:169 0:384 1:172 0:175 1:162 0:171 1:172 0:597 52950 52950 +32 0:387 1:162 0:172 1:169 0:390 1:159 0:175 1:169 0:384 1:175 0:172 1:171 0:179 1:158 0:604 52950 52950 +32 0:381 1:168 0:172 1:172 0:378 1:171 0:172 1:169 0:390 1:169 0:175 1:175 0:162 1:172 0:600 52950 52950 diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 76b65212..93a22d6b 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -39,8 +39,14 @@ class EscDriverBase int write(size_t channel, int pulse) { return 1; } void apply() {} int pin(size_t channel) const { return -1; } + uint32_t telemetry(size_t channel) const { return 0; } #endif + static inline uint16_t dshotConvert(uint16_t pulse) + { + return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0; + } + static inline uint16_t dshotEncode(uint16_t value, bool inverted = false) { value <<= 1; @@ -62,6 +68,110 @@ class EscDriverBase return (value << 4) | csum; } + static inline uint32_t durationToBitLen(uint32_t duration, uint32_t len) + { + return (duration + (len >> 1)) / len; + } + + static inline uint32_t pushBits(uint32_t value, uint32_t bitVal, size_t bitLen) + { + while(bitLen--) + { + value <<= 1; + value |= bitVal; + } + return value; + } + + /** + * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1) + * @param len number of data items + * @param bitLen duration of single bit + * @return uint32_t raw gcr value + */ + static inline uint32_t extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen) + { + int bitCount = 0; + uint32_t value = 0; + for(size_t i = 0; i < len; i++) + { + uint32_t duration0 = data[i] & 0x7fff; + if(!duration0) break; + + uint32_t level0 = (data[i] >> 15) & 0x01; + uint32_t len0 = durationToBitLen(duration0, bitLen); + value = pushBits(value, level0, len0); + bitCount += len0; + + uint32_t duration1 = (data[i] >> 16) & 0x7fff; + if(!duration1) break; + uint32_t level1 = (data[i] >> 31) & 0x01; + uint32_t len1 = durationToBitLen(duration1, bitLen); + value = pushBits(value, level1, len1); + bitCount += len1; + } + + // fill missing bits with 1 + if(bitCount < 21) + { + value = pushBits(value, 1, 21 - bitCount); + } + value = value ^ (value >> 1); // extract gcr + + return value; + } + + static const uint32_t INVALID_TELEMETRY_VALUE = 0xffff; + + static inline uint32_t convertToErpm(uint32_t value) + { + if (!value || value == INVALID_TELEMETRY_VALUE) { + return INVALID_TELEMETRY_VALUE; + } + + // Convert period to erpm * 100 + return (1000000 * 60 / 100 + value / 2) / value; + } + + static inline uint32_t convertToValue(uint32_t value) + { + // eRPM range + if (value == 0x0fff) { + return 0; + } + + // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm) + return (value & 0x01ff) << ((value & 0xfe00) >> 9); + } + + static uint32_t gcrToRawValue(uint32_t value) + { + constexpr uint32_t iv = 0xffffffff; // invalid code + // First bit is start bit so discard it. + value &= 0xfffff; + static const uint32_t decode[32] = { + iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15, + iv, iv, 2, 3, iv, 5, 6, 7, iv, 0, 8, 1, iv, 4, 12, iv, + }; + + uint32_t decodedValue = decode[value & 0x1f]; + decodedValue |= decode[(value >> 5) & 0x1f] << 4; + decodedValue |= decode[(value >> 10) & 0x1f] << 8; + decodedValue |= decode[(value >> 15) & 0x1f] << 12; + + uint32_t csum = decodedValue; + csum = csum ^ (csum >> 8); // xor bytes + csum = csum ^ (csum >> 4); // xor nibbles + + if ((csum & 0xf) != 0xf || decodedValue > 0xffff) { + value = INVALID_TELEMETRY_VALUE; + } else { + value = decodedValue >> 4; + } + + return value; + } + static const size_t DSHOT_BIT_COUNT = 16; }; diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 4cee4108..7d67b20b 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -50,19 +50,6 @@ IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) #define _rmt_fill_tx_items rmt_fill_tx_items #endif -static int IRAM_ATTR _rmt_rx_get_mem_len_in_isr(rmt_channel_t channel) -{ - volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[RMT_ENCODE_RX_CHANNEL(channel)].data32; - for (size_t idx = 0; idx < RMT_MEM_ITEM_NUM; idx++) { - if (data[idx].duration0 == 0) { - return idx; - } else if (data[idx].duration1 == 0) { - return idx + 1; - } - } - return RMT_MEM_ITEM_NUM; -} - static void IRAM_ATTR _rmt_zero_mem(rmt_channel_t channel, size_t len) { volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[channel].data32; @@ -95,12 +82,13 @@ void EscDriverEsp32::end() int EscDriverEsp32::begin(const EscConfig& conf) { - _protocol = conf.protocol; - _digital = isDigital(conf.protocol); - _dshot_tlm = conf.dshotTelemetry; - _async = conf.async; - _rate = conf.rate; + _protocol = ESC_PROTOCOL_SANITIZE(conf.protocol); + _async = _protocol == ESC_PROTOCOL_BRUSHED ? true : conf.async; // force async for brushed + _rate = constrain(conf.rate, 50, 8000); _interval = TO_INTERVAL_US(_rate); + _digital = isDigital(_protocol); + _dshot_tlm = conf.dshotTelemetry && (_protocol == ESC_PROTOCOL_DSHOT300 || _protocol == ESC_PROTOCOL_DSHOT600); + _timer.setInterval(1000000); return 1; } @@ -132,6 +120,12 @@ int EscDriverEsp32::pin(size_t channel) const return _channel[channel].pin; } +uint32_t EscDriverEsp32::telemetry(size_t channel) const +{ + if (channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; + return _channel[channel].telemetryValue; +} + void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) { if (pin == -1) return; @@ -322,21 +316,46 @@ void EscDriverEsp32::transmitAll() void EscDriverEsp32::readTelemetry() { PIN_DEBUG(HIGH); + uint32_t bit_len = getAnalogPulse(2000); for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { - if (!_digital || !_dshot_tlm) continue; - if (!_channel[i].attached()) continue; - //_channel[i].telemetryValue = _rmt_rx_get_mem_len_in_isr((rmt_channel_t)i); + if(!_digital || !_dshot_tlm) continue; + if(!_channel[i].attached()) continue; + + HardwareSerial* s = (i == 0 && _timer.check()) ? &Serial : nullptr; RingbufHandle_t rb = NULL; - rmt_get_ringbuf_handle((rmt_channel_t)i, &rb); + if(ESP_OK != rmt_get_ringbuf_handle((rmt_channel_t)i, &rb)) continue; size_t rmt_len = 0; - rmt_item32_t * data = (rmt_item32_t *)xRingbufferReceive(rb, &rmt_len, 0/*portMAX_DELAY*/); - if (data) { - // process data here + rmt_item32_t* data = (rmt_item32_t*)xRingbufferReceive(rb, &rmt_len, 0); + if(s) { s->print((uint8_t)rmt_len); } + if (data) + { + for(size_t j = 0; j < rmt_len >> 2; j++) + { + if(!data[j].duration0) break; + if(s) s->print(' '); + if(s) s->print(data[j].level0); + if(s) s->print(':'); + if(s) s->print(data[j].duration0); + + if(!data[j].duration1) break; + if(s) s->print(' '); + if(s) s->print(data[j].level1); + if(s) s->print(':'); + if(s) s->print(data[j].duration1); + } + uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, bit_len); + + if(s) s->print(' '); + if(s) s->print(value, HEX); + vRingbufferReturnItem(rb, (void *)data); + + _channel[i].telemetryValue = value; } + if(s) s->print('\n'); } PIN_DEBUG(LOW); } diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 5564a1dd..231561ca 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -5,6 +5,7 @@ #include "EscDriver.h" #include +#include "Timer.h" class EscDriverEsp32: public EscDriverBase { @@ -73,7 +74,7 @@ class EscDriverEsp32: public EscDriverBase uint16_t dshot_t0l; uint16_t dshot_t1h; uint16_t dshot_t1l; - int telemetryValue; + uint32_t telemetryValue; }; EscDriverEsp32(); @@ -83,6 +84,7 @@ class EscDriverEsp32: public EscDriverBase int write(size_t channel, int pulse); void apply(); int pin(size_t channel) const; + uint32_t telemetry(size_t channel) const; private: void initChannel(int channel, gpio_num_t pin, int pulse); @@ -118,6 +120,7 @@ class EscDriverEsp32: public EscDriverBase static bool _tx_end_installed; static EscDriverEsp32* instances[]; + Espfc::Timer _timer; }; #endif diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 1fe2ef78..c7ee698b 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -172,6 +172,7 @@ class Model bool armingDisabled() const /* IRAM_ATTR */ { + return false; return state.armingDisabledFlags != 0; } diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index b486275e..fc3d9df5 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -219,6 +219,17 @@ struct ModelState int16_t outputDisarmed[OUTPUT_CHANNELS]; bool outputSaturated; + int16_t outputTelemetryErrors[OUTPUT_CHANNELS]; + float outputTelemetryRpm[OUTPUT_CHANNELS]; + float outputTelemetryFreq[OUTPUT_CHANNELS]; + int8_t outputTelemetryTemperature[OUTPUT_CHANNELS]; + int8_t outputTelemetryVoltage[OUTPUT_CHANNELS]; + int8_t outputTelemetryCurrent[OUTPUT_CHANNELS]; + int8_t outputTelemetryDebug1[OUTPUT_CHANNELS]; + int8_t outputTelemetryDebug2[OUTPUT_CHANNELS]; + int8_t outputTelemetryDebug3[OUTPUT_CHANNELS]; + int8_t outputTelemetryEvents[OUTPUT_CHANNELS]; + // other state Kalman kalman[AXES]; VectorFloat accelPrev; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index 7dba113d..ad848779 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1268,6 +1268,35 @@ class MspProcessor } break; + case MSP_MOTOR_TELEMETRY: + r.writeU8(OUTPUT_CHANNELS); + for (size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + int rpm = 0; + uint16_t invalidPct = 0; + uint8_t escTemperature = 0; // degrees celcius + uint16_t escVoltage = 0; // 0.01V per unit + uint16_t escCurrent = 0; // 0.01A per unit + uint16_t escConsumption = 0; // mAh + + if (_model.config.pin[i + PIN_OUTPUT_0] != -1) + { + rpm = lrintf(_model.state.outputTelemetryRpm[i]); + invalidPct = _model.state.outputTelemetryErrors[i]; + escTemperature = _model.state.outputTelemetryTemperature[i]; + escVoltage = _model.state.outputTelemetryVoltage[i]; + escCurrent = _model.state.outputTelemetryCurrent[i]; + } + + r.writeU32(rpm); + r.writeU16(invalidPct); + r.writeU8(escTemperature); + r.writeU16(escVoltage); + r.writeU16(escCurrent); + r.writeU16(escConsumption); + } + break; + case MSP_SET_MOTOR: for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 66115013..70589334 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -6,6 +6,9 @@ #include "EscDriver.h" #include "platform.h" +#define ERPM_PER_LSB 100.0f +#define SECONDS_PER_MINUTE 60 + namespace Espfc { namespace Output { @@ -40,6 +43,8 @@ class Mixer escServo.begin(servoConf); _model.state.escServo = _servo = &escServo; _model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); + + _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); } for(size_t i = 0; i < OUTPUT_CHANNELS; ++i) @@ -270,8 +275,71 @@ class Mixer if(_motor) _motor->write(i, _model.state.outputUs[i]); } } + if(_motor) _motor->apply(); if(_servo) _servo->apply(); + + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + const OutputChannelConfig& och = _model.config.output.channel[i]; + if(och.servo || !_motor || !_model.config.output.dshotTelemetry) continue; + uint32_t value = _motor->telemetry(i); + value = EscDriver::gcrToRawValue(value); + // Decode Extended DSHOT telemetry + switch (value & 0x0f00) + { + case 0x0200: + // Temperature range (in degree Celsius, just like Blheli_32 and KISS) + _model.state.outputTelemetryTemperature[i] = value & 0x00ff; + break; + case 0x0400: + // Voltage range (0-63,75V step 0,25V) + _model.state.outputTelemetryVoltage[i] = value & 0x00ff; + break; + case 0x0600: + // Current range (0-255A step 1A) + _model.state.outputTelemetryCurrent[i] = value & 0x00ff; + break; + case 0x0800: + // Debug 1 value + _model.state.outputTelemetryDebug1[i] = value & 0x00ff; + break; + case 0x0A00: + // Debug 2 value + _model.state.outputTelemetryDebug2[i] = value & 0x00ff; + break; + case 0x0C00: + // Debug 3 value + _model.state.outputTelemetryDebug3[i] = value & 0x00ff; + break; + case 0x0E00: + // State / Events + _model.state.outputTelemetryEvents[i] = value & 0x00ff; + break; + default: + value = EscDriver::convertToValue(value); + uint32_t erpm = EscDriver::convertToErpm(value); + if(_model.config.debugMode == DEBUG_DSHOT_RPM_TELEMETRY) + { + _model.state.debug[i] = erpm; + } + _model.state.outputTelemetryRpm[i] = erpmToRpm(erpm); + _model.state.outputTelemetryFreq[i] = erpmToHz(erpm); + break; + } + } + } + + float inline erpmToHz(float erpm) + { + // rpm = (erpm * ERPM_PER_LSB) / (motorConfig()->motorPoleCount / 2) + // _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); + return _erpmToHz * erpm; + } + + float inline erpmToRpm(float erpm) + { + return erpmToHz(erpm) * SECONDS_PER_MINUTE; } bool _stop(void) @@ -287,6 +355,7 @@ class Mixer EscDriver escMotor; EscDriver escServo; + float _erpmToHz; }; } diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp new file mode 100644 index 00000000..100b7287 --- /dev/null +++ b/test/test_esc/test_esc.cpp @@ -0,0 +1,51 @@ +#include +#include +#include "EscDriver.h" + +void test_esc_dshot_encode() +{ + TEST_ASSERT_EQUAL_UINT16( 0, EscDriver::dshotEncode( 0, false)); + TEST_ASSERT_EQUAL_UINT16( 34, EscDriver::dshotEncode( 1, false)); + TEST_ASSERT_EQUAL_UINT16( 68, EscDriver::dshotEncode( 2, false)); + TEST_ASSERT_EQUAL_UINT16( 1515, EscDriver::dshotEncode( 47, false)); + TEST_ASSERT_EQUAL_UINT16(32010, EscDriver::dshotEncode(1000, false)); + TEST_ASSERT_EQUAL_UINT16(32040, EscDriver::dshotEncode(1001, false)); + TEST_ASSERT_EQUAL_UINT16(32742, EscDriver::dshotEncode(1023, false)); + TEST_ASSERT_EQUAL_UINT16(32776, EscDriver::dshotEncode(1024, false)); + TEST_ASSERT_EQUAL_UINT16(65518, EscDriver::dshotEncode(2047, false)); +} + +void test_esc_dshot_encode_inverted() +{ + TEST_ASSERT_EQUAL_UINT16( 15, EscDriver::dshotEncode(0, true)); + TEST_ASSERT_EQUAL_UINT16( 45, EscDriver::dshotEncode(1, true)); + TEST_ASSERT_EQUAL_UINT16( 75, EscDriver::dshotEncode(2, true)); + TEST_ASSERT_EQUAL_UINT16( 1508, EscDriver::dshotEncode(47, true)); + TEST_ASSERT_EQUAL_UINT16(32005, EscDriver::dshotEncode(1000, true)); + TEST_ASSERT_EQUAL_UINT16(32039, EscDriver::dshotEncode(1001, true)); + TEST_ASSERT_EQUAL_UINT16(32745, EscDriver::dshotEncode(1023, true)); + TEST_ASSERT_EQUAL_UINT16(32775, EscDriver::dshotEncode(1024, true)); + TEST_ASSERT_EQUAL_UINT16(65505, EscDriver::dshotEncode(2047, true)); +} + +void test_esc_dshot_convert() +{ + TEST_ASSERT_EQUAL_UINT16( 0, EscDriver::dshotConvert( 0)); + TEST_ASSERT_EQUAL_UINT16( 0, EscDriver::dshotConvert( 1)); + TEST_ASSERT_EQUAL_UINT16( 0, EscDriver::dshotConvert( 999)); + TEST_ASSERT_EQUAL_UINT16( 0, EscDriver::dshotConvert(1000)); + TEST_ASSERT_EQUAL_UINT16( 49, EscDriver::dshotConvert(1001)); + TEST_ASSERT_EQUAL_UINT16(1047, EscDriver::dshotConvert(1500)); + TEST_ASSERT_EQUAL_UINT16(2047, EscDriver::dshotConvert(2000)); +} + +int main(int argc, char **argv) +{ + UNITY_BEGIN(); + RUN_TEST(test_esc_dshot_encode); + RUN_TEST(test_esc_dshot_encode_inverted); + RUN_TEST(test_esc_dshot_convert); + UNITY_END(); + + return 0; +} From e8d3e85825700b9b3de0744c261ca53c79ea4488 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 19 Feb 2024 12:39:17 +0100 Subject: [PATCH 21/28] fix other targets + add tests --- lib/EscDriver/src/EscDriver.h | 21 +++-- lib/EscDriver/src/EscDriverEsp32.cpp | 20 ++--- lib/EscDriver/src/EscDriverEsp32c3.cpp | 5 ++ lib/EscDriver/src/EscDriverEsp32c3.h | 1 + lib/EscDriver/src/EscDriverEsp8266.cpp | 5 ++ lib/EscDriver/src/EscDriverEsp8266.h | 1 + lib/EscDriver/src/EscDriverRP2040.cpp | 5 ++ lib/EscDriver/src/EscDriverRP2040.h | 1 + lib/Espfc/src/Model.h | 2 +- lib/Espfc/src/Output/Mixer.h | 2 +- test/test_esc/test_esc.cpp | 118 +++++++++++++++++++++++++ test/test_msp/test_msp.cpp | 1 + 12 files changed, 163 insertions(+), 19 deletions(-) diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 93a22d6b..9afc74d1 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -97,26 +97,31 @@ class EscDriverBase { uint32_t duration0 = data[i] & 0x7fff; if(!duration0) break; - uint32_t level0 = (data[i] >> 15) & 0x01; uint32_t len0 = durationToBitLen(duration0, bitLen); - value = pushBits(value, level0, len0); - bitCount += len0; + if(len0) + { + value = pushBits(value, level0, len0); + bitCount += len0; + } uint32_t duration1 = (data[i] >> 16) & 0x7fff; if(!duration1) break; uint32_t level1 = (data[i] >> 31) & 0x01; uint32_t len1 = durationToBitLen(duration1, bitLen); - value = pushBits(value, level1, len1); - bitCount += len1; + if(len1) + { + value = pushBits(value, level1, len1); + bitCount += len1; + } } // fill missing bits with 1 if(bitCount < 21) { - value = pushBits(value, 1, 21 - bitCount); + value = pushBits(value, 0x1, 21 - bitCount); } - value = value ^ (value >> 1); // extract gcr + //value = value ^ (value >> 1); // extract gcr return value; } @@ -146,6 +151,8 @@ class EscDriverBase static uint32_t gcrToRawValue(uint32_t value) { + value = value ^ (value >> 1); // extract gcr + constexpr uint32_t iv = 0xffffffff; // invalid code // First bit is start bit so discard it. value &= 0xfffff; diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 7d67b20b..074f3456 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -315,24 +315,24 @@ void EscDriverEsp32::transmitAll() void EscDriverEsp32::readTelemetry() { - PIN_DEBUG(HIGH); - uint32_t bit_len = getAnalogPulse(2000); + //PIN_DEBUG(HIGH); + uint32_t bit_len = getAnalogPulse(2136); for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { if(!_digital || !_dshot_tlm) continue; if(!_channel[i].attached()) continue; - HardwareSerial* s = (i == 0 && _timer.check()) ? &Serial : nullptr; + //HardwareSerial* s = (i == 0 && _timer.check()) ? &Serial : nullptr; RingbufHandle_t rb = NULL; if(ESP_OK != rmt_get_ringbuf_handle((rmt_channel_t)i, &rb)) continue; size_t rmt_len = 0; rmt_item32_t* data = (rmt_item32_t*)xRingbufferReceive(rb, &rmt_len, 0); - if(s) { s->print((uint8_t)rmt_len); } + //if(s) { s->print((uint8_t)rmt_len); } if (data) { - for(size_t j = 0; j < rmt_len >> 2; j++) + /*for(size_t j = 0; j < rmt_len >> 2; j++) { if(!data[j].duration0) break; if(s) s->print(' '); @@ -345,19 +345,19 @@ void EscDriverEsp32::readTelemetry() if(s) s->print(data[j].level1); if(s) s->print(':'); if(s) s->print(data[j].duration1); - } + }*/ uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, bit_len); - if(s) s->print(' '); - if(s) s->print(value, HEX); + //if(s) s->print(' '); + //if(s) s->print(value, HEX); vRingbufferReturnItem(rb, (void *)data); _channel[i].telemetryValue = value; } - if(s) s->print('\n'); + //if(s) s->print('\n'); } - PIN_DEBUG(LOW); + //PIN_DEBUG(LOW); } void EscDriverEsp32::writeAnalogCommand(uint8_t channel, int32_t pulse) diff --git a/lib/EscDriver/src/EscDriverEsp32c3.cpp b/lib/EscDriver/src/EscDriverEsp32c3.cpp index aa4f1121..351c523a 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.cpp +++ b/lib/EscDriver/src/EscDriverEsp32c3.cpp @@ -88,6 +88,11 @@ int EscDriverEsp32c3::pin(size_t channel) const return _slots[channel].pin; } +uint32_t EscDriverEsp32c3::telemetry(size_t channel) const +{ + return 0; +} + void EscDriverEsp32c3::apply() { if(_protocol == ESC_PROTOCOL_DISABLED) return; diff --git a/lib/EscDriver/src/EscDriverEsp32c3.h b/lib/EscDriver/src/EscDriverEsp32c3.h index faa2a0ba..93134dd2 100644 --- a/lib/EscDriver/src/EscDriverEsp32c3.h +++ b/lib/EscDriver/src/EscDriverEsp32c3.h @@ -49,6 +49,7 @@ class EscDriverEsp32c3: public EscDriverBase int write(size_t channel, int pulse) IRAM_ATTR; void apply() IRAM_ATTR; int pin(size_t channel) const; + uint32_t telemetry(size_t channel) const; static bool handle(void * p) IRAM_ATTR; private: diff --git a/lib/EscDriver/src/EscDriverEsp8266.cpp b/lib/EscDriver/src/EscDriverEsp8266.cpp index af592940..5a00d01b 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.cpp +++ b/lib/EscDriver/src/EscDriverEsp8266.cpp @@ -168,6 +168,11 @@ int EscDriverEsp8266::pin(size_t channel) const return _slots[channel].pin; } +uint32_t EscDriverEsp8266::telemetry(size_t channel) const +{ + return 0; +} + int EscDriverEsp8266::write(size_t channel, int pulse) { if(channel < 0 || channel >= ESC_CHANNEL_COUNT) return 0; diff --git a/lib/EscDriver/src/EscDriverEsp8266.h b/lib/EscDriver/src/EscDriverEsp8266.h index 66e9cb20..8e0fd58b 100644 --- a/lib/EscDriver/src/EscDriverEsp8266.h +++ b/lib/EscDriver/src/EscDriverEsp8266.h @@ -56,6 +56,7 @@ class EscDriverEsp8266: public EscDriverBase int write(size_t channel, int pulse) IRAM_ATTR; void apply() IRAM_ATTR; int pin(size_t channel) const; + uint32_t telemetry(size_t channel) const; static void handle(void * p, void * x) IRAM_ATTR; private: diff --git a/lib/EscDriver/src/EscDriverRP2040.cpp b/lib/EscDriver/src/EscDriverRP2040.cpp index b31699a3..2db789e8 100644 --- a/lib/EscDriver/src/EscDriverRP2040.cpp +++ b/lib/EscDriver/src/EscDriverRP2040.cpp @@ -83,6 +83,11 @@ int EscDriverRP2040::pin(size_t channel) const return _slots[channel].pin; } +uint32_t EscDriverRP2040::telemetry(size_t channel) const +{ + return 0; +} + bool EscDriverRP2040::isSliceDriven(int slice) { for(size_t i = 0; i < ESC_CHANNEL_COUNT; ++i) { diff --git a/lib/EscDriver/src/EscDriverRP2040.h b/lib/EscDriver/src/EscDriverRP2040.h index ac495f5d..75abfef3 100644 --- a/lib/EscDriver/src/EscDriverRP2040.h +++ b/lib/EscDriver/src/EscDriverRP2040.h @@ -43,6 +43,7 @@ class EscDriverRP2040: public EscDriverBase int attach(size_t channel, int pin, int pulse) IRAM_ATTR; int write(size_t channel, int pulse) IRAM_ATTR; int pin(size_t channel) const; + uint32_t telemetry(size_t channel) const; void apply() IRAM_ATTR; private: diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index c7ee698b..98c38059 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -172,7 +172,7 @@ class Model bool armingDisabled() const /* IRAM_ATTR */ { - return false; + //return false; return state.armingDisabledFlags != 0; } diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 70589334..50c02f4f 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -332,7 +332,7 @@ class Mixer float inline erpmToHz(float erpm) { - // rpm = (erpm * ERPM_PER_LSB) / (motorConfig()->motorPoleCount / 2) + // rpm = (erpm * ERPM_PER_LSB) / (_model.config.output.motorPoles / 2) // _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); return _erpmToHz * erpm; } diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp index 100b7287..087afaa3 100644 --- a/test/test_esc/test_esc.cpp +++ b/test/test_esc/test_esc.cpp @@ -39,12 +39,130 @@ void test_esc_dshot_convert() TEST_ASSERT_EQUAL_UINT16(2047, EscDriver::dshotConvert(2000)); } +void test_esc_gcr_to_raw_value() +{ + TEST_ASSERT_EQUAL_UINT32(1942, EscDriver::gcrToRawValue(0b011010011101101100101)); + TEST_ASSERT_EQUAL_UINT32(1944, EscDriver::gcrToRawValue(0b011010011101001110001)); + TEST_ASSERT_EQUAL_UINT32(1947, EscDriver::gcrToRawValue(0b011010011100110110011)); + TEST_ASSERT_EQUAL_UINT32(1948, EscDriver::gcrToRawValue(0b011010011101010001001)); + TEST_ASSERT_EQUAL_UINT32(1949, EscDriver::gcrToRawValue(0b011010011100100101011)); + TEST_ASSERT_EQUAL_UINT32(1950, EscDriver::gcrToRawValue(0b011010011100101110101)); +} + +void test_esc_gcr_convert_to_value() +{ + TEST_ASSERT_EQUAL_UINT32(3248, EscDriver::convertToValue(1942)); + TEST_ASSERT_EQUAL_UINT32(3264, EscDriver::convertToValue(1944)); + TEST_ASSERT_EQUAL_UINT32(3288, EscDriver::convertToValue(1947)); + TEST_ASSERT_EQUAL_UINT32(3296, EscDriver::convertToValue(1948)); + TEST_ASSERT_EQUAL_UINT32(3304, EscDriver::convertToValue(1949)); + TEST_ASSERT_EQUAL_UINT32(3312, EscDriver::convertToValue(1950)); +} + +void test_esc_gcr_convert_to_erpm() +{ + TEST_ASSERT_EQUAL_UINT32(185, EscDriver::convertToErpm(EscDriver::convertToValue(1942))); + TEST_ASSERT_EQUAL_UINT32(184, EscDriver::convertToErpm(EscDriver::convertToValue(1944))); + TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1947))); + TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1948))); + TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1949))); + TEST_ASSERT_EQUAL_UINT32(181, EscDriver::convertToErpm(EscDriver::convertToValue(1950))); +} + +void test_esc_duration_to_bitlen() +{ + uint32_t bit_len = 178; + TEST_ASSERT_EQUAL_UINT32(0, EscDriver::durationToBitLen(80, bit_len)); + + TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(150, bit_len)); + TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(160, bit_len)); + TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(177, bit_len)); + + TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(300, bit_len)); + TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(373, bit_len)); + TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(383, bit_len)); + + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(450, bit_len)); + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(498, bit_len)); + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(595, bit_len)); + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(607, bit_len)); + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(610, bit_len)); + + TEST_ASSERT_EQUAL_UINT32(4, EscDriver::durationToBitLen(664, bit_len)); +} + +constexpr uint32_t make_item(uint32_t duration0, uint32_t level0, uint32_t duration1, uint32_t level1) +{ + return (duration0 & 0x07fff) | (level0 & 0x1) << 15 | (duration1 & 0x07fff) << 16 | (level1 & 0x1) << 31; +} + +void test_esc_extract_telemetry_gcr_real() +{ + // 0b 0 11 0 1 00 111 0 11 0 11 00 1 0 1 + // 24 0:100 1:200 0:100 1:100 0:200 1:300 0:100 1:200 0:100 1:200 0:200 1:100 0:100 + uint32_t data[] = { + make_item(100, 0, 200, 1), make_item(100, 0, 100, 1), make_item(200, 0, 300, 1), + make_item(100, 0, 200, 1), make_item(100, 0, 200, 1), make_item(200, 0, 100, 1), + make_item(100, 0, 0, 0), + }; + uint32_t bit_len = 100; + uint32_t data_len = sizeof(data); + TEST_ASSERT_EQUAL_UINT32(100, bit_len); + TEST_ASSERT_EQUAL_UINT32(28, data_len); + uint32_t exp = 0b011010011101101100101; + TEST_ASSERT_EQUAL_HEX32(0xD3B65, exp); + TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); +} + + +void test_esc_extract_telemetry_gcr1() +{ + // 24 0:373 1:605 0:163 1:605 0:174 1:376 0:379 1:599 0:177 1:163 0:173 1EF67A 1EF67A? + // 00 111 0 111 0 11 00 111 0 1 0 + uint32_t data[] = { + make_item(373, 0, 605, 1), make_item(163, 0, 605, 1), make_item(174, 0, 376, 1), + make_item(379, 0, 599, 1), make_item(177, 0, 163, 1), make_item(173, 0, 0, 1), + }; + uint32_t bit_len = 2136 / (1000 / 80); + uint32_t data_len = sizeof(data); + TEST_ASSERT_EQUAL_UINT32(178, bit_len); + TEST_ASSERT_EQUAL_UINT32(24, data_len); + uint32_t exp = 0b001110111011001110101; // +1 + TEST_ASSERT_EQUAL_HEX32(0x77675, exp); + TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); +} + +void test_esc_extract_telemetry_gcr2() +{ + // 28 0:177 1:379 0:177 1:167 0:173 1:164 0:379 1:177 0:373 1:379 0:380 1:608 0:164 D499E D499E? + // 0 11 0 1 0 1 00 1 00 11 00 111 0 + uint32_t data[] = { + make_item(177, 0, 379, 1), make_item(177, 0, 167, 1), make_item(173, 0, 164, 1), + make_item(379, 0, 177, 1), make_item(373, 0, 379, 1), make_item(380, 0, 608, 1), + make_item(164, 0, 0, 0), + }; + uint32_t bit_len = 2136 / (1000 / 80); + uint32_t data_len = sizeof(data); + uint32_t exp = 0b011010100100110011101; // +1 + TEST_ASSERT_EQUAL_UINT32(178, bit_len); + TEST_ASSERT_EQUAL_UINT32(28, data_len); + TEST_ASSERT_EQUAL_HEX32(0xD499D, exp); + TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); +} + int main(int argc, char **argv) { UNITY_BEGIN(); RUN_TEST(test_esc_dshot_encode); RUN_TEST(test_esc_dshot_encode_inverted); RUN_TEST(test_esc_dshot_convert); + RUN_TEST(test_esc_gcr_to_raw_value); + RUN_TEST(test_esc_gcr_convert_to_value); + RUN_TEST(test_esc_gcr_convert_to_erpm); + RUN_TEST(test_esc_duration_to_bitlen); + RUN_TEST(test_esc_extract_telemetry_gcr_real); + RUN_TEST(test_esc_extract_telemetry_gcr1); + RUN_TEST(test_esc_extract_telemetry_gcr2); UNITY_END(); return 0; diff --git a/test/test_msp/test_msp.cpp b/test/test_msp/test_msp.cpp index 3cfb8d44..a2d49df9 100644 --- a/test/test_msp/test_msp.cpp +++ b/test/test_msp/test_msp.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "Msp/Msp.h" #include "Msp/MspParser.h" From 931bf7eab3e48156dd00ed19faedda0e6c42aa95 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 19 Feb 2024 22:06:20 +0100 Subject: [PATCH 22/28] erpm tune + blackbox + stats --- lib/EscDriver/src/EscDriver.h | 8 ++-- lib/EscDriver/src/EscDriverEsp32.cpp | 2 +- lib/Espfc/src/Blackbox.h | 7 ++++ lib/Espfc/src/ModelState.h | 3 ++ lib/Espfc/src/Output/Mixer.h | 59 ++++++++++++++++++++++++---- lib/Espfc/src/Stats.h | 2 + lib/betaflight/src/platform.h | 4 ++ 7 files changed, 73 insertions(+), 12 deletions(-) diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index 9afc74d1..f68765e0 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -130,7 +130,9 @@ class EscDriverBase static inline uint32_t convertToErpm(uint32_t value) { - if (!value || value == INVALID_TELEMETRY_VALUE) { + if(!value) return 0; + + if(!value || value == INVALID_TELEMETRY_VALUE) { return INVALID_TELEMETRY_VALUE; } @@ -141,7 +143,7 @@ class EscDriverBase static inline uint32_t convertToValue(uint32_t value) { // eRPM range - if (value == 0x0fff) { + if(value == 0x0fff) { return 0; } @@ -170,7 +172,7 @@ class EscDriverBase csum = csum ^ (csum >> 8); // xor bytes csum = csum ^ (csum >> 4); // xor nibbles - if ((csum & 0xf) != 0xf || decodedValue > 0xffff) { + if((csum & 0xf) != 0xf || decodedValue > 0xffff) { value = INVALID_TELEMETRY_VALUE; } else { value = decodedValue >> 4; diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 074f3456..4e0cbd59 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -165,7 +165,7 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) // add RX specifics int rx_ch = RMT_ENCODE_RX_CHANNEL(i); rmt_ll_rx_set_idle_thres(&RMT, rx_ch, getAnalogPulse(30000)); // max bit len, (30us) - rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 40); // min bit len, 80 ticks = 1us for div = 1, max value = 255 + rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 30); // min bit len, 80 ticks = 1us for div = 1, max value = 255 rmt_ll_rx_enable_filter(&RMT, rx_ch, 1); _rmt_zero_mem((rmt_channel_t)rx_ch, RMT_MEM_ITEM_NUM); diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index be91e6bc..d362aabc 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -169,6 +169,11 @@ bool areMotorsRunning(void) return _model_ptr->areMotorsRunning(); } +uint16_t getDshotTelemetry(uint8_t i) +{ + return _model_ptr->state.outputTelemetryErpm[i]; +} + namespace Espfc { class Blackbox @@ -285,6 +290,8 @@ class Blackbox motorConfigMutable()->digitalIdleOffsetValue = _model.config.output.dshotIdle; motorConfigMutable()->minthrottle = _model.state.minThrottle; motorConfigMutable()->maxthrottle = _model.state.maxThrottle; + motorConfigMutable()->dev.useDshotTelemetry = _model.config.output.dshotTelemetry; + motorConfigMutable()->motorPoleCount = _model.config.output.motorPoles; gyroConfigMutable()->gyro_sync_denom = 1; pidConfigMutable()->pid_process_denom = _model.config.loopSync; diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index fc3d9df5..fd776fca 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -220,6 +220,9 @@ struct ModelState bool outputSaturated; int16_t outputTelemetryErrors[OUTPUT_CHANNELS]; + int32_t outputTelemetryErrorsSum[OUTPUT_CHANNELS]; + int32_t outputTelemetryErrorsCount[OUTPUT_CHANNELS]; + uint32_t outputTelemetryErpm[OUTPUT_CHANNELS]; float outputTelemetryRpm[OUTPUT_CHANNELS]; float outputTelemetryFreq[OUTPUT_CHANNELS]; int8_t outputTelemetryTemperature[OUTPUT_CHANNELS]; diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 50c02f4f..5300eb12 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -43,9 +43,10 @@ class Mixer escServo.begin(servoConf); _model.state.escServo = _servo = &escServo; _model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); - - _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); } + _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); + _statsCounterMax = _model.state.mixerTimer.rate / 2; + _statsCounter = 0; for(size_t i = 0; i < OUTPUT_CHANNELS; ++i) { @@ -63,6 +64,15 @@ class Mixer _motor->attach(i, _model.config.pin[PIN_OUTPUT_0 + i], 1000); _model.logger.info().log(F("MOTOR PIN")).log(i).logln(_model.config.pin[PIN_OUTPUT_0 + i]); } + _model.state.outputTelemetryErrors[i] = 0; + _model.state.outputTelemetryErrorsSum[i] = 0; + _model.state.outputTelemetryErrorsCount[i] = 0; + _model.state.outputTelemetryTemperature[i] = 0; + _model.state.outputTelemetryCurrent[i] = 0; + _model.state.outputTelemetryVoltage[i] = 0; + _model.state.outputTelemetryErpm[i] = 0; + _model.state.outputTelemetryRpm[i] = 0; + _model.state.outputTelemetryFreq[i] = 0; } motorInitEscDevice(_motor); @@ -85,6 +95,7 @@ class Mixer float outputs[OUTPUT_CHANNELS]; const MixerConfig& mixer = _model.state.currentMixer; + readTelemetry(); updateMixer(mixer, outputs); writeOutput(mixer, outputs); @@ -258,11 +269,7 @@ class Mixer } } } - applyOutput(); - } - void applyOutput() - { for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { const OutputChannelConfig& och = _model.config.output.channel[i]; @@ -278,13 +285,28 @@ class Mixer if(_motor) _motor->apply(); if(_servo) _servo->apply(); + } + + void readTelemetry() + { + Stats::Measure mixerMeasure(_model.state.stats, COUNTER_MIXER_READ); + if(!_model.config.output.dshotTelemetry || !_motor) return; for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { const OutputChannelConfig& och = _model.config.output.channel[i]; - if(och.servo || !_motor || !_model.config.output.dshotTelemetry) continue; + if(och.servo) continue; uint32_t value = _motor->telemetry(i); value = EscDriver::gcrToRawValue(value); + + _model.state.outputTelemetryErrorsCount[i]++; + + if(value == EscDriver::INVALID_TELEMETRY_VALUE) + { + _model.state.outputTelemetryErrorsSum[i]++; + continue; + } + // Decode Extended DSHOT telemetry switch (value & 0x0f00) { @@ -318,7 +340,7 @@ class Mixer break; default: value = EscDriver::convertToValue(value); - uint32_t erpm = EscDriver::convertToErpm(value); + uint32_t erpm = _model.state.outputTelemetryErpm[i] = EscDriver::convertToErpm(value); if(_model.config.debugMode == DEBUG_DSHOT_RPM_TELEMETRY) { _model.state.debug[i] = erpm; @@ -328,6 +350,25 @@ class Mixer break; } } + + _statsCounter++; + if(_statsCounter >= _statsCounterMax) + { + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + if(_model.state.outputTelemetryErrorsCount[i]) + { + _model.state.outputTelemetryErrors[i] = _model.state.outputTelemetryErrorsSum[i] * 10000 / _model.state.outputTelemetryErrorsCount[i]; + if(_model.config.debugMode == DEBUG_DSHOT_RPM_ERRORS) + { + _model.state.debug[i] = _model.state.outputTelemetryErrors[i]; + } + } + _model.state.outputTelemetryErrorsCount[i] = 0; + _model.state.outputTelemetryErrorsSum[i] = 0; + } + _statsCounter = 0; + } } float inline erpmToHz(float erpm) @@ -355,6 +396,8 @@ class Mixer EscDriver escMotor; EscDriver escServo; + uint32_t _statsCounter; + uint32_t _statsCounterMax; float _erpmToHz; }; diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 21a5bc97..3405f6d3 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -25,6 +25,7 @@ enum StatCounter : int8_t { COUNTER_INNER_PID, COUNTER_MIXER, COUNTER_MIXER_WRITE, + COUNTER_MIXER_READ, COUNTER_BLACKBOX, COUNTER_TELEMETRY, COUNTER_SERIAL, @@ -183,6 +184,7 @@ class Stats case COUNTER_INNER_PID: return PSTR(" pid_i"); case COUNTER_MIXER: return PSTR("mixer_p"); case COUNTER_MIXER_WRITE: return PSTR("mixer_w"); + case COUNTER_MIXER_READ: return PSTR("mixer_r"); case COUNTER_BLACKBOX: return PSTR(" bblog"); case COUNTER_TELEMETRY: return PSTR(" tlm"); case COUNTER_SERIAL: return PSTR(" serial"); diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index b48c04ce..0065b1be 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -8,6 +8,7 @@ #define USE_D_MIN #define USE_DYN_NOTCH_FILTER #define USE_ITERM_RELAX +#define USE_DSHOT_TELEMETRY #include #include @@ -388,6 +389,7 @@ typedef struct motorDevConfig_s { uint8_t motorPwmProtocol; // Pwm Protocol uint8_t motorPwmInversion; // Active-High vs Active-Low. Useful for brushed FCs converted for brushless operation uint8_t useUnsyncedPwm; + uint8_t useDshotTelemetry; // ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; } motorDevConfig_t; @@ -397,6 +399,7 @@ typedef struct motorConfig_s { uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs + uint8_t motorPoleCount; } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); @@ -952,6 +955,7 @@ float mixerGetThrottle(void); bool isRssiConfigured(void); int16_t getMotorOutputLow(); int16_t getMotorOutputHigh(); +uint16_t getDshotTelemetry(uint8_t i); /* FAILSAFE END */ #define PARAM_NAME_GYRO_HARDWARE_LPF "gyro_hardware_lpf" From 9e68c06fbc4e48ac86f38f2aa5d034098fec9e17 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Wed, 21 Feb 2024 23:44:17 +0100 Subject: [PATCH 23/28] tune dshot telemetry + more tests --- docs/dshot_gcr_dump.txt | 4 +- lib/EscDriver/src/EscDriver.h | 24 +++-- lib/EscDriver/src/EscDriverEsp32.cpp | 67 ++++++------ lib/EscDriver/src/EscDriverEsp32.h | 4 +- lib/Espfc/src/Output/Mixer.h | 9 +- test/test_esc/test_esc.cpp | 152 ++++++++++++++++++++++----- 6 files changed, 178 insertions(+), 82 deletions(-) diff --git a/docs/dshot_gcr_dump.txt b/docs/dshot_gcr_dump.txt index 10c04dfb..4ca7f050 100644 --- a/docs/dshot_gcr_dump.txt +++ b/docs/dshot_gcr_dump.txt @@ -1,6 +1,6 @@ // bluejay esc, dshot 300 -// power off +// power on 32 0:375 1:168 0:10368 0:14 0:381 1:162 0:172 1:172 0:384 1:159 0:178 1:168 0:166 1:168 0:603 2950 2950 32 0:381 1:165 0:172 1:172 0:377 1:176 0:165 1:171 0:381 1:169 0:168 1:172 0:175 1:172 0:593 52950 52950 32 0:381 1:172 0:171 1:165 0:385 1:168 0:168 1:169 0:381 1:169 0:178 1:162 0:175 1:171 0:607 52950 52950 @@ -11,7 +11,7 @@ 32 0:379 1:167 0:170 1:174 0:376 1:176 0:164 1:173 0:380 1:170 0:167 1:173 0:173 1:174 0:592 52950 52950 32 0:380 1:173 0:177 1:170 0:382 1:174 0:173 1:163 0:380 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950 -// power on +// running 24 0:373 1:605 0:163 1:605 0:174 1:376 0:379 1:599 0:177 1:163 0:173 1EF67A 1EF67A 24 0:379 1:599 0:170 1:654 0:170 1:170 0:184 1:605 0:173 1:379 0:380 1EF5EC 1EF5EC 24 0:383 1:605 0:163 1:602 0:177 1:163 0:383 1:170 0:170 1:167 0:605 F7A50 F7A50 diff --git a/lib/EscDriver/src/EscDriver.h b/lib/EscDriver/src/EscDriver.h index f68765e0..5e0673ee 100644 --- a/lib/EscDriver/src/EscDriver.h +++ b/lib/EscDriver/src/EscDriver.h @@ -121,18 +121,25 @@ class EscDriverBase { value = pushBits(value, 0x1, 21 - bitCount); } - //value = value ^ (value >> 1); // extract gcr return value; } - static const uint32_t INVALID_TELEMETRY_VALUE = 0xffff; + static constexpr uint32_t INVALID_TELEMETRY_VALUE = 0xffff; + static constexpr int SECONDS_PER_MINUTE = 60; + static constexpr float ERPM_PER_LSB = 100.0f; + + static inline float getErpmToHzRatio(int poles) + { + return ERPM_PER_LSB / SECONDS_PER_MINUTE / (poles / 2.0f); + } static inline uint32_t convertToErpm(uint32_t value) { if(!value) return 0; - if(!value || value == INVALID_TELEMETRY_VALUE) { + if(!value || value == INVALID_TELEMETRY_VALUE) + { return INVALID_TELEMETRY_VALUE; } @@ -143,7 +150,8 @@ class EscDriverBase static inline uint32_t convertToValue(uint32_t value) { // eRPM range - if(value == 0x0fff) { + if(value == 0x0fff) + { return 0; } @@ -172,11 +180,11 @@ class EscDriverBase csum = csum ^ (csum >> 8); // xor bytes csum = csum ^ (csum >> 4); // xor nibbles - if((csum & 0xf) != 0xf || decodedValue > 0xffff) { - value = INVALID_TELEMETRY_VALUE; - } else { - value = decodedValue >> 4; + if((csum & 0xf) != 0xf || decodedValue > 0xffff) + { + return INVALID_TELEMETRY_VALUE; } + value = decodedValue >> 4; return value; } diff --git a/lib/EscDriver/src/EscDriverEsp32.cpp b/lib/EscDriver/src/EscDriverEsp32.cpp index 4e0cbd59..b1f237b6 100644 --- a/lib/EscDriver/src/EscDriverEsp32.cpp +++ b/lib/EscDriver/src/EscDriverEsp32.cpp @@ -37,7 +37,8 @@ IRAM_ATTR static esp_err_t _rmt_fill_tx_items(rmt_channel_t channel, const rmt_i } IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) { - if(tx_idx_rst) { + if(tx_idx_rst) + { rmt_ll_tx_reset_pointer(&RMT, channel); // BC } rmt_ll_rx_set_mem_owner(&RMT, channel, RMT_MEM_OWNER_TX); // moved to _rmt_fill_tx_items @@ -53,7 +54,8 @@ IRAM_ATTR static esp_err_t _rmt_tx_start(rmt_channel_t channel, bool tx_idx_rst) static void IRAM_ATTR _rmt_zero_mem(rmt_channel_t channel, size_t len) { volatile rmt_item32_t *data = (rmt_item32_t *)RMTMEM.chan[channel].data32; - for (size_t idx = 0; idx < len; idx++) { + for (size_t idx = 0; idx < len; idx++) + { data[idx].val = 0; } } @@ -140,7 +142,7 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) _channel[i].divider = getClockDivider(); _channel[i].pulse_min = getPulseMin(); _channel[i].pulse_max = getPulseMax(); - _channel[i].pulse_space = getPulseInterval(); + _channel[i].pulse_space = nsToTicks(_interval * 1000); // specification 0:37%, 1:75% of 1670ns for dshot600 //_channel[i].dshot_t0h = getDshotPulse(625); @@ -149,10 +151,11 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) //_channel[i].dshot_t1l = getDshotPulse(420); // betaflight 0:35%, 1:70% of 1670ns for dshot600 - _channel[i].dshot_t0h = getDshotPulse(584 - 16); - _channel[i].dshot_t0l = getDshotPulse(1086 - 32); - _channel[i].dshot_t1h = getDshotPulse(1170 - 32); - _channel[i].dshot_t1l = getDshotPulse(500 - 16); + _channel[i].dshot_t0h = getDshotPulse(584); + _channel[i].dshot_t0l = getDshotPulse(1086); + _channel[i].dshot_t1h = getDshotPulse(1170); + _channel[i].dshot_t1l = getDshotPulse(500); + _channel[i].dshot_tlm_bit_len = (_channel[i].dshot_t0h + _channel[i].dshot_t0l) * 4 / 5; instances[i] = this; @@ -164,12 +167,12 @@ void EscDriverEsp32::initChannel(int i, gpio_num_t pin, int pulse) // add RX specifics int rx_ch = RMT_ENCODE_RX_CHANNEL(i); - rmt_ll_rx_set_idle_thres(&RMT, rx_ch, getAnalogPulse(30000)); // max bit len, (30us) - rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 30); // min bit len, 80 ticks = 1us for div = 1, max value = 255 - rmt_ll_rx_enable_filter(&RMT, rx_ch, 1); + rmt_ll_rx_set_idle_thres(&RMT, rx_ch, nsToTicks(30000)); // max bit len, (30us) + rmt_ll_rx_set_filter_thres(&RMT, rx_ch, 40); // min bit len, 80 ticks = 1us for div = 1, max value = 255 + rmt_ll_rx_enable_filter(&RMT, rx_ch, true); _rmt_zero_mem((rmt_channel_t)rx_ch, RMT_MEM_ITEM_NUM); -#if SOC_RMT_SUPPORT_TX_SYNCHRO +#if 0 && SOC_RMT_SUPPORT_TX_SYNCHRO // sync all channels if(!_async) { @@ -248,7 +251,7 @@ void EscDriverEsp32::enableRx(rmt_channel_t channel) rmt_ll_rx_reset_pointer(&RMT, rx_ch); rmt_ll_clear_rx_end_interrupt(&RMT, rx_ch); rmt_ll_enable_rx_end_interrupt(&RMT, rx_ch, true); - _rmt_zero_mem(rx_ch, 1); // clear first item of rx buffer to avoid reading tx items + _rmt_zero_mem(rx_ch, 2); // clear first item of rx buffer to avoid reading tx items rmt_ll_rx_enable(&RMT, rx_ch, true); } @@ -316,7 +319,6 @@ void EscDriverEsp32::transmitAll() void EscDriverEsp32::readTelemetry() { //PIN_DEBUG(HIGH); - uint32_t bit_len = getAnalogPulse(2136); for (size_t i = 0; i < ESC_CHANNEL_COUNT; i++) { if(!_digital || !_dshot_tlm) continue; @@ -346,7 +348,7 @@ void EscDriverEsp32::readTelemetry() if(s) s->print(':'); if(s) s->print(data[j].duration1); }*/ - uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, bit_len); + uint32_t value = extractTelemetryGcr((uint32_t*)data, rmt_len >> 2, _channel[i].dshot_tlm_bit_len); //if(s) s->print(' '); //if(s) s->print(value, HEX); @@ -426,7 +428,7 @@ uint32_t EscDriverEsp32::getClockDivider() const case ESC_PROTOCOL_PWM: return 24; case ESC_PROTOCOL_ONESHOT125: return 4; case ESC_PROTOCOL_ONESHOT42: return 2; - case ESC_PROTOCOL_MULTISHOT: return 2; + case ESC_PROTOCOL_MULTISHOT: return 1; case ESC_PROTOCOL_BRUSHED: return 8; default: return 1; } @@ -436,10 +438,10 @@ uint32_t EscDriverEsp32::getPulseMin() const { switch (_protocol) { - case ESC_PROTOCOL_PWM: return getAnalogPulse(1000 * 1000); - case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse(125 * 1000); - case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse(42 * 1000); - case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse(5 * 1000); + case ESC_PROTOCOL_PWM: return nsToTicks(1000 * 1000); + case ESC_PROTOCOL_ONESHOT125: return nsToTicks(125 * 1000); + case ESC_PROTOCOL_ONESHOT42: return nsToTicks(42 * 1000); + case ESC_PROTOCOL_MULTISHOT: return nsToTicks(5 * 1000); case ESC_PROTOCOL_BRUSHED: return 0; case ESC_PROTOCOL_DSHOT150: case ESC_PROTOCOL_DSHOT300: @@ -453,11 +455,11 @@ uint32_t EscDriverEsp32::getPulseMax() const { switch (_protocol) { - case ESC_PROTOCOL_PWM: return getAnalogPulse(2000 * 1000); - case ESC_PROTOCOL_ONESHOT125: return getAnalogPulse(250 * 1000); - case ESC_PROTOCOL_ONESHOT42: return getAnalogPulse(84 * 1000); - case ESC_PROTOCOL_MULTISHOT: return getAnalogPulse(25 * 1000); - case ESC_PROTOCOL_BRUSHED: return getAnalogPulse(_interval * 1000); + case ESC_PROTOCOL_PWM: return nsToTicks(2000 * 1000); + case ESC_PROTOCOL_ONESHOT125: return nsToTicks(250 * 1000); + case ESC_PROTOCOL_ONESHOT42: return nsToTicks(84 * 1000); + case ESC_PROTOCOL_MULTISHOT: return nsToTicks(25 * 1000); + case ESC_PROTOCOL_BRUSHED: return nsToTicks(_interval * 1000); case ESC_PROTOCOL_DSHOT150: case ESC_PROTOCOL_DSHOT300: case ESC_PROTOCOL_DSHOT600: @@ -466,25 +468,20 @@ uint32_t EscDriverEsp32::getPulseMax() const } } -uint32_t EscDriverEsp32::getPulseInterval() const -{ - return getAnalogPulse(_interval * 1000); -} - -uint32_t EscDriverEsp32::getAnalogPulse(int32_t ns) const +uint32_t EscDriverEsp32::nsToTicks(uint32_t ns) const { int div = getClockDivider(); - return ns / ((div * DURATION_CLOCK) / 2); + // multiply by 2 as DURATION_CLOCK is aleredy doubled, and round to nearest integer + return (ns * 2 + ((div * DURATION_CLOCK) >> 1)) / div / DURATION_CLOCK; } uint16_t EscDriverEsp32::getDshotPulse(uint32_t width) const { - int div = getClockDivider(); switch (_protocol) { - case ESC_PROTOCOL_DSHOT150: return ((width * 4) / (div * DURATION_CLOCK / 2)); - case ESC_PROTOCOL_DSHOT300: return ((width * 2) / (div * DURATION_CLOCK / 2)); - case ESC_PROTOCOL_DSHOT600: return ((width * 1) / (div * DURATION_CLOCK / 2)); + case ESC_PROTOCOL_DSHOT150: return nsToTicks(width * 4); + case ESC_PROTOCOL_DSHOT300: return nsToTicks(width * 2); + case ESC_PROTOCOL_DSHOT600: return nsToTicks(width * 1); case ESC_PROTOCOL_BRUSHED: case ESC_PROTOCOL_PWM: case ESC_PROTOCOL_ONESHOT125: diff --git a/lib/EscDriver/src/EscDriverEsp32.h b/lib/EscDriver/src/EscDriverEsp32.h index 231561ca..c79b110c 100644 --- a/lib/EscDriver/src/EscDriverEsp32.h +++ b/lib/EscDriver/src/EscDriverEsp32.h @@ -74,6 +74,7 @@ class EscDriverEsp32: public EscDriverBase uint16_t dshot_t0l; uint16_t dshot_t1h; uint16_t dshot_t1l; + uint16_t dshot_tlm_bit_len; uint32_t telemetryValue; }; @@ -104,8 +105,7 @@ class EscDriverEsp32: public EscDriverBase uint32_t getClockDivider() const; uint32_t getPulseMin() const; uint32_t getPulseMax() const; - uint32_t getPulseInterval() const; - uint32_t getAnalogPulse(int32_t ns) const; + uint32_t nsToTicks(uint32_t ns) const; uint16_t getDshotPulse(uint32_t width) const; bool isDigital(EscProtocol protocol) const; diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 5300eb12..f995cf92 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -6,9 +6,6 @@ #include "EscDriver.h" #include "platform.h" -#define ERPM_PER_LSB 100.0f -#define SECONDS_PER_MINUTE 60 - namespace Espfc { namespace Output { @@ -44,7 +41,7 @@ class Mixer _model.state.escServo = _servo = &escServo; _model.logger.info().log(F("SERVO CONF")).log(ESC_PROTOCOL_PWM).log(true).logln(_model.config.output.servoRate).logln(ESC_DRIVER_SERVO_TIMER); } - _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); + _erpmToHz = EscDriver::getErpmToHzRatio(_model.config.output.motorPoles); _statsCounterMax = _model.state.mixerTimer.rate / 2; _statsCounter = 0; @@ -373,14 +370,12 @@ class Mixer float inline erpmToHz(float erpm) { - // rpm = (erpm * ERPM_PER_LSB) / (_model.config.output.motorPoles / 2) - // _erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (_model.config.output.motorPoles / 2.0f); return _erpmToHz * erpm; } float inline erpmToRpm(float erpm) { - return erpmToHz(erpm) * SECONDS_PER_MINUTE; + return erpmToHz(erpm) * EscDriver::SECONDS_PER_MINUTE; } bool _stop(void) diff --git a/test/test_esc/test_esc.cpp b/test/test_esc/test_esc.cpp index 087afaa3..e437b7a4 100644 --- a/test/test_esc/test_esc.cpp +++ b/test/test_esc/test_esc.cpp @@ -61,34 +61,33 @@ void test_esc_gcr_convert_to_value() void test_esc_gcr_convert_to_erpm() { - TEST_ASSERT_EQUAL_UINT32(185, EscDriver::convertToErpm(EscDriver::convertToValue(1942))); - TEST_ASSERT_EQUAL_UINT32(184, EscDriver::convertToErpm(EscDriver::convertToValue(1944))); - TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1947))); - TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1948))); - TEST_ASSERT_EQUAL_UINT32(182, EscDriver::convertToErpm(EscDriver::convertToValue(1949))); - TEST_ASSERT_EQUAL_UINT32(181, EscDriver::convertToErpm(EscDriver::convertToValue(1950))); + TEST_ASSERT_EQUAL_UINT32( 60, EscDriver::convertToErpm(10000)); + TEST_ASSERT_EQUAL_UINT32( 120, EscDriver::convertToErpm( 5000)); + TEST_ASSERT_EQUAL_UINT32( 300, EscDriver::convertToErpm( 2000)); + TEST_ASSERT_EQUAL_UINT32( 600, EscDriver::convertToErpm( 1000)); + TEST_ASSERT_EQUAL_UINT32(1200, EscDriver::convertToErpm( 500)); + TEST_ASSERT_EQUAL_UINT32(3000, EscDriver::convertToErpm( 200)); + TEST_ASSERT_EQUAL_UINT32(6000, EscDriver::convertToErpm( 100)); } void test_esc_duration_to_bitlen() { - uint32_t bit_len = 178; - TEST_ASSERT_EQUAL_UINT32(0, EscDriver::durationToBitLen(80, bit_len)); + uint32_t bit_len = 213; // dshot300: 2.666us/12.5, dshot600: 1.333us/12.5 + TEST_ASSERT_EQUAL_UINT32(0, EscDriver::durationToBitLen( 80, bit_len)); TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(150, bit_len)); TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(160, bit_len)); TEST_ASSERT_EQUAL_UINT32(1, EscDriver::durationToBitLen(177, bit_len)); - TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(300, bit_len)); TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(373, bit_len)); TEST_ASSERT_EQUAL_UINT32(2, EscDriver::durationToBitLen(383, bit_len)); - TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(450, bit_len)); - TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(498, bit_len)); TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(595, bit_len)); TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(607, bit_len)); TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(610, bit_len)); + TEST_ASSERT_EQUAL_UINT32(3, EscDriver::durationToBitLen(640, bit_len)); - TEST_ASSERT_EQUAL_UINT32(4, EscDriver::durationToBitLen(664, bit_len)); + TEST_ASSERT_EQUAL_UINT32(4, EscDriver::durationToBitLen(850, bit_len)); } constexpr uint32_t make_item(uint32_t duration0, uint32_t level0, uint32_t duration1, uint32_t level1) @@ -96,7 +95,7 @@ constexpr uint32_t make_item(uint32_t duration0, uint32_t level0, uint32_t durat return (duration0 & 0x07fff) | (level0 & 0x1) << 15 | (duration1 & 0x07fff) << 16 | (level1 & 0x1) << 31; } -void test_esc_extract_telemetry_gcr_real() +void test_esc_extract_telemetry_gcr_synth() { // 0b 0 11 0 1 00 111 0 11 0 11 00 1 0 1 // 24 0:100 1:200 0:100 1:100 0:200 1:300 0:100 1:200 0:100 1:200 0:200 1:100 0:100 @@ -114,8 +113,40 @@ void test_esc_extract_telemetry_gcr_real() TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); } +void test_esc_extract_telemetry_gcr_synth_idle() +{ + // 0b 00 1 0 1 00 1 0 1 00 1 0 1 0 1 000 1 + // 32 0:200 1:100 0:100 1:100 0:200 1:100 0:100 1:100 0:200 1:100 0:100 1:100 0:100 1:100 0:300 + uint32_t data[] = { + make_item(200, 0, 100, 1), make_item(100, 0, 100, 1), make_item(200, 0, 100, 1), + make_item(100, 0, 100, 1), make_item(200, 0, 100, 1), make_item(100, 0, 100, 1), + make_item(100, 0, 100, 1), make_item(300, 0, 0, 1), + }; + uint32_t bit_len = 100; + uint32_t data_len = sizeof(data); + TEST_ASSERT_EQUAL_UINT32(100, bit_len); + TEST_ASSERT_EQUAL_UINT32(32, data_len); + uint32_t gcr = 0b001010010100101010001; + TEST_ASSERT_EQUAL_HEX32(0x52951, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x0fff, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x0, value); -void test_esc_extract_telemetry_gcr1() + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(0, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, rpm); +} + +void test_esc_extract_telemetry_dshot300_sample() { // 24 0:373 1:605 0:163 1:605 0:174 1:376 0:379 1:599 0:177 1:163 0:173 1EF67A 1EF67A? // 00 111 0 111 0 11 00 111 0 1 0 @@ -123,16 +154,31 @@ void test_esc_extract_telemetry_gcr1() make_item(373, 0, 605, 1), make_item(163, 0, 605, 1), make_item(174, 0, 376, 1), make_item(379, 0, 599, 1), make_item(177, 0, 163, 1), make_item(173, 0, 0, 1), }; - uint32_t bit_len = 2136 / (1000 / 80); + uint32_t bit_len = 2667 / (1000.0 / 80); uint32_t data_len = sizeof(data); - TEST_ASSERT_EQUAL_UINT32(178, bit_len); + TEST_ASSERT_EQUAL_UINT32(213, bit_len); TEST_ASSERT_EQUAL_UINT32(24, data_len); - uint32_t exp = 0b001110111011001110101; // +1 - TEST_ASSERT_EQUAL_HEX32(0x77675, exp); - TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + uint32_t gcr = 0b001110111011001110101; // +1 + TEST_ASSERT_EQUAL_HEX32(0x77675, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x093a, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x13a0, value); + + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(119, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 28.33f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 1700.0f, rpm); } -void test_esc_extract_telemetry_gcr2() +void test_esc_extract_telemetry_dshot300_running() { // 28 0:177 1:379 0:177 1:167 0:173 1:164 0:379 1:177 0:373 1:379 0:380 1:608 0:164 D499E D499E? // 0 11 0 1 0 1 00 1 00 11 00 111 0 @@ -141,13 +187,61 @@ void test_esc_extract_telemetry_gcr2() make_item(379, 0, 177, 1), make_item(373, 0, 379, 1), make_item(380, 0, 608, 1), make_item(164, 0, 0, 0), }; - uint32_t bit_len = 2136 / (1000 / 80); + uint32_t bit_len = 2667 / (1000.0 / 80); uint32_t data_len = sizeof(data); - uint32_t exp = 0b011010100100110011101; // +1 - TEST_ASSERT_EQUAL_UINT32(178, bit_len); + uint32_t gcr = 0b011010100100110011101; // +1 + TEST_ASSERT_EQUAL_UINT32(213, bit_len); TEST_ASSERT_EQUAL_UINT32(28, data_len); - TEST_ASSERT_EQUAL_HEX32(0xD499D, exp); - TEST_ASSERT_EQUAL_HEX32(exp, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + TEST_ASSERT_EQUAL_HEX32(0xD499D, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x071a, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x08d0, value); + + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(266, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 63.33f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 3800.0f, rpm); +} + +void test_esc_extract_telemetry_dshot300_idle() +{ + // 32 0:380 1:172 0:175 1:172 0:371 1:172 0:175 1:172 0:371 1:172 0:174 1:162 0:172 1:172 0:606 52950 52950 + // 00 1 0 1 00 1 0 1 00 1 0 1 0 1 000 1 + uint32_t data[] = { + make_item(380, 0, 172, 1), make_item(175, 0, 172, 1), make_item(371, 0, 172, 1), + make_item(175, 0, 172, 1), make_item(371, 0, 172, 1), make_item(174, 0, 162, 1), + make_item(172, 0, 172, 1), make_item(606, 0, 0, 1), + }; + uint32_t bit_len = 2667 / (1000.0 / 80); + uint32_t data_len = sizeof(data); + TEST_ASSERT_EQUAL_UINT32(213, bit_len); + TEST_ASSERT_EQUAL_UINT32(32, data_len); + uint32_t gcr = 0b001010010100101010001; // +1 + TEST_ASSERT_EQUAL_HEX32(0x52951, gcr); + TEST_ASSERT_EQUAL_HEX32(gcr, EscDriver::extractTelemetryGcr(data, data_len, bit_len)); + + uint32_t value = EscDriver::gcrToRawValue(gcr); + TEST_ASSERT_EQUAL_HEX32(0x0fff, value); + + value = EscDriver::convertToValue(value); + TEST_ASSERT_EQUAL_HEX32(0x0, value); + + uint32_t erpm = EscDriver::convertToErpm(value); + TEST_ASSERT_EQUAL_UINT32(0, erpm); + + float erpmToHz = EscDriver::getErpmToHzRatio(14); + float freq = erpmToHz * erpm; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, freq); + float rpm = freq * 60; + TEST_ASSERT_FLOAT_WITHIN(0.01f, 0.0f, rpm); } int main(int argc, char **argv) @@ -160,9 +254,11 @@ int main(int argc, char **argv) RUN_TEST(test_esc_gcr_convert_to_value); RUN_TEST(test_esc_gcr_convert_to_erpm); RUN_TEST(test_esc_duration_to_bitlen); - RUN_TEST(test_esc_extract_telemetry_gcr_real); - RUN_TEST(test_esc_extract_telemetry_gcr1); - RUN_TEST(test_esc_extract_telemetry_gcr2); + RUN_TEST(test_esc_extract_telemetry_gcr_synth); + RUN_TEST(test_esc_extract_telemetry_gcr_synth_idle); + RUN_TEST(test_esc_extract_telemetry_dshot300_sample); + RUN_TEST(test_esc_extract_telemetry_dshot300_running); + RUN_TEST(test_esc_extract_telemetry_dshot300_idle); UNITY_END(); return 0; From 180e1d7725ad12dcf082d9b673d793f328515c69 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sat, 24 Feb 2024 15:17:43 +0100 Subject: [PATCH 24/28] init rpm filter --- lib/Espfc/src/Blackbox.h | 6 ++ lib/Espfc/src/Cli.h | 4 + lib/Espfc/src/Model.h | 25 ++++- lib/Espfc/src/ModelConfig.h | 13 +++ lib/Espfc/src/ModelState.h | 4 +- lib/Espfc/src/Msp/MspProcessor.h | 10 +- lib/Espfc/src/Output/Mixer.h | 14 ++- lib/Espfc/src/Sensor/GyroSensor.h | 148 ++++++++++++++++++++++++----- lib/Espfc/src/SensorManager.h | 4 +- lib/Espfc/src/Utils/FilterHelper.h | 26 +++++ lib/betaflight/src/platform.c | 1 + lib/betaflight/src/platform.h | 12 +++ 12 files changed, 229 insertions(+), 38 deletions(-) create mode 100644 lib/Espfc/src/Utils/FilterHelper.h diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index d362aabc..073393e5 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -330,6 +330,12 @@ class Blackbox rxConfigMutable()->airModeActivateThreshold = 40; rxConfigMutable()->serialrx_provider = _model.config.input.serialRxProvider; + rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilterHarmonics; + rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilterQ; + rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilterMinFreq; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = 0; + rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilterFreqLpf; + blackboxInit(); return 1; diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 835ecb24..d69798da 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -407,6 +407,10 @@ class Cli Param(PSTR("gyro_dyn_notch_count"), &c.dynamicFilter.width), Param(PSTR("gyro_dyn_notch_min"), &c.dynamicFilter.min_freq), Param(PSTR("gyro_dyn_notch_max"), &c.dynamicFilter.max_freq), + Param(PSTR("gyro_rpm_harmonics"), &c.rpmFilterHarmonics), + Param(PSTR("gyro_rpm_q"), &c.rpmFilterQ), + Param(PSTR("gyro_rpm_min_freq"), &c.rpmFilterMinFreq), + Param(PSTR("gyro_rpm_freq_lpf"), &c.rpmFilterFreqLpf), Param(PSTR("gyro_offset_x"), &c.gyroBias[0]), Param(PSTR("gyro_offset_y"), &c.gyroBias[1]), Param(PSTR("gyro_offset_z"), &c.gyroBias[2]), diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 98c38059..e412d774 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -209,6 +209,13 @@ class Model return false; } + void inline setDebug(DebugMode mode, size_t index, int16_t value) + { + if(index >= 8) return; + if(config.debugMode != mode) return; + state.debug[index] = value; + } + Device::SerialDevice * getSerialStream(SerialPort i) { return state.serial[i].stream; @@ -462,20 +469,32 @@ class Model { for(size_t p = 0; p < (size_t)config.dynamicFilter.width; p++) { - state.gyroDynNotchFilter[i][p].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); + state.gyroDynNotchFilter[p][i].begin(FilterConfig(FILTER_NOTCH_DF1, 400, 380), gyroFilterRate); } } state.gyroNotch1Filter[i].begin(config.gyroNotch1Filter, gyroFilterRate); state.gyroNotch2Filter[i].begin(config.gyroNotch2Filter, gyroFilterRate); - if(config.gyroDynLpfFilter.cutoff > 0) { + if(config.gyroDynLpfFilter.cutoff > 0) + { state.gyroFilter[i].begin(FilterConfig((FilterType)config.gyroFilter.type, config.gyroDynLpfFilter.cutoff), gyroFilterRate); - } else { + } + else + { state.gyroFilter[i].begin(config.gyroFilter, gyroFilterRate); } state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate); state.gyroFilter3[i].begin(config.gyroFilter3, gyroPreFilterRate); state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), gyroFilterRate); + for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + { + state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.rpmFilterFreqLpf), gyroFilterRate); + for(size_t n = 0; n < config.rpmFilterHarmonics; n++) + { + int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.rpmFilterHarmonics, config.rpmFilterMinFreq, gyroFilterRate / 2); + state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center - 10), gyroFilterRate); + } + } if(magActive()) { state.magFilter[i].begin(config.magFilter, state.magTimer.rate); diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 633a11df..42604416 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -226,10 +226,14 @@ enum InputFilterType : uint8_t { const size_t MODEL_NAME_LEN = 16; const size_t AXES = 4; +const size_t AXES_RPY = 3; const size_t INPUT_CHANNELS = AXIS_COUNT; const size_t OUTPUT_CHANNELS = ESC_CHANNEL_COUNT; static_assert(ESC_CHANNEL_COUNT == ESPFC_OUTPUT_COUNT, "ESC_CHANNEL_COUNT and ESPFC_OUTPUT_COUNT must be equal"); +const size_t RPM_FILTER_MOTOR_MAX = 4; +const size_t RPM_FILTER_HARMONICS_MAX = 3; + enum PinFunction { #ifdef ESPFC_INPUT PIN_INPUT_RX, @@ -643,6 +647,10 @@ class ModelConfig WirelessConfig wireless; DynamicFilterConfig dynamicFilter; + uint8_t rpmFilterHarmonics; + uint8_t rpmFilterMinFreq; + int16_t rpmFilterQ; + uint8_t rpmFilterFreqLpf; ModelConfig() { @@ -745,6 +753,11 @@ class ModelConfig //dtermFilter = FilterConfig(FILTER_PT1, 113); //dtermFilter2 = FilterConfig(FILTER_PT1, 113); + rpmFilterHarmonics = 3; + rpmFilterMinFreq = 100; + rpmFilterQ = 500; + rpmFilterFreqLpf = 150; + gyroFilter3 = FilterConfig(FILTER_PT1, 150); gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index fd776fca..fb3d6ddd 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -174,12 +174,14 @@ struct ModelState Filter gyroFilter3[3]; Filter gyroNotch1Filter[3]; Filter gyroNotch2Filter[3]; - Filter gyroDynNotchFilter[3][6]; + Filter gyroDynNotchFilter[6][3]; Filter gyroImuFilter[3]; Filter accelFilter[3]; Filter magFilter[3]; Filter inputFilter[4]; + Filter rpmFreqFilter[RPM_FILTER_MOTOR_MAX]; + Filter rpmFilter[RPM_FILTER_MOTOR_MAX][RPM_FILTER_HARMONICS_MAX][3]; VectorFloat velocity; VectorFloat desiredVelocity; diff --git a/lib/Espfc/src/Msp/MspProcessor.h b/lib/Espfc/src/Msp/MspProcessor.h index ad848779..6a5b7cbb 100644 --- a/lib/Espfc/src/Msp/MspProcessor.h +++ b/lib/Espfc/src/Msp/MspProcessor.h @@ -1052,11 +1052,10 @@ class MspProcessor r.writeU16(_model.config.dynamicFilter.q); // dyn_notch_q r.writeU16(_model.config.dynamicFilter.min_freq); // dyn_notch_min_hz // rpm filter - r.writeU8(0); // gyro_rpm_notch_harmonics - r.writeU8(0); // gyro_rpm_notch_min + r.writeU8(_model.config.rpmFilterHarmonics); // gyro_rpm_notch_harmonics + r.writeU8(_model.config.rpmFilterMinFreq); // gyro_rpm_notch_min // 1.43+ r.writeU16(_model.config.dynamicFilter.max_freq); // dyn_notch_max_hz - break; case MSP_SET_FILTER_CONFIG: @@ -1098,8 +1097,8 @@ class MspProcessor _model.config.dynamicFilter.width = m.readU8(); // dyn_notch_width_percent _model.config.dynamicFilter.q = m.readU16(); // dyn_notch_q _model.config.dynamicFilter.min_freq = m.readU16(); // dyn_notch_min_hz - m.readU8(); // gyro_rpm_notch_harmonics - m.readU8(); // gyro_rpm_notch_min + _model.config.rpmFilterHarmonics = m.readU8(); // gyro_rpm_notch_harmonics + _model.config.rpmFilterMinFreq = m.readU8(); // gyro_rpm_notch_min } // 1.43+ if (m.remain() >= 1) { @@ -1178,7 +1177,6 @@ class MspProcessor r.writeU8(_model.config.output.motorLimit); // motor_output_limit r.writeU8(0); // auto_profile_cell_count r.writeU8(0); // idle_min_rpm - break; case MSP_SET_PID_ADVANCED: diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index f995cf92..3e8eab5f 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -291,6 +291,7 @@ class Mixer for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { + if(i >= RPM_FILTER_MOTOR_MAX) continue; const OutputChannelConfig& och = _model.config.output.channel[i]; if(och.servo) continue; uint32_t value = _motor->telemetry(i); @@ -337,17 +338,22 @@ class Mixer break; default: value = EscDriver::convertToValue(value); - uint32_t erpm = _model.state.outputTelemetryErpm[i] = EscDriver::convertToErpm(value); + _model.state.outputTelemetryErpm[i] = EscDriver::convertToErpm(value); if(_model.config.debugMode == DEBUG_DSHOT_RPM_TELEMETRY) { - _model.state.debug[i] = erpm; + _model.state.debug[i] = _model.state.outputTelemetryErpm[i]; } - _model.state.outputTelemetryRpm[i] = erpmToRpm(erpm); - _model.state.outputTelemetryFreq[i] = erpmToHz(erpm); + _model.state.outputTelemetryRpm[i] = erpmToRpm(_model.state.outputTelemetryErpm[i]); break; } } + for(size_t i = 0; i < OUTPUT_CHANNELS; i++) + { + if(i >= RPM_FILTER_MOTOR_MAX) continue; + _model.state.outputTelemetryFreq[i] = _model.state.rpmFreqFilter[i].update(erpmToHz(_model.state.outputTelemetryErpm[i])); + } + _statsCounter++; if(_statsCounter >= _statsCounterMax) { diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 09e361fd..627c7673 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -2,6 +2,7 @@ #define _ESPFC_SENSOR_GYRO_SENSOR_H_ #include "BaseSensor.h" +#include "Utils/FilterHelper.h" #include "Device/GyroDevice.h" #include "Math/Sma.h" #include "Math/FreqAnalyzer.h" @@ -38,6 +39,10 @@ class GyroSensor: public BaseSensor _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); _dyn_notch_sma.begin(_dyn_notch_denom); + _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; + _rpm_enabled = _model.config.rpmFilterHarmonics > 0; + for(size_t i = 0; i < 3; i++) { #ifdef ESPFC_DSP @@ -75,10 +80,7 @@ class GyroSensor: public BaseSensor if(_model.config.gyroFilter3.freq) { - for(size_t i = 0; i < 3; ++i) - { - _model.state.gyroSampled.set(i, _model.state.gyroFilter3[i].update(input[i])); - } + _model.state.gyroSampled = Utils::applyFilter(_model.state.gyroFilter3, input); } else { @@ -99,10 +101,9 @@ class GyroSensor: public BaseSensor calibrate(); - _model.state.gyroScaled = _model.state.gyro; - - bool dynNotchEnabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; + _model.state.gyroScaled = _model.state.gyro; // must be after calibration +#if 0 // filtering for(size_t i = 0; i < 3; ++i) { @@ -126,26 +127,44 @@ class GyroSensor: public BaseSensor _model.state.debug[1] = lrintf(degrees(_model.state.gyro[i])); } - _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); + if(_rpm_enabled) + { + for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + { + for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + { + _model.state.gyro.set(i, _model.state.rpmFilter[m][n][i].update(_model.state.gyro[i])); + } + } + } if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); } + _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) { _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); } + } + + if(_dyn_notch_enabled || _dyn_notch_debug) + { + _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + } - if(dynNotchEnabled) + for(size_t i = 0; i < 3; ++i) + { + if(_dyn_notch_enabled) { for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) { - _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[i][p].update(_model.state.gyro[i])); + _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[p][i].update(_model.state.gyro[i])); } } @@ -158,28 +177,110 @@ class GyroSensor: public BaseSensor _model.state.gyroImu.set(i, _model.state.gyroImuFilter[i].update(_model.state.gyro[i])); } } +#else + for(size_t i = 0; i < 3; ++i) + { + _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyroRaw[i]); + _model.setDebug(DEBUG_GYRO_SCALED, i, lrintf(degrees(_model.state.gyroScaled[i]))); + } + + _model.setDebug(DEBUG_GYRO_SAMPLE, 0, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter2, _model.state.gyro); + + _model.setDebug(DEBUG_GYRO_SAMPLE, 1, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + if(_rpm_enabled) + { + for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + { + for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + { + _model.state.gyro = Utils::applyFilter(_model.state.rpmFilter[m][n], _model.state.gyro); + } + } + } + + _model.setDebug(DEBUG_GYRO_SAMPLE, 2, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch1Filter, _model.state.gyro); + _model.state.gyro = Utils::applyFilter(_model.state.gyroNotch2Filter, _model.state.gyro); + _model.state.gyro = Utils::applyFilter(_model.state.gyroFilter, _model.state.gyro); + + _model.setDebug(DEBUG_GYRO_SAMPLE, 3, lrintf(degrees(_model.state.gyro[_model.config.debugAxis]))); + + if(_dyn_notch_enabled || _dyn_notch_debug) + { + _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); + } + + if(_dyn_notch_enabled) + { + for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) + { + _model.state.gyro = Utils::applyFilter(_model.state.gyroDynNotchFilter[p], _model.state.gyro); + } + } + + for(size_t i = 0; i < 3; ++i) + { + _model.setDebug(DEBUG_GYRO_FILTERED, i, lrintf(degrees(_model.state.gyro[i]))); + } + + if(_model.accelActive()) + { + _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro); + } +#endif return 1; } - void dynNotchAnalyze() + void postLoop() + { + rpmFilterUpdate(); + dynNotchFilterUpdate(); + } + + void rpmFilterUpdate() + { + if(!_rpm_enabled) return; + + const float minFreq = _model.config.rpmFilterMinFreq; + const float maxFreq = 0.48f * _model.state.loopTimer.rate; + const float q = _model.config.rpmFilterQ * 0.01f; + // TODO: optimize, to update filters in steps + for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + { + const float motorFreq = _model.state.outputTelemetryFreq[m]; + for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + { + const float freq = motorFreq * (n + 1); + if(minFreq < freq && freq < maxFreq) + { + for(size_t i = 0; i < 3; ++i) + { + _model.state.rpmFilter[m][n][i].reconfigure(freq, freq, q); + } + } + } + } + } + + void dynNotchFilterUpdate() { if(!_model.gyroActive()) return; if(_model.state.loopTimer.rate < DynamicFilterConfig::MIN_FREQ) return; Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); - bool enabled = _model.isActive(FEATURE_DYNAMIC_FILTER); - bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; - bool debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; const float q = _model.config.dynamicFilter.q * 0.01; const size_t peakCount = _model.config.dynamicFilter.width; + bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; bool update = _model.state.dynamicFilterTimer.check(); - if(enabled || debug) + if(_dyn_notch_enabled || _dyn_notch_debug) { - _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); - for(size_t i = 0; i < 3; ++i) { #ifdef ESPFC_DSP @@ -200,14 +301,14 @@ class GyroSensor: public BaseSensor _model.state.debug[2] = lrintf(_fft[i].peaks[2].freq); _model.state.debug[3] = lrintf(_fft[i].peaks[3].freq); } - if(enabled && status) + if(_dyn_notch_enabled && status) { for(size_t p = 0; p < peakCount; p++) { float freq = _fft[i].peaks[p].freq; if(freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) { - _model.state.gyroDynNotchFilter[i][p].reconfigure(freq, freq, q); + _model.state.gyroDynNotchFilter[p][i].reconfigure(freq, freq, q); } } } @@ -228,7 +329,7 @@ class GyroSensor: public BaseSensor if(update) _model.state.debug[i] = lrintf(freq); if(i == _model.config.debugAxis) _model.state.debug[3] = lrintf(degrees(_model.state.gyroDynNotch[i])); } - if(enabled && update) + if(_dyn_notch_enabled && update) { if(freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) { @@ -237,7 +338,7 @@ class GyroSensor: public BaseSensor size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; int16_t f = Math::clamp((int16_t)lrintf(freq * harmonic), _model.config.dynamicFilter.min_freq, _model.config.dynamicFilter.max_freq); - _model.state.gyroDynNotchFilter[x][p].reconfigure(f, f, q); + _model.state.gyroDynNotchFilter[p][x].reconfigure(f, f, q); } } } @@ -288,6 +389,9 @@ class GyroSensor: public BaseSensor Math::Sma _sma; Math::Sma _dyn_notch_sma; size_t _dyn_notch_denom; + bool _dyn_notch_enabled; + bool _dyn_notch_debug; + bool _rpm_enabled; Model& _model; Device::GyroDevice * _gyro; diff --git a/lib/Espfc/src/SensorManager.h b/lib/Espfc/src/SensorManager.h index b3a5af5d..c140b253 100644 --- a/lib/Espfc/src/SensorManager.h +++ b/lib/Espfc/src/SensorManager.h @@ -76,7 +76,7 @@ class SensorManager int postLoop() { - _gyro.dynNotchAnalyze(); + _gyro.postLoop(); return 1; } @@ -102,7 +102,7 @@ class SensorManager // sub task int updateDelayed() { - _gyro.dynNotchAnalyze(); + _gyro.postLoop(); int status = _accel.update(); if(_fusionUpdate) { diff --git a/lib/Espfc/src/Utils/FilterHelper.h b/lib/Espfc/src/Utils/FilterHelper.h new file mode 100644 index 00000000..3f6edeec --- /dev/null +++ b/lib/Espfc/src/Utils/FilterHelper.h @@ -0,0 +1,26 @@ +#pragma once + +#include "Filter.h" +#include "helper_3dmath.h" + +namespace Espfc { + +namespace Utils { + +float applyFilter(Filter& filter, float sample) +{ + return filter.update(sample); +} + +VectorFloat applyFilter(Filter * filters, const VectorFloat& samples) +{ + VectorFloat result; + result.x = filters[0].update(samples.x); + result.y = filters[1].update(samples.y); + result.z = filters[2].update(samples.z); + return result; +} + +} + +} diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index 6042a88d..f318e982 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -19,6 +19,7 @@ const char * boardIdentifier = "ESPF"; PG_RESET_TEMPLATE_DEF(serialConfig_t, serialConfig); PG_RESET_TEMPLATE_DEF(mixerConfig_t, mixerConfig); PG_RESET_TEMPLATE_DEF(motorConfig_t, motorConfig); +PG_RESET_TEMPLATE_DEF(rpmFilterConfig_t, rpmFilterConfig); PG_RESET_TEMPLATE_DEF(featureConfig_t, featureConfig); PG_RESET_TEMPLATE_DEF(flight3DConfig_t, flight3DConfig); PG_RESET_TEMPLATE_DEF(armingConfig_t, armingConfig); diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index 0065b1be..f494418d 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -9,6 +9,7 @@ #define USE_DYN_NOTCH_FILTER #define USE_ITERM_RELAX #define USE_DSHOT_TELEMETRY +#define USE_RPM_FILTER #include #include @@ -404,6 +405,17 @@ typedef struct motorConfig_s { PG_DECLARE(motorConfig_t, motorConfig); +typedef struct rpmFilterConfig_s { + uint8_t rpm_filter_harmonics; // how many harmonics should be covered with notches? 0 means filter off + //uint8_t rpm_filter_weights[RPM_FILTER_HARMONICS_MAX]; // effect or "weight" (0% - 100%) of each RPM filter harmonic + uint8_t rpm_filter_min_hz; // minimum frequency of the notches + uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz + uint16_t rpm_filter_q; // q of the notches + uint16_t rpm_filter_lpf_hz; // the cutoff of the lpf on reported motor rpm +} rpmFilterConfig_t; + +PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig); + extern float motor[MAX_SUPPORTED_MOTORS]; extern float motor_disarmed[MAX_SUPPORTED_MOTORS]; From 1d0c51f4827604f4f96596f6041e6c1e49e9c5b3 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 25 Feb 2024 20:47:38 +0100 Subject: [PATCH 25/28] refactor filters + fix stats --- lib/Espfc/src/Cli.h | 1 + lib/Espfc/src/Device/BusSPI.h | 4 +- lib/Espfc/src/Model.h | 4 +- lib/Espfc/src/Sensor/GyroSensor.h | 98 ++++--------------------------- lib/Espfc/src/Stats.h | 8 +-- 5 files changed, 19 insertions(+), 96 deletions(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index d69798da..0a4e256b 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -1234,6 +1234,7 @@ class Cli float load = _model.state.stats.getLoad(c); int freq = lrintf(_model.state.stats.getFreq(c)); int real = lrintf(_model.state.stats.getReal(c)); + if(freq == 0) continue; s.print(FPSTR(_model.state.stats.getName(c))); s.print(": "); diff --git a/lib/Espfc/src/Device/BusSPI.h b/lib/Espfc/src/Device/BusSPI.h index 8dbc9f92..10e54ee8 100644 --- a/lib/Espfc/src/Device/BusSPI.h +++ b/lib/Espfc/src/Device/BusSPI.h @@ -54,7 +54,7 @@ class BusSPI: public BusDevice void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, const uint8_t *in, uint8_t *out, uint32_t speed) { _dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0)); - digitalWrite(devAddr, LOW); + EspGpio::digitalWrite(devAddr, LOW); #if defined (ARCH_RP2040) _dev.transfer(regAddr); _dev.transfer(in, out, length); @@ -62,7 +62,7 @@ class BusSPI: public BusDevice _dev.transfer(regAddr); _dev.transferBytes(in, out, length); #endif - digitalWrite(devAddr, HIGH); + EspGpio::digitalWrite(devAddr, HIGH); _dev.endTransaction(); } ESPFC_SPI_0_DEV_T& _dev; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index e412d774..96478fc5 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -446,7 +446,7 @@ class Model state.actuatorTimer.setRate(50); state.dynamicFilterTimer.setRate(50); state.telemetryTimer.setInterval(config.telemetryInterval * 1000); - state.stats.timer.setRate(2); + state.stats.timer.setRate(4); #if defined(ESPFC_MULTI_CORE) state.serialTimer.setRate(4000); #else @@ -485,7 +485,7 @@ class Model state.gyroFilter2[i].begin(config.gyroFilter2, gyroFilterRate); state.gyroFilter3[i].begin(config.gyroFilter3, gyroPreFilterRate); state.accelFilter[i].begin(config.accelFilter, gyroFilterRate); - state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 2), gyroFilterRate); + state.gyroImuFilter[i].begin(FilterConfig(FILTER_PT1, state.accelTimer.rate / 3), gyroFilterRate); for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) { state.rpmFreqFilter[m].begin(FilterConfig(FILTER_PT1, config.rpmFilterFreqLpf), gyroFilterRate); diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index 627c7673..a348995d 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -41,7 +41,7 @@ class GyroSensor: public BaseSensor _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; - _rpm_enabled = _model.config.rpmFilterHarmonics > 0; + _rpm_enabled = _model.config.rpmFilterHarmonics > 0 && _model.config.output.dshotTelemetry; for(size_t i = 0; i < 3; i++) { @@ -76,7 +76,7 @@ class GyroSensor: public BaseSensor align(_model.state.gyroRaw, _model.config.gyroAlign); - VectorFloat input = (VectorFloat)_model.state.gyroRaw * _model.state.gyroScale; + VectorFloat input = static_cast(_model.state.gyroRaw) * _model.state.gyroScale; if(_model.config.gyroFilter3.freq) { @@ -84,7 +84,6 @@ class GyroSensor: public BaseSensor } else { - // moving average filter _model.state.gyroSampled = _sma.update(input); } @@ -103,81 +102,6 @@ class GyroSensor: public BaseSensor _model.state.gyroScaled = _model.state.gyro; // must be after calibration -#if 0 - // filtering - for(size_t i = 0; i < 3; ++i) - { - if(_model.config.debugMode == DEBUG_GYRO_RAW) - { - _model.state.debug[i] = _model.state.gyroRaw[i]; - } - if(_model.config.debugMode == DEBUG_GYRO_SCALED) - { - _model.state.debug[i] = lrintf(degrees(_model.state.gyroScaled[i])); - } - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) - { - _model.state.debug[0] = lrintf(degrees(_model.state.gyro[i])); - } - - _model.state.gyro.set(i, _model.state.gyroFilter2[i].update(_model.state.gyro[i])); - - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) - { - _model.state.debug[1] = lrintf(degrees(_model.state.gyro[i])); - } - - if(_rpm_enabled) - { - for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) - { - for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) - { - _model.state.gyro.set(i, _model.state.rpmFilter[m][n][i].update(_model.state.gyro[i])); - } - } - } - - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) - { - _model.state.debug[2] = lrintf(degrees(_model.state.gyro[i])); - } - - _model.state.gyro.set(i, _model.state.gyroNotch1Filter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroNotch2Filter[i].update(_model.state.gyro[i])); - _model.state.gyro.set(i, _model.state.gyroFilter[i].update(_model.state.gyro[i])); - - if(_model.config.debugMode == DEBUG_GYRO_SAMPLE && i == _model.config.debugAxis) - { - _model.state.debug[3] = lrintf(degrees(_model.state.gyro[i])); - } - } - - if(_dyn_notch_enabled || _dyn_notch_debug) - { - _model.state.gyroDynNotch = _dyn_notch_sma.update(_model.state.gyro); - } - - for(size_t i = 0; i < 3; ++i) - { - if(_dyn_notch_enabled) - { - for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) - { - _model.state.gyro.set(i, _model.state.gyroDynNotchFilter[p][i].update(_model.state.gyro[i])); - } - } - - if(_model.config.debugMode == DEBUG_GYRO_FILTERED) - { - _model.state.debug[i] = lrintf(degrees(_model.state.gyro[i])); - } - if(_model.accelActive()) - { - _model.state.gyroImu.set(i, _model.state.gyroImuFilter[i].update(_model.state.gyro[i])); - } - } -#else for(size_t i = 0; i < 3; ++i) { _model.setDebug(DEBUG_GYRO_RAW, i, _model.state.gyroRaw[i]); @@ -231,7 +155,6 @@ class GyroSensor: public BaseSensor { _model.state.gyroImu = Utils::applyFilter(_model.state.gyroImuFilter, _model.state.gyro); } -#endif return 1; } @@ -272,15 +195,14 @@ class GyroSensor: public BaseSensor if(!_model.gyroActive()) return; if(_model.state.loopTimer.rate < DynamicFilterConfig::MIN_FREQ) return; - Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); - - const float q = _model.config.dynamicFilter.q * 0.01; - const size_t peakCount = _model.config.dynamicFilter.width; - bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; - bool update = _model.state.dynamicFilterTimer.check(); - if(_dyn_notch_enabled || _dyn_notch_debug) { + Stats::Measure measure(_model.state.stats, COUNTER_GYRO_FFT); + + const float q = _model.config.dynamicFilter.q * 0.01; + bool feed = _model.state.loopTimer.iteration % _dyn_notch_denom == 0; + bool update = _model.state.dynamicFilterTimer.check(); + for(size_t i = 0; i < 3; ++i) { #ifdef ESPFC_DSP @@ -303,7 +225,7 @@ class GyroSensor: public BaseSensor } if(_dyn_notch_enabled && status) { - for(size_t p = 0; p < peakCount; p++) + for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) { float freq = _fft[i].peaks[p].freq; if(freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) @@ -333,7 +255,7 @@ class GyroSensor: public BaseSensor { if(freq >= _model.config.dynamicFilter.min_freq && freq <= _model.config.dynamicFilter.max_freq) { - for(size_t p = 0; p < peakCount; p++) + for(size_t p = 0; p < (size_t)_model.config.dynamicFilter.width; p++) { size_t x = (p + i) % 3; int harmonic = (p / 3) + 1; diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index 3405f6d3..acadcb26 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -98,9 +98,9 @@ class Stats if(!timer.check()) return; for(size_t i = 0; i < COUNTER_COUNT; i++) { - _avg[i] = (float)_sum[i] / timer.delta; + _avg[i] = (float)(_sum[i] + (_count[i] >> 1)) / timer.delta; _freq[i] = (float)_count[i] * 1e6 / timer.delta; - _real[i] = _count[i] > 0 ? ((float)_sum[i] / _count[i]) : 0.0f; + _real[i] = _count[i] > 0 ? ((float)(_sum[i] + (_count[i] >> 1)) / _count[i]) : 0.0f; _sum[i] = 0; _count[i] = 0; } @@ -112,11 +112,11 @@ class Stats } /** - * @brief Get the Time of counter normalized to one ms + * @brief Get the Time of counter normalized to 1 ms */ float getTime(StatCounter c) const { - return _avg[c] * timer.interval * 0.001f; + return _avg[c] * 1000.0f; } float getReal(StatCounter c) const From 4ccd2e3dc0db3cce72b62435a9f2ac10608e7757 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Sun, 25 Feb 2024 23:36:33 +0100 Subject: [PATCH 26/28] blackbox update for rpm filter --- lib/Espfc/src/Blackbox.h | 18 +++++++++-- lib/Espfc/src/Model.h | 7 ++-- lib/Espfc/src/Output/Mixer.h | 10 ++---- lib/Espfc/src/Sensor/GyroSensor.h | 1 + lib/betaflight/src/blackbox/blackbox.c | 45 +++++++++++++++++--------- lib/betaflight/src/cli/settings.h | 0 lib/betaflight/src/platform.c | 4 +++ lib/betaflight/src/platform.h | 23 +++++++++++-- 8 files changed, 78 insertions(+), 30 deletions(-) create mode 100644 lib/betaflight/src/cli/settings.h diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index 073393e5..fbd64b4e 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -169,7 +169,7 @@ bool areMotorsRunning(void) return _model_ptr->areMotorsRunning(); } -uint16_t getDshotTelemetry(uint8_t i) +uint16_t getDshotErpm(uint8_t i) { return _model_ptr->state.outputTelemetryErpm[i]; } @@ -257,6 +257,12 @@ class Blackbox cp->iterm_relax = _model.config.itermRelax; cp->iterm_relax_type = 1; cp->iterm_relax_cutoff = _model.config.itermRelaxCutoff; + cp->dterm_lpf1_dyn_expo = 5; + cp->tpa_low_rate = 20; + cp->tpa_low_breakpoint = 1050; + cp->tpa_low_always = 0; + cp->ez_landing_threshold = 25; + cp->ez_landing_limit = 5; rcControlsConfigMutable()->deadband = _model.config.input.deadband; rcControlsConfigMutable()->yaw_deadband = _model.config.input.deadband; @@ -266,12 +272,14 @@ class Blackbox gyroConfigMutable()->gyro_lpf1_static_hz = _model.config.gyroFilter.freq; gyroConfigMutable()->gyro_lpf1_dyn_min_hz = _model.config.gyroDynLpfFilter.cutoff; gyroConfigMutable()->gyro_lpf1_dyn_max_hz = _model.config.gyroDynLpfFilter.freq; + gyroConfigMutable()->gyro_lpf1_dyn_expo = 5; gyroConfigMutable()->gyro_lpf2_type = _model.config.gyroFilter2.type; gyroConfigMutable()->gyro_lpf2_static_hz = _model.config.gyroFilter2.freq; gyroConfigMutable()->gyro_soft_notch_cutoff_1 = _model.config.gyroNotch1Filter.cutoff; gyroConfigMutable()->gyro_soft_notch_hz_1 = _model.config.gyroNotch1Filter.freq; gyroConfigMutable()->gyro_soft_notch_cutoff_2 = _model.config.gyroNotch2Filter.cutoff; gyroConfigMutable()->gyro_soft_notch_hz_2 = _model.config.gyroNotch2Filter.freq; + gyroConfigMutable()->gyro_sync_denom = 1; dynNotchConfigMutable()->dyn_notch_count = _model.config.dynamicFilter.width; dynNotchConfigMutable()->dyn_notch_q = _model.config.dynamicFilter.q; @@ -293,9 +301,10 @@ class Blackbox motorConfigMutable()->dev.useDshotTelemetry = _model.config.output.dshotTelemetry; motorConfigMutable()->motorPoleCount = _model.config.output.motorPoles; - gyroConfigMutable()->gyro_sync_denom = 1; pidConfigMutable()->pid_process_denom = _model.config.loopSync; + mixerConfigMutable()->mixer_type = 0; + if(_model.accelActive()) enabledSensors |= SENSOR_ACC; if(_model.magActive()) enabledSensors |= SENSOR_MAG; if(_model.baroActive()) enabledSensors |= SENSOR_BARO; @@ -333,8 +342,11 @@ class Blackbox rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilterHarmonics; rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilterQ; rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilterMinFreq; - rpmFilterConfigMutable()->rpm_filter_fade_range_hz = 0; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = 50; rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilterFreqLpf; + rpmFilterConfigMutable()->rpm_filter_weights[0] = 100; + rpmFilterConfigMutable()->rpm_filter_weights[1] = 100; + rpmFilterConfigMutable()->rpm_filter_weights[2] = 100; blackboxInit(); diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 96478fc5..1de4d519 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -172,8 +172,11 @@ class Model bool armingDisabled() const /* IRAM_ATTR */ { - //return false; +#if defined(ESPFC_DEV_PRESET_UNSAFE_ARMING) + return false; +#else return state.armingDisabledFlags != 0; +#endif } void setArmingDisabled(ArmingDisabledFlags flag, bool value) @@ -492,7 +495,7 @@ class Model for(size_t n = 0; n < config.rpmFilterHarmonics; n++) { int center = Math::mapi(m * RPM_FILTER_HARMONICS_MAX + n, 0, RPM_FILTER_MOTOR_MAX * config.rpmFilterHarmonics, config.rpmFilterMinFreq, gyroFilterRate / 2); - state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center - 10), gyroFilterRate); + state.rpmFilter[m][n][i].begin(FilterConfig(FILTER_NOTCH_DF1, center, center * 0.98f), gyroFilterRate); } } if(magActive()) diff --git a/lib/Espfc/src/Output/Mixer.h b/lib/Espfc/src/Output/Mixer.h index 3e8eab5f..0dbb426a 100644 --- a/lib/Espfc/src/Output/Mixer.h +++ b/lib/Espfc/src/Output/Mixer.h @@ -291,9 +291,7 @@ class Mixer for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { - if(i >= RPM_FILTER_MOTOR_MAX) continue; - const OutputChannelConfig& och = _model.config.output.channel[i]; - if(och.servo) continue; + if(_model.config.output.channel[i].servo) continue; uint32_t value = _motor->telemetry(i); value = EscDriver::gcrToRawValue(value); @@ -339,18 +337,14 @@ class Mixer default: value = EscDriver::convertToValue(value); _model.state.outputTelemetryErpm[i] = EscDriver::convertToErpm(value); - if(_model.config.debugMode == DEBUG_DSHOT_RPM_TELEMETRY) - { - _model.state.debug[i] = _model.state.outputTelemetryErpm[i]; - } _model.state.outputTelemetryRpm[i] = erpmToRpm(_model.state.outputTelemetryErpm[i]); + _model.setDebug(DEBUG_DSHOT_RPM_TELEMETRY, i, _model.state.outputTelemetryErpm[i]); break; } } for(size_t i = 0; i < OUTPUT_CHANNELS; i++) { - if(i >= RPM_FILTER_MOTOR_MAX) continue; _model.state.outputTelemetryFreq[i] = _model.state.rpmFreqFilter[i].update(erpmToHz(_model.state.outputTelemetryErpm[i])); } diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index a348995d..fcc92ab6 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -187,6 +187,7 @@ class GyroSensor: public BaseSensor } } } + _model.setDebug(DEBUG_RPM_FILTER, m, lrintf(_model.state.outputTelemetryFreq[m])); } } diff --git a/lib/betaflight/src/blackbox/blackbox.c b/lib/betaflight/src/blackbox/blackbox.c index bd5c1950..0ef8467c 100644 --- a/lib/betaflight/src/blackbox/blackbox.c +++ b/lib/betaflight/src/blackbox/blackbox.c @@ -45,6 +45,8 @@ #include "config/config.h" #include "config/feature.h" +#include "cli/settings.h" + #include "drivers/compass/compass.h" #include "drivers/sensor.h" #include "drivers/time.h" @@ -256,14 +258,15 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"servo", 5, UNSIGNED, .Ipredict = PREDICT(1500), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(TRICOPTER)}, #ifdef USE_DSHOT_TELEMETRY - {"eRPM(/100)", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_1_HAS_RPM)}, - {"eRPM(/100)", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_2_HAS_RPM)}, - {"eRPM(/100)", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_3_HAS_RPM)}, - {"eRPM(/100)", 3, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_4_HAS_RPM)}, - {"eRPM(/100)", 4, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_5_HAS_RPM)}, - {"eRPM(/100)", 5, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_6_HAS_RPM)}, - {"eRPM(/100)", 6, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_7_HAS_RPM)}, - {"eRPM(/100)", 7, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_8_HAS_RPM)}, + // eRPM / 100 + {"eRPM", 0, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_1_HAS_RPM)}, + {"eRPM", 1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_2_HAS_RPM)}, + {"eRPM", 2, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_3_HAS_RPM)}, + {"eRPM", 3, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_4_HAS_RPM)}, + {"eRPM", 4, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_5_HAS_RPM)}, + {"eRPM", 5, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_6_HAS_RPM)}, + {"eRPM", 6, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_7_HAS_RPM)}, + {"eRPM", 7, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MOTOR_8_HAS_RPM)}, #endif /* USE_DSHOT_TELEMETRY */ }; @@ -1164,7 +1167,7 @@ static void loadMainState(timeUs_t currentTimeUs) #ifdef USE_DSHOT_TELEMETRY for (int i = 0; i < motorCount; i++) { - blackboxCurrent->erpm[i] = getDshotTelemetry(i); + blackboxCurrent->erpm[i] = getDshotErpm(i); } #endif @@ -1380,7 +1383,7 @@ static bool blackboxWriteSysinfo(void) } else { xmitState.headerIndex += 2; // Skip the next two vbat fields too } - ); + ); BLACKBOX_PRINT_HEADER_LINE("vbatcellvoltage", "%u,%u,%u", batteryConfig()->vbatmincellvoltage, batteryConfig()->vbatwarningcellvoltage, @@ -1391,7 +1394,7 @@ static bool blackboxWriteSysinfo(void) if (batteryConfig()->currentMeterSource == CURRENT_METER_ADC) { blackboxPrintfHeaderLine("currentSensor", "%d,%d",currentSensorADCConfig()->offset, currentSensorADCConfig()->scale); } - ); + ); BLACKBOX_PRINT_HEADER_LINE("looptime", "%d", gyro.sampleLooptime); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom", "%d", 1); @@ -1401,6 +1404,13 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_MODE, "%d", currentPidProfile->tpa_mode); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_RATE, "%d", currentPidProfile->tpa_rate); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_BREAKPOINT, "%d", currentPidProfile->tpa_breakpoint); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_RATE, "%d", currentPidProfile->tpa_low_rate); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_BREAKPOINT, "%d", currentPidProfile->tpa_low_breakpoint); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_TPA_LOW_ALWAYS, "%d", currentPidProfile->tpa_low_always); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MIXER_TYPE, "%s", lookupTableMixerType[mixerConfig()->mixer_type]); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_THRESHOLD, "%d", currentPidProfile->ez_landing_threshold); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_EZ_LANDING_LIMIT, "%d", currentPidProfile->ez_landing_limit); + BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d", currentControlRateProfile->rcRates[ROLL], currentControlRateProfile->rcRates[PITCH], currentControlRateProfile->rcRates[YAW]); @@ -1438,6 +1448,7 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_DYN_LPF BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_dyn_hz", "%d,%d", currentPidProfile->dterm_lpf1_dyn_min_hz, currentPidProfile->dterm_lpf1_dyn_max_hz); + BLACKBOX_PRINT_HEADER_LINE("dterm_lpf1_dyn_expo", "%d", currentPidProfile->dterm_lpf1_dyn_expo); #endif BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF2_TYPE, "%d", currentPidProfile->dterm_lpf2_type); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DTERM_LPF2_STATIC_HZ, "%d", currentPidProfile->dterm_lpf2_static_hz); @@ -1496,6 +1507,7 @@ static bool blackboxWriteSysinfo(void) #ifdef USE_DYN_LPF BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_hz", "%d,%d", gyroConfig()->gyro_lpf1_dyn_min_hz, gyroConfig()->gyro_lpf1_dyn_max_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf1_dyn_expo", "%d", gyroConfig()->gyro_lpf1_dyn_expo); #endif BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_TYPE, "%d", gyroConfig()->gyro_lpf2_type); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_LPF2_STATIC_HZ, "%d", gyroConfig()->gyro_lpf2_static_hz); @@ -1516,6 +1528,9 @@ static bool blackboxWriteSysinfo(void) #endif #ifdef USE_RPM_FILTER BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_HARMONICS, "%d", rpmFilterConfig()->rpm_filter_harmonics); + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_WEIGHTS, "%d,%d,%d", rpmFilterConfig()->rpm_filter_weights[0], + rpmFilterConfig()->rpm_filter_weights[1], + rpmFilterConfig()->rpm_filter_weights[2]); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_Q, "%d", rpmFilterConfig()->rpm_filter_q); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_MIN_HZ, "%d", rpmFilterConfig()->rpm_filter_min_hz); BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ, "%d", rpmFilterConfig()->rpm_filter_fade_range_hz); @@ -1608,13 +1623,13 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_USE_3D_SPEED, "%d", gpsConfig()->gps_use_3d_speed) #ifdef USE_GPS_RESCUE - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minRescueDth) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MIN_START_DIST, "%d", gpsRescueConfig()->minStartDistM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ALT_MODE, "%d", gpsRescueConfig()->altitudeMode) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->rescueAltitudeBufferM) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_INITIAL_CLIMB, "%d", gpsRescueConfig()->initialClimbM) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ASCEND_RATE, "%d", gpsRescueConfig()->ascendRate) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->initialAltitudeM) - BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_SPEED, "%d", gpsRescueConfig()->rescueGroundspeed) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_RETURN_ALT, "%d", gpsRescueConfig()->returnAltitudeM) + BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_GROUND_SPEED, "%d", gpsRescueConfig()->groundSpeedCmS) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_MAX_RESCUE_ANGLE, "%d", gpsRescueConfig()->maxRescueAngle) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_ROLL_MIX, "%d", gpsRescueConfig()->rollMix) BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GPS_RESCUE_PITCH_CUTOFF, "%d", gpsRescueConfig()->pitchCutoffHz) diff --git a/lib/betaflight/src/cli/settings.h b/lib/betaflight/src/cli/settings.h new file mode 100644 index 00000000..e69de29b diff --git a/lib/betaflight/src/platform.c b/lib/betaflight/src/platform.c index f318e982..9440c7ff 100644 --- a/lib/betaflight/src/platform.c +++ b/lib/betaflight/src/platform.c @@ -69,6 +69,10 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS]; uint32_t targetPidLooptime; float rcCommand[4]; +const char* const lookupTableMixerType[] = { + "LEGACY", "LINEAR", "DYNAMIC", "EZLANDING", +}; + int gcd(int num, int denom) { if (denom == 0) return num; diff --git a/lib/betaflight/src/platform.h b/lib/betaflight/src/platform.h index f494418d..07591218 100644 --- a/lib/betaflight/src/platform.h +++ b/lib/betaflight/src/platform.h @@ -381,8 +381,11 @@ typedef enum mixerMode typedef struct mixerConfig_s { uint8_t mixerMode; bool yaw_motors_reversed; + uint8_t mixer_type; } mixerConfig_t; +extern const char * const lookupTableMixerType[]; + PG_DECLARE(mixerConfig_t, mixerConfig); typedef struct motorDevConfig_s { @@ -405,9 +408,11 @@ typedef struct motorConfig_s { PG_DECLARE(motorConfig_t, motorConfig); +#define RPM_FILTER_HARMONICS_MAX 3 + typedef struct rpmFilterConfig_s { uint8_t rpm_filter_harmonics; // how many harmonics should be covered with notches? 0 means filter off - //uint8_t rpm_filter_weights[RPM_FILTER_HARMONICS_MAX]; // effect or "weight" (0% - 100%) of each RPM filter harmonic + uint8_t rpm_filter_weights[RPM_FILTER_HARMONICS_MAX]; // effect or "weight" (0% - 100%) of each RPM filter harmonic uint8_t rpm_filter_min_hz; // minimum frequency of the notches uint16_t rpm_filter_fade_range_hz; // range in which to gradually turn off notches down to minHz uint16_t rpm_filter_q; // q of the notches @@ -636,6 +641,7 @@ typedef struct pidProfile_s { uint16_t dterm_lpf1_static_hz; // Static Dterm lowpass 1 filter cutoff value in hz uint16_t dterm_lpf1_dyn_min_hz; // Dterm lowpass filter 1 min hz when in dynamic mode uint16_t dterm_lpf1_dyn_max_hz; // Dterm lowpass filter 1 max hz when in dynamic mode + uint8_t dterm_lpf1_dyn_expo; // set the curve for dynamic dterm lowpass filter uint8_t itermWindupPointPercent; // Experimental ITerm windup threshold, percent motor saturation uint16_t pidSumLimit; uint16_t pidSumLimitYaw; @@ -673,6 +679,11 @@ typedef struct pidProfile_s { uint8_t iterm_relax_type; // Specifies type of relax algorithm uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax; // Enable iterm suppression during stick input + uint8_t tpa_low_rate; // Percent reduction in P or D at zero throttle + uint16_t tpa_low_breakpoint; // Breakpoint where lower TPA is deactivated + uint8_t tpa_low_always; // off, on - if OFF then low TPA is only active until tpa_low_breakpoint is reached the first time + uint8_t ez_landing_threshold; // Threshold stick position below which motor output is limited + uint8_t ez_landing_limit; // Maximum motor output when all sticks centred and throttle zero } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles); @@ -905,6 +916,7 @@ typedef struct gyroConfig_s { uint16_t gyro_lpf1_static_hz; uint16_t gyro_lpf1_dyn_min_hz; uint16_t gyro_lpf1_dyn_max_hz; + uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter uint8_t gyro_lpf2_type; uint16_t gyro_lpf2_static_hz; } gyroConfig_t; @@ -967,7 +979,7 @@ float mixerGetThrottle(void); bool isRssiConfigured(void); int16_t getMotorOutputLow(); int16_t getMotorOutputHigh(); -uint16_t getDshotTelemetry(uint8_t i); +uint16_t getDshotErpm(uint8_t i); /* FAILSAFE END */ #define PARAM_NAME_GYRO_HARDWARE_LPF "gyro_hardware_lpf" @@ -1004,7 +1016,13 @@ uint16_t getDshotTelemetry(uint8_t i); #define PARAM_NAME_RATES_TYPE "rates_type" #define PARAM_NAME_TPA_RATE "tpa_rate" #define PARAM_NAME_TPA_BREAKPOINT "tpa_breakpoint" +#define PARAM_NAME_TPA_LOW_RATE "tpa_low_rate" +#define PARAM_NAME_TPA_LOW_BREAKPOINT "tpa_low_breakpoint" +#define PARAM_NAME_TPA_LOW_ALWAYS "tpa_low_always" #define PARAM_NAME_TPA_MODE "tpa_mode" +#define PARAM_NAME_MIXER_TYPE "mixer_type" +#define PARAM_NAME_EZ_LANDING_THRESHOLD "ez_landing_threshold" +#define PARAM_NAME_EZ_LANDING_LIMIT "ez_landing_limit" #define PARAM_NAME_THROTTLE_LIMIT_TYPE "throttle_limit_type" #define PARAM_NAME_THROTTLE_LIMIT_PERCENT "throttle_limit_percent" #define PARAM_NAME_GYRO_CAL_ON_FIRST_ARM "gyro_cal_on_first_arm" @@ -1066,6 +1084,7 @@ uint16_t getDshotTelemetry(uint8_t i); #define PARAM_NAME_SIMPLIFIED_GYRO_FILTER_MULTIPLIER "simplified_gyro_filter_multiplier" #define PARAM_NAME_DEBUG_MODE "debug_mode" #define PARAM_NAME_RPM_FILTER_HARMONICS "rpm_filter_harmonics" +#define PARAM_NAME_RPM_FILTER_WEIGHTS "rpm_filter_weights" #define PARAM_NAME_RPM_FILTER_Q "rpm_filter_q" #define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz" #define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz" From d84a2860bb8694dbb8508dc349afc0fcba69e942 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 26 Feb 2024 22:50:44 +0100 Subject: [PATCH 27/28] optimize rpm filter updates + freq fading + filter weight --- lib/Espfc/src/Blackbox.h | 8 +-- lib/Espfc/src/Cli.h | 6 ++- lib/Espfc/src/Filter.h | 89 ++++++++++++++++++++++++++++--- lib/Espfc/src/ModelConfig.h | 7 +++ lib/Espfc/src/Sensor/GyroSensor.h | 56 +++++++++++++------ lib/Espfc/src/Stats.h | 2 + 6 files changed, 139 insertions(+), 29 deletions(-) diff --git a/lib/Espfc/src/Blackbox.h b/lib/Espfc/src/Blackbox.h index fbd64b4e..8267c3b5 100644 --- a/lib/Espfc/src/Blackbox.h +++ b/lib/Espfc/src/Blackbox.h @@ -342,11 +342,11 @@ class Blackbox rpmFilterConfigMutable()->rpm_filter_harmonics = _model.config.rpmFilterHarmonics; rpmFilterConfigMutable()->rpm_filter_q = _model.config.rpmFilterQ; rpmFilterConfigMutable()->rpm_filter_min_hz = _model.config.rpmFilterMinFreq; - rpmFilterConfigMutable()->rpm_filter_fade_range_hz = 50; + rpmFilterConfigMutable()->rpm_filter_fade_range_hz = _model.config.rpmFilterFade; rpmFilterConfigMutable()->rpm_filter_lpf_hz = _model.config.rpmFilterFreqLpf; - rpmFilterConfigMutable()->rpm_filter_weights[0] = 100; - rpmFilterConfigMutable()->rpm_filter_weights[1] = 100; - rpmFilterConfigMutable()->rpm_filter_weights[2] = 100; + rpmFilterConfigMutable()->rpm_filter_weights[0] = _model.config.rpmFilterWeights[0]; + rpmFilterConfigMutable()->rpm_filter_weights[1] = _model.config.rpmFilterWeights[1]; + rpmFilterConfigMutable()->rpm_filter_weights[2] = _model.config.rpmFilterWeights[2]; blackboxInit(); diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index 0a4e256b..e1c581dd 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -410,7 +410,11 @@ class Cli Param(PSTR("gyro_rpm_harmonics"), &c.rpmFilterHarmonics), Param(PSTR("gyro_rpm_q"), &c.rpmFilterQ), Param(PSTR("gyro_rpm_min_freq"), &c.rpmFilterMinFreq), - Param(PSTR("gyro_rpm_freq_lpf"), &c.rpmFilterFreqLpf), + Param(PSTR("gyro_rpm_fade"), &c.rpmFilterFade), + Param(PSTR("gyro_rpm_weight_1"), &c.rpmFilterWeights[0]), + Param(PSTR("gyro_rpm_weight_2"), &c.rpmFilterWeights[1]), + Param(PSTR("gyro_rpm_weight_3"), &c.rpmFilterWeights[2]), + Param(PSTR("gyro_rpm_tlm_lpf_freq"), &c.rpmFilterFreqLpf), Param(PSTR("gyro_offset_x"), &c.gyroBias[0]), Param(PSTR("gyro_offset_y"), &c.gyroBias[1]), Param(PSTR("gyro_offset_z"), &c.gyroBias[2]), diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h index 5ef825d3..6ab1f1b2 100644 --- a/lib/Espfc/src/Filter.h +++ b/lib/Espfc/src/Filter.h @@ -87,6 +87,11 @@ class FilterStatePt1 { v = 0.f; } + void reconfigure(const FilterStatePt1& from) + { + k = from.k; + } + void init(float rate, float freq) { k = pt1Gain(rate, freq); @@ -113,6 +118,10 @@ class FilterStateFir2 { { } + void reconfigure(const FilterStateFir2& from) + { + } + float update(float n) { v[0] = (n + v[1]) * 0.5f; @@ -173,6 +182,15 @@ class FilterStateBiquad { this->a2 = a2 / a0; } + void reconfigure(const FilterStateBiquad& from) + { + b0 = from.b0; + b1 = from.b1; + b2 = from.b2; + a1 = from.a1; + a2 = from.a2; + } + float update(float n) { // DF2 @@ -212,6 +230,10 @@ class FilterStateMedian { } + void reconfigure(const FilterStateMedian& from) + { + } + float update(float n) { v[0] = v[1]; @@ -241,6 +263,11 @@ class FilterStatePt2 { k = pt1Gain(rate, freq * correction); } + void reconfigure(const FilterStatePt2& from) + { + k = from.k; + } + float update(float n) { v[0] += k * (n - v[0]); @@ -265,6 +292,11 @@ class FilterStatePt3 { k = pt1Gain(rate, freq * correction); } + void reconfigure(const FilterStatePt3& from) + { + k = from.k; + } + float update(float n) { v[0] += k * (n - v[0]); @@ -304,7 +336,7 @@ class Filter case FILTER_BPF: return _state.bq.update(v); case FILTER_NOTCH_DF1: - return _state.bq.updateDF1(v); + return _output_weight * _state.bq.updateDF1(v) + _input_weight * v; case FILTER_FIR2: return _state.fir2.update(v); case FILTER_MEDIAN3: @@ -353,9 +385,9 @@ class Filter reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate); } - void reconfigure(int16_t freq, int16_t cutoff, float q) + void reconfigure(int16_t freq, int16_t cutoff, float q, float weight = 1.0f) { - reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q); + reconfigure(FilterConfig((FilterType)_conf.type, freq, cutoff), _rate, q, weight); } void reconfigure(const FilterConfig& config, int rate) @@ -365,22 +397,23 @@ class Filter switch(_conf.type) { case FILTER_BIQUAD: - reconfigure(config, rate, 0.70710678118f); // 1.0f / sqrtf(2.0f); // quality factor for butterworth lpf + reconfigure(config, rate, 0.70710678118f, 1.0f); // 1.0f / sqrtf(2.0f); // quality factor for butterworth lpf break; case FILTER_NOTCH: case FILTER_NOTCH_DF1: case FILTER_BPF: - reconfigure(config, rate, getNotchQApprox(config.freq, config.cutoff)); + reconfigure(config, rate, getNotchQApprox(config.freq, config.cutoff), 1.0f); break; default: - reconfigure(config, rate, 0.f); + reconfigure(config, rate, 0.0f, 1.0f); } } - void reconfigure(const FilterConfig& config, int rate, float q) + void reconfigure(const FilterConfig& config, int rate, float q, float weight) { _rate = rate; _conf = config.sanitize(_rate); + setWeight(weight); switch(_conf.type) { case FILTER_PT1: @@ -414,6 +447,46 @@ class Filter } } + void reconfigure(const Filter& filter) + { + _rate = filter._rate; + _conf = filter._conf; + _output_weight = filter._output_weight; + _input_weight = filter._input_weight; + switch(_conf.type) + { + case FILTER_PT1: + _state.pt1.reconfigure(filter._state.pt1); + break; + case FILTER_BIQUAD: + case FILTER_NOTCH: + case FILTER_NOTCH_DF1: + _state.bq.reconfigure(filter._state.bq); + break; + case FILTER_FIR2: + _state.fir2.reconfigure(filter._state.fir2); + break; + case FILTER_MEDIAN3: + _state.median.reconfigure(filter._state.median); + break; + case FILTER_PT2: + _state.pt2.reconfigure(filter._state.pt2); + break; + case FILTER_PT3: + _state.pt3.reconfigure(filter._state.pt3); + break; + case FILTER_NONE: + default: + ; + } + } + + void setWeight(float weight) + { + _output_weight = std::max(0.0f, std::min(weight, 1.0)); + _input_weight = 1.0f - _output_weight; + } + float getNotchQApprox(float freq, float cutoff) { return ((float)(cutoff * freq) / ((float)(freq - cutoff) * (float)(freq + cutoff))); @@ -439,6 +512,8 @@ class Filter FilterStatePt2 pt2; FilterStatePt3 pt3; } _state; + float _input_weight; + float _output_weight; }; } diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 42604416..1cef6708 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -647,10 +647,13 @@ class ModelConfig WirelessConfig wireless; DynamicFilterConfig dynamicFilter; + uint8_t rpmFilterHarmonics; uint8_t rpmFilterMinFreq; int16_t rpmFilterQ; uint8_t rpmFilterFreqLpf; + uint8_t rpmFilterWeights[RPM_FILTER_HARMONICS_MAX]; + uint8_t rpmFilterFade; ModelConfig() { @@ -756,6 +759,10 @@ class ModelConfig rpmFilterHarmonics = 3; rpmFilterMinFreq = 100; rpmFilterQ = 500; + rpmFilterFade = 50; + rpmFilterWeights[0] = 100; + rpmFilterWeights[1] = 100; + rpmFilterWeights[2] = 100; rpmFilterFreqLpf = 150; gyroFilter3 = FilterConfig(FILTER_PT1, 150); diff --git a/lib/Espfc/src/Sensor/GyroSensor.h b/lib/Espfc/src/Sensor/GyroSensor.h index fcc92ab6..9c8fcde0 100644 --- a/lib/Espfc/src/Sensor/GyroSensor.h +++ b/lib/Espfc/src/Sensor/GyroSensor.h @@ -38,11 +38,20 @@ class GyroSensor: public BaseSensor _sma.begin(_model.config.loopSync); _dyn_notch_denom = std::max((uint32_t)1, _model.state.loopTimer.rate / 1000); _dyn_notch_sma.begin(_dyn_notch_denom); - _dyn_notch_enabled = _model.isActive(FEATURE_DYNAMIC_FILTER) && _model.config.dynamicFilter.width > 0 && _model.state.loopTimer.rate >= DynamicFilterConfig::MIN_FREQ; _dyn_notch_debug = _model.config.debugMode == DEBUG_FFT_FREQ || _model.config.debugMode == DEBUG_FFT_TIME; + _rpm_enabled = _model.config.rpmFilterHarmonics > 0 && _model.config.output.dshotTelemetry; + _rpm_motor_index = 0; + _rpm_fade_inv = 1.0f / _model.config.rpmFilterFade; + _rpm_min_freq = _model.config.rpmFilterMinFreq; + _rpm_max_freq = 0.48f * _model.state.loopTimer.rate; + _rpm_q = _model.config.rpmFilterQ * 0.01f; + for(size_t i = 0; i < RPM_FILTER_HARMONICS_MAX; i++) + { + _rpm_weights[i] = Math::clamp(0.01f * _model.config.rpmFilterWeights[i], 0.0f, 1.0f); + } for(size_t i = 0; i < 3; i++) { #ifdef ESPFC_DSP @@ -169,25 +178,32 @@ class GyroSensor: public BaseSensor { if(!_rpm_enabled) return; - const float minFreq = _model.config.rpmFilterMinFreq; - const float maxFreq = 0.48f * _model.state.loopTimer.rate; - const float q = _model.config.rpmFilterQ * 0.01f; - // TODO: optimize, to update filters in steps - for(size_t m = 0; m < RPM_FILTER_MOTOR_MAX; m++) + Stats::Measure measure(_model.state.stats, COUNTER_RPM_UPDATE); + + const float motorFreq = _model.state.outputTelemetryFreq[_rpm_motor_index]; + for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) { - const float motorFreq = _model.state.outputTelemetryFreq[m]; - for(size_t n = 0; n < _model.config.rpmFilterHarmonics; n++) + const float freq = Math::clamp(motorFreq * (n + 1), _rpm_min_freq, _rpm_max_freq); + const float freqMargin = freq - _rpm_min_freq; + float weight = _rpm_weights[n]; + if(freqMargin < _model.config.rpmFilterFade) { - const float freq = motorFreq * (n + 1); - if(minFreq < freq && freq < maxFreq) - { - for(size_t i = 0; i < 3; ++i) - { - _model.state.rpmFilter[m][n][i].reconfigure(freq, freq, q); - } - } + weight *= freqMargin * _rpm_fade_inv; + } + _model.state.rpmFilter[_rpm_motor_index][n][0].reconfigure(freq, freq, _rpm_q, weight); + for(size_t i = 1; i < 3; ++i) + { + // copy coefs from roll to pitch and yaw + _model.state.rpmFilter[_rpm_motor_index][n][i].reconfigure(_model.state.rpmFilter[_rpm_motor_index][n][0]); } - _model.setDebug(DEBUG_RPM_FILTER, m, lrintf(_model.state.outputTelemetryFreq[m])); + } + + _model.setDebug(DEBUG_RPM_FILTER, _rpm_motor_index, lrintf(_model.state.outputTelemetryFreq[_rpm_motor_index])); + + _rpm_motor_index++; + if(_rpm_motor_index >= RPM_FILTER_MOTOR_MAX) + { + _rpm_motor_index = 0; } } @@ -315,6 +331,12 @@ class GyroSensor: public BaseSensor bool _dyn_notch_enabled; bool _dyn_notch_debug; bool _rpm_enabled; + size_t _rpm_motor_index; + float _rpm_weights[3]; + float _rpm_fade_inv; + float _rpm_min_freq; + float _rpm_max_freq; + float _rpm_q; Model& _model; Device::GyroDevice * _gyro; diff --git a/lib/Espfc/src/Stats.h b/lib/Espfc/src/Stats.h index acadcb26..9b5e3fcd 100644 --- a/lib/Espfc/src/Stats.h +++ b/lib/Espfc/src/Stats.h @@ -10,6 +10,7 @@ enum StatCounter : int8_t { COUNTER_GYRO_READ, COUNTER_GYRO_FILTER, COUNTER_GYRO_FFT, + COUNTER_RPM_UPDATE, COUNTER_ACCEL_READ, COUNTER_ACCEL_FILTER, COUNTER_MAG_READ, @@ -169,6 +170,7 @@ class Stats case COUNTER_GYRO_READ: return PSTR(" gyro_r"); case COUNTER_GYRO_FILTER: return PSTR(" gyro_f"); case COUNTER_GYRO_FFT: return PSTR(" gyro_a"); + case COUNTER_RPM_UPDATE: return PSTR(" rpm_u"); case COUNTER_ACCEL_READ: return PSTR(" acc_r"); case COUNTER_ACCEL_FILTER: return PSTR(" acc_f"); case COUNTER_MAG_READ: return PSTR(" mag_r"); From 61424e51d6240b63de04f79a6787296c8e23b0be Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 26 Feb 2024 23:34:17 +0100 Subject: [PATCH 28/28] first order filter --- lib/Espfc/src/Cli.h | 2 +- lib/Espfc/src/Filter.h | 63 ++++++++++++++++++++++++++++++++++++- lib/Espfc/src/ModelConfig.h | 4 +-- 3 files changed, 65 insertions(+), 4 deletions(-) diff --git a/lib/Espfc/src/Cli.h b/lib/Espfc/src/Cli.h index e1c581dd..0912e277 100644 --- a/lib/Espfc/src/Cli.h +++ b/lib/Espfc/src/Cli.h @@ -356,7 +356,7 @@ class Cli PSTR("DSHOT_RPM_TELEMETRY"), PSTR("RPM_FILTER"), PSTR("D_MIN"), PSTR("AC_CORRECTION"), PSTR("AC_ERROR"), PSTR("DUAL_GYRO_SCALED"), PSTR("DSHOT_RPM_ERRORS"), PSTR("CRSF_LINK_STATISTICS_UPLINK"), PSTR("CRSF_LINK_STATISTICS_PWR"), PSTR("CRSF_LINK_STATISTICS_DOWN"), PSTR("BARO"), PSTR("GPS_RESCUE_THROTTLE_PID"), PSTR("DYN_IDLE"), PSTR("FF_LIMIT"), PSTR("FF_INTERPOLATED"), PSTR("BLACKBOX_OUTPUT"), PSTR("GYRO_SAMPLE"), PSTR("RX_TIMING"), NULL }; - static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL }; + static const char* filterTypeChoices[] = { PSTR("PT1"), PSTR("BIQUAD"), PSTR("PT2"), PSTR("PT3"), PSTR("NOTCH"), PSTR("NOTCH_DF1"), PSTR("BPF"), PSTR("FO"), PSTR("FIR2"), PSTR("MEDIAN3"), PSTR("NONE"), NULL }; static const char* alignChoices[] = { PSTR("DEFAULT"), PSTR("CW0"), PSTR("CW90"), PSTR("CW180"), PSTR("CW270"), PSTR("CW0_FLIP"), PSTR("CW90_FLIP"), PSTR("CW180_FLIP"), PSTR("CW270_FLIP"), PSTR("CUSTOM"), NULL }; static const char* mixerTypeChoices[] = { PSTR("NONE"), PSTR("TRI"), PSTR("QUADP"), PSTR("QUADX"), PSTR("BI"), PSTR("GIMBAL"), PSTR("Y6"), PSTR("HEX6"), PSTR("FWING"), PSTR("Y4"), diff --git a/lib/Espfc/src/Filter.h b/lib/Espfc/src/Filter.h index 6ab1f1b2..0dabd8d4 100644 --- a/lib/Espfc/src/Filter.h +++ b/lib/Espfc/src/Filter.h @@ -34,6 +34,7 @@ enum FilterType { FILTER_NOTCH, FILTER_NOTCH_DF1, FILTER_BPF, + FILTER_FO, FILTER_FIR2, FILTER_MEDIAN3, FILTER_NONE, @@ -218,6 +219,56 @@ class FilterStateBiquad { float x1, x2, y1, y2; }; +class FilterStateFirstOrder { + public: + void reset() + { + x1 = y1 = 0; + } + + void init(float rate, float freq) + { + freq = Math::clamp(freq, 0.0f, rate * 0.48f); + + const float W = std::tan(Math::pi() * freq / rate); + + a1 = (W - 1) / (W + 1); + b1 = b0 = W / (W + 1); + } + + void reconfigure(const FilterStateFirstOrder& from) + { + b0 = from.b0; + b1 = from.b1; + a1 = from.a1; + } + + float update(float n) + { + // DF2 + const float result = b0 * n + x1; + x1 = b1 * n - a1 * result; + return result; + } + + float updateDF1(float n) + { + /* compute result */ + const float result = b0 * n + b1 * x1 - a1 * y1; + + /* shift input to x1 */ + x1 = n; + + /* shift result to y1 */ + y1 = result; + + return result; + } + + float b0, b1, a1; + float x1, y1; +}; + class FilterStateMedian { public: void reset() @@ -227,7 +278,6 @@ class FilterStateMedian { void init() { - } void reconfigure(const FilterStateMedian& from) @@ -345,6 +395,8 @@ class Filter return _state.pt2.update(v); case FILTER_PT3: return _state.pt3.update(v); + case FILTER_FO: + return _state.fo.update(v); case FILTER_NONE: default: return v; @@ -374,6 +426,8 @@ class Filter return _state.pt2.reset(); case FILTER_PT3: return _state.pt3.reset(); + case FILTER_FO: + return _state.fo.reset(); case FILTER_NONE: default: ; @@ -441,6 +495,9 @@ class Filter case FILTER_PT3: _state.pt3.init(_rate, _conf.freq); break; + case FILTER_FO: + _state.fo.init(_rate, _conf.freq); + break; case FILTER_NONE: default: ; @@ -475,6 +532,9 @@ class Filter case FILTER_PT3: _state.pt3.reconfigure(filter._state.pt3); break; + case FILTER_FO: + _state.fo.reconfigure(filter._state.fo); + break; case FILTER_NONE: default: ; @@ -511,6 +571,7 @@ class Filter FilterStateMedian median; FilterStatePt2 pt2; FilterStatePt3 pt3; + FilterStateFirstOrder fo; } _state; float _input_weight; float _output_weight; diff --git a/lib/Espfc/src/ModelConfig.h b/lib/Espfc/src/ModelConfig.h index 1cef6708..09f181bd 100644 --- a/lib/Espfc/src/ModelConfig.h +++ b/lib/Espfc/src/ModelConfig.h @@ -759,13 +759,13 @@ class ModelConfig rpmFilterHarmonics = 3; rpmFilterMinFreq = 100; rpmFilterQ = 500; - rpmFilterFade = 50; + rpmFilterFade = 30; rpmFilterWeights[0] = 100; rpmFilterWeights[1] = 100; rpmFilterWeights[2] = 100; rpmFilterFreqLpf = 150; - gyroFilter3 = FilterConfig(FILTER_PT1, 150); + gyroFilter3 = FilterConfig(FILTER_FO, 150); gyroNotch1Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off gyroNotch2Filter = FilterConfig(FILTER_NOTCH, 0, 0); // off dtermNotchFilter = FilterConfig(FILTER_NOTCH, 0, 0); // off