mirror of https://github.com/ArduPilot/ardupilot
876 lines
28 KiB
C++
876 lines
28 KiB
C++
/*
|
|
(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
|