Ardupilot通过mavlink + 4way_protocol对BLHeli_s电调的读写

前言

当装机完成之后，发现电调与电机匹配不太好，或者转向相反，各种拆线很难受。BetaFlight飞控可支持通过信号线对BL电调进行读写，那么Ardupilot、PX4这两来源飞控系统呢？ 答案是支持的，本文以Ardupilot为例记录我踩的坑以及对其不能连接8位BL电调的疑惑。

上位机

1、Mission Planner

Ardupilot地面站软件，本文使用它来对参数进行修改配置

2、BLHeliSuite

BLHeli_s 8位电调地面站，可以对8位电调进行读写

3、BLHeliSuite32

BLHeli_s 32位电调地面站，可以对32位电调进行读写

Ardupilot 源码编译

建议大家在最新的4.0版本上编译，我先是在4.0验证之后 转到3.6版本上使用的。
本文只从4.0版本进行说明。

1、启动BLHeli_s支持

在目录ardupilot/libraries/AP_HAL下的AP_HAL_Board.h文件中修改HAL_SUPPORT_RCOUT_SERIAL为1

#ifndef HAL_SUPPORT_RCOUT_SERIAL
#define HAL_SUPPORT_RCOUT_SERIAL 1
#endif

2、修改打印连接信息方式

默认会通过mavink发送连接信息到地面站，如果没有连接数传的时候，USB接口被BL地面站占用了，就无法在飞控地面站MP 查看连接信息，所以修改到debug接口打印出来，不过这就需要外接一个USB转TTL模块。
在AP_BLHeli.c文件中修改宏定义，然后在下面的debug 后面添加 \n 让其换行

// #define debug(fmt, args ...) do { if (debug_level) { gcs().send_text(MAV_SEVERITY_INFO, "ESC: " fmt, ## args); } } while (0)
#define debug(fmt, args ...)  printf("ESC:" fmt, ## args)

例如

//debug("MSP_API_VERSION");
debug("MSP_API_VERSION\n");

3、编译源码

1、板子配置及编译

我使用的是Pixracer，可根据自己手上的飞控板进行对应的指令修改

./waf configure --board Pixracer 		//Pixracer就是飞控板的名称
./waf copter	//多旋翼固件编译

2、固件烧录

./waf --targets bin/arducopter --upload

参数配置

SERVO_BLH_DEBUG 1 //需要修改 SERVO_BLH_DEBUG 使能对BL电调的连接
具体参数看图

BL地面站连接

1、连接地面站

直接通过USB连接飞控，打开BL地面站，选择连接方式为Betaflight/Cleanfight

找到对应的接口然后点击Connect 连接

连接时 debug接口打印的信息如下：

2、读取电调

给电调上电，然后点击Read Setup


试着改个电机旋转方向写入测试：

到这里，Ardupilot 对BLHeli_s 32位电调的直接读写宣布完成

Ardupilot 对BLHeli_s 8位的操作不成功疑惑

1、8位地面站连接

串口输出信息：

2、尝试读取电调配置

可读取到32位电调的信息：


但是BL地面站 只能对应的使用，8位的不能看32位的，所以使用8位电调再测试看看

正常连接至地面站

读取电调：发现了 多旋翼电调配置， 并且是SiLabs 主控的，但是 没能连接上

提示说需要给电调上电，问题是电源一直给的。
说明：地面站，8位32位 电调都经过Beatflight 飞控直连测试通过，肯定了电调没有问题

3、查看Ardupilot 是否有对8位的读写接口

在BL_ConnectEx(void)函数中，发现已存在对8位的支持，0xE8B1 0xE8B2就是对应的主控为EFM8BB10、EFM8BB21

/*
  connect to a blheli ESC
 */
bool AP_BLHeli::BL_ConnectEx(void)
{
    if (blheli.connected[blheli.chan] != 0) {
        debug("Using cached interface 0x%x for %u\n", blheli.interface_mode[blheli.chan], blheli.chan);
        return true;
    }
    debug("BL_ConnectEx %u/%u at %u\n", blheli.chan, num_motors, motor_map[blheli.chan]);
    setDisconnected();
    const uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
    if (!BL_SendBuf(BootInit, 21)) {
        return false;
    }

    uint8_t BootInfo[8];
    if (!BL_ReadBuf(BootInfo, 8)) {
        return false;
    }

    // reply must start with 471
    if (strncmp((const char *)BootInfo, "471", 3) != 0) {
        blheli.ack = ACK_D_GENERAL_ERROR;        
        return false;
    }

    // extract device information
    blheli.deviceInfo[blheli.chan][2] = BootInfo[3];
    blheli.deviceInfo[blheli.chan][1] = BootInfo[4];
    blheli.deviceInfo[blheli.chan][0] = BootInfo[5];

    blheli.interface_mode[blheli.chan] = 0;
    
    uint16_t *devword = (uint16_t *)blheli.deviceInfo[blheli.chan];
    switch (*devword) {
    case 0x9307:
    case 0x930A:
    case 0x930F:
    case 0x940B:
        blheli.interface_mode[blheli.chan] = imATM_BLB;
        debug("Interface type imATM_BLB\n");
        break;
    case 0xF310:
    case 0xF330:
    case 0xF410:
    case 0xF390:
    case 0xF850:
    case 0xE8B1:
    case 0xE8B2:
        blheli.interface_mode[blheli.chan] = imSIL_BLB;
        debug("Interface type imSIL_BLB\n");
        break;
    case 0x1F06:
    case 0x3306:
    case 0x3406:
    case 0x3506:
    case 0x2B06:
    case 0x4706:
        blheli.interface_mode[blheli.chan] = imARM_BLB;
        debug("Interface type imARM_BLB\n");
        break;
    default:
        blheli.ack = ACK_D_GENERAL_ERROR;        
        debug("Unknown interface type 0x%04x", *devword);
        break;
    }
    blheli.deviceInfo[blheli.chan][3] = blheli.interface_mode[blheli.chan];
    if (blheli.interface_mode[blheli.chan] != 0) {
        blheli.connected[blheli.chan] = true;
    }
    return true;
}

查看消息记录可以看到：查到的电机数为0


具体是将函数是void AP_BLHeli::blheli_process_command(void)

/*
  process a blheli 4way command from GCS
 */
void AP_BLHeli::blheli_process_command(void)
{
    debug("BLHeli cmd 0x%02x len=%u\n", blheli.command, blheli.param_len);
    blheli.ack = ACK_OK;
    switch (blheli.command) {
    case cmd_InterfaceTestAlive: {
        debug("cmd_InterfaceTestAlive\n");
        BL_SendCMDKeepAlive();
        if (blheli.ack != ACK_OK) {
            setDisconnected();
        }
        uint8_t b = 0;
        blheli_send_reply(&b, 1);
        break;
    }
    case cmd_ProtocolGetVersion: {
        debug("cmd_ProtocolGetVersion\n");
        uint8_t buf[1];
        buf[0] = SERIAL_4WAY_PROTOCOL_VER;
        blheli_send_reply(buf, sizeof(buf));
        break;
    }
    case cmd_InterfaceGetName: {
        debug("cmd_InterfaceGetName\n");
        uint8_t buf[5] = { 4, 'A', 'R', 'D', 'U' };
        blheli_send_reply(buf, sizeof(buf));
        break;
    }
    case cmd_InterfaceGetVersion: {
        debug("cmd_InterfaceGetVersion\n");
        uint8_t buf[2] = { SERIAL_4WAY_VERSION_HI, SERIAL_4WAY_VERSION_LO };
        blheli_send_reply(buf, sizeof(buf));
        break;
    }
    case cmd_InterfaceExit: {
        debug("cmd_InterfaceExit\n");
        msp.escMode = PROTOCOL_NONE;
        uint8_t b = 0;
        blheli_send_reply(&b, 1);
        hal.rcout->serial_end();
        serial_start_ms = 0;
        if (motors_disabled) {
            motors_disabled = false;
            SRV_Channels::set_disabled_channel_mask(0);            
        }
        if (uart_locked) {
            debug("Unlocked UART\n");
            uart->lock_port(0);
            uart_locked = false;
        }
        memset(blheli.connected, 0, sizeof(blheli.connected));
        break;
    }
    case cmd_DeviceReset: {
        debug("cmd_DeviceReset(%u)\n", unsigned(blheli.buf[0]));
        if (blheli.buf[0] >= num_motors) {
            debug("bad reset channel %u\n", blheli.buf[0]);
            blheli.ack = ACK_D_GENERAL_ERROR;
            blheli_send_reply(&blheli.buf[0], 1);            
            break;
        }
        blheli.chan = blheli.buf[0];
        switch (blheli.interface_mode[blheli.chan]) {
        case imSIL_BLB:
        case imATM_BLB:
        case imARM_BLB:
            debug("imSIL_BLB ATM  ARM bootloader\n");
            BL_SendCMDRunRestartBootloader();
            break;
        case imSK:
            break;
        }
        blheli_send_reply(&blheli.chan, 1);
        setDisconnected();
        break;
    }

    case cmd_DeviceInitFlash: {
        debug("cmd_DeviceInitFlash(%u)\n", unsigned(blheli.buf[0]));
        if (blheli.buf[0] >= num_motors) {
            debug("bad channel %u\n", blheli.buf[0]);
            break;
        }
        blheli.chan = blheli.buf[0];
        blheli.ack = ACK_OK;
        BL_ConnectEx();
        uint8_t buf[4] = {blheli.deviceInfo[blheli.chan][0],
                          blheli.deviceInfo[blheli.chan][1],
                          blheli.deviceInfo[blheli.chan][2],
                          blheli.deviceInfo[blheli.chan][3]};  // device ID
        blheli_send_reply(buf, sizeof(buf));
        break;
    }

    case cmd_InterfaceSetMode: {
        debug("cmd_InterfaceSetMode(%u)\n", unsigned(blheli.buf[0]));
        blheli.interface_mode[blheli.chan] = blheli.buf[0];
        blheli_send_reply(&blheli.interface_mode[blheli.chan], 1);
        break;
    }

    case cmd_DeviceRead: {
        uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256;
        debug("cmd_DeviceRead(%u) n=%u\n", blheli.chan, nbytes);
        uint8_t buf[nbytes];
        uint8_t cmd = blheli.interface_mode[blheli.chan]==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL;
        if (!BL_ReadA(cmd, buf, nbytes)) {
            nbytes = 1;
        }
        blheli_send_reply(buf, nbytes);
        break;
    }

    case cmd_DevicePageErase: {
        uint8_t page = blheli.buf[0];
        debug("cmd_DevicePageErase(%u) im=%u\n", page, blheli.interface_mode[blheli.chan]);
        switch (blheli.interface_mode[blheli.chan]) {
        case imSIL_BLB:
        case imARM_BLB: {
            if  (blheli.interface_mode[blheli.chan] == imARM_BLB) {
                // Address =Page * 1024
                blheli.address = page << 10;
            } else {
                // Address =Page * 512
                blheli.address = page << 9;
            }
            debug("ARM PageErase 0x%04x\n", blheli.address);
            BL_PageErase();
            blheli.address = 0;
            blheli_send_reply(&page, 1);
            break;
        }
        default:
            blheli.ack = ACK_I_INVALID_CMD;
            blheli_send_reply(&page, 1);
            break;
        }
        break;
    }

    case cmd_DeviceWrite: {
        uint16_t nbytes = blheli.param_len;
        debug("cmd_DeviceWrite n=%u im=%u\n", nbytes, blheli.interface_mode[blheli.chan]);
        uint8_t buf[nbytes];
        memcpy(buf, blheli.buf, nbytes);
        switch (blheli.interface_mode[blheli.chan]) {
        case imSIL_BLB:
        case imATM_BLB:
        case imARM_BLB: {
            BL_WriteFlash(buf, nbytes);
            break;
        }
        case imSK: {
            debug("Unsupported flash mode imSK\n");
            break;
        }
        }
        uint8_t b=0;
        blheli_send_reply(&b, 1);        
        break;
    }

    case cmd_DeviceVerify: {
        uint16_t nbytes = blheli.param_len;
        debug("cmd_DeviceWrite n=%u im=%u\n", nbytes, blheli.interface_mode[blheli.chan]);
        switch (blheli.interface_mode[blheli.chan]) {
        case imARM_BLB: {
            uint8_t buf[nbytes];
            memcpy(buf, blheli.buf, nbytes);            
            BL_VerifyFlash(buf, nbytes);
            break;
        }
        default:
            blheli.ack = ACK_I_INVALID_CMD;
            break;
        }
        uint8_t b=0;
        blheli_send_reply(&b, 1);        
        break;
    }

    case cmd_DeviceReadEEprom: {
        uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256;
        uint8_t buf[nbytes];
        debug("cmd_DeviceReadEEprom n=%u im=%u\n", nbytes, blheli.interface_mode[blheli.chan]);
        switch (blheli.interface_mode[blheli.chan]) {
        case imATM_BLB: {
            if (!BL_ReadA(CMD_READ_EEPROM, buf, nbytes)) {
                blheli.ack = ACK_D_GENERAL_ERROR;
            }
            break;
        }
        default:
            blheli.ack = ACK_I_INVALID_CMD;
            break;
        }
        if (blheli.ack != ACK_OK) {
            nbytes = 1;
            buf[0] = 0;
        }
        blheli_send_reply(buf, nbytes);
        break;
    }

    case cmd_DeviceWriteEEprom: {
        uint16_t nbytes = blheli.param_len;
        uint8_t buf[nbytes];
        memcpy(buf, blheli.buf, nbytes);
        debug("cmd_DeviceWriteEEprom n=%u im=%u\n", nbytes, blheli.interface_mode[blheli.chan]);
        switch (blheli.interface_mode[blheli.chan]) {
        case imATM_BLB:
            BL_WriteA(CMD_PROG_EEPROM, buf, nbytes, 1000);
            break;
        default:
            blheli.ack = ACK_D_GENERAL_ERROR;
            break;
        }
        uint8_t b = 0;
        blheli_send_reply(&b, 1);
        break;
    }
        
    case cmd_DeviceEraseAll:
    case cmd_DeviceC2CK_LOW:
    default:
        // ack=unknown command
        blheli.ack = ACK_I_INVALID_CMD;
        debug("Unknown BLHeli protocol 0x%02x\n", blheli.command);
        uint8_t b = 0;
        blheli_send_reply(&b, 1);
        break;
    }
}

/*
  process an input byte, return true if we have received a whole
  packet with correct CRC
 */
bool AP_BLHeli::process_input(uint8_t b)
{
    bool valid_packet = false;
    
    if (msp.escMode == PROTOCOL_4WAY && blheli.state == BLHELI_IDLE && b == '$') {
        debug("Change to MSP mode\n");
        msp.escMode = PROTOCOL_NONE;
        hal.rcout->serial_end();
        serial_start_ms = 0;
    }
    if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') {
        debug("Change to BLHeli mode\n");
        memset(blheli.connected, 0, sizeof(blheli.connected));
        msp.escMode = PROTOCOL_4WAY;
    }
    if (msp.escMode == PROTOCOL_4WAY) {
        blheli_4way_process_byte(b);
    } else {
        msp_process_byte(b);
    }
    if (msp.escMode == PROTOCOL_4WAY) {
        if (blheli.state == BLHELI_COMMAND_RECEIVED) {
            valid_packet = true;
            last_valid_ms = AP_HAL::millis();
            if (uart->lock_port(BLHELI_UART_LOCK_KEY)) {
                uart_locked = true;
            }
            blheli_process_command();
            blheli.state = BLHELI_IDLE;
            msp.state = MSP_IDLE;
        }
    } else if (msp.state == MSP_COMMAND_RECEIVED) {
        if (msp.packetType == MSP_PACKET_COMMAND) {
            valid_packet = true;
            if (uart->lock_port(BLHELI_UART_LOCK_KEY)) {
                uart_locked = true;
            }
            last_valid_ms = AP_HAL::millis();
            msp_process_command();
        }
        msp.state = MSP_IDLE;
        blheli.state = BLHELI_IDLE;
    }

    return valid_packet;
}

结合源码及debug消息初步分析是地面站点击读取的时候发的0x31后直接到0x37，导致前面的配置信息都没有获取到，所以就连接不上。但还存在疑惑的是 为什么32位电调可以顺利连接，8位电调的固件版本我从16.7慢慢降低到15版本也不行。

希望能在评论区得到大佬的解答，感激不尽！！！