You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
875 lines
28 KiB
875 lines
28 KiB
/* |
|
(c) 2017 night_ghost@ykoctpa.ru |
|
|
|
based on: |
|
|
|
* Cleanflight 4way driver |
|
*/ |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
|
|
#ifdef USE_SERIAL_4WAY_BLHELI_INTERFACE |
|
|
|
#include <stdbool.h> |
|
#include <stdint.h> |
|
#include <string.h> |
|
|
|
|
|
|
|
#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
|
|
|