/* (c) 2017 night_ghost@ykoctpa.ru based on: * Cleanflight 4way driver */ #include #ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE #include #include #include #include "serial_4way.h" #ifdef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #include "serial_4way_avrootloader.h" #endif #if defined(USE_SERIAL_4WAY_SK_BOOTLOADER) #include "serial_4way_stk500v2.h" #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) 02 #define SERIAL_4WAY_PROTOCOL_VER 107 // *** 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; static escHardware_t escHardware[MAX_SUPPORTED_MOTORS]; uint8_t selected_esc; uint8_32_u DeviceInfo; #define DeviceInfoSize 4 bool isMcuConnected(void){ return (DeviceInfo.bytes[0] > 0); } bool isEscHi(uint8_t selEsc) { escHardware_t &pp = escHardware[selEsc]; return gpio_read_bit(pp.gpio_device, pp.gpio_bit) != 0; } bool isEscLo(uint8_t selEsc) { escHardware_t &pp = escHardware[selEsc]; return gpio_read_bit(pp.gpio_device, pp.gpio_bit) == 0; } void setEscHi(uint8_t selEsc) { escHardware_t &pp = escHardware[selEsc]; gpio_write_bit(pp.gpio_device, pp.gpio_bit, HIGH); } void setEscLo(uint8_t selEsc) { escHardware_t &pp = escHardware[selEsc]; gpio_write_bit(pp.gpio_device, pp.gpio_bit, LOW); } void setEscInput(uint8_t selEsc) { escHardware_t &pp = escHardware[selEsc]; gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_INPUT_PU); } void setEscOutput(uint8_t selEsc) { escHardware_t &pp = escHardware[selEsc]; gpio_set_mode(pp.gpio_device, pp.gpio_bit, GPIO_OUTPUT_PP); } uint8_t esc4wayInit(const uint8_t *output_channels, uint8_t nm) { escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); for (volatile uint8_t i = 0; i < nm; i++) { uint8_t pin = output_channels[i]; const stm32_pin_info &pp = PIN_MAP[pin]; escHardware[escCount].gpio_device = pp.gpio_device; escHardware[escCount].gpio_bit = pp.gpio_bit; setEscInput(escCount); setEscHi(escCount); escCount++; } return escCount; } void esc4wayRelease(void) { while (escCount > 0) { escCount--; // F4LightRCOutput::_set_pin_output_mode(escCount); let just reboot } } #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] == 0xF310)||(pDeviceInfo->words[0] ==0xF330) || \ (pDeviceInfo->words[0] == 0xF410) || (pDeviceInfo->words[0] == 0xF390) || \ (pDeviceInfo->words[0] == 0xF850) || (pDeviceInfo->words[0] == 0xE8B1) || \ (pDeviceInfo->words[0] == 0xE8B2)) #define ARM_DEVICE_MATCH ((pDeviceInfo->words[0] == 0x1F06) || \ (pDeviceInfo->words[0] == 0x3306) || (pDeviceInfo->words[0] == 0x3406) || (pDeviceInfo->words[0] == 0x3506)) 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 AP_HAL::UARTDriver *uart; static uint8_t ReadByte(void) { // need timeout? while (!uart->available()); return uart->read(); } static uint8_16_u CRC_in; static uint8_16_u CRCout; 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) { uart->write(b); } static void WriteByteCrc(uint8_t b) { WriteByte(b); CRCout.word = _crc_xmodem_update(CRCout.word, b); } static bool UartTxPending() { return uart->tx_pending(); } void esc4wayProcess(AP_HAL::UARTDriver *uartPort) { 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; uart = uartPort; // Start here with UART Main loop 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: { if (ParamBuf[0] < escCount) { // Channel may change here selected_esc = ParamBuf[0]; } 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); 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: case imARM_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: { 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(UartTxPending()) hal_yield(0); 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