From 476a40c906a67ccbb8c01647f8e03799c6dda12c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Aug 2018 13:35:54 +1000 Subject: [PATCH] AP_BLHeli: improved reliability of pass-thru support this uses a connection cache to prevent re-connecting to an ESC when not needed, and allows for pass-thru comms on any port, using SERVO_BLH_PORT parameter --- libraries/AP_BLHeli/AP_BLHeli.cpp | 149 ++++++++++++++++++++---------- libraries/AP_BLHeli/AP_BLHeli.h | 27 +++++- 2 files changed, 127 insertions(+), 49 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 5e2ee2263d..374371440a 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -93,6 +93,13 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @Values: 0:None,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200 // @User: Advanced AP_GROUPINFO("OTYPE", 7, AP_BLHeli, output_type, 0), + + // @Param: PORT + // @DisplayName: Control port + // @Description: This sets the telemetry port to use for blheli pass-thru + // @Values: 0:Console,1:Telem1,2:Telem2,3:Telem3,4:Telem4,5:Telem5 + // @User: Advanced + AP_GROUPINFO("PORT", 8, AP_BLHeli, control_port, 0), AP_GROUPEND }; @@ -105,6 +112,7 @@ AP_BLHeli::AP_BLHeli(void) // set defaults from the parameter table AP_Param::setup_object_defaults(this, var_info); singleton = this; + last_control_port = -1; } /* @@ -488,13 +496,14 @@ uint16_t AP_BLHeli::BL_CRC(const uint8_t *buf, uint16_t len) bool AP_BLHeli::isMcuConnected(void) { - return blheli.deviceInfo[0] > 0; + return blheli.connected[blheli.chan]; } void AP_BLHeli::setDisconnected(void) { - blheli.deviceInfo[0] = 0; - blheli.deviceInfo[1] = 0; + blheli.connected[blheli.chan] = false; + blheli.deviceInfo[blheli.chan][0] = 0; + blheli.deviceInfo[blheli.chan][1] = 0; } /* @@ -520,8 +529,6 @@ bool AP_BLHeli::BL_SendBuf(const uint8_t *buf, uint16_t len) least 1 second before we start sending serial data. */ hal.scheduler->delay(1100); - } else { - hal.scheduler->delay(10); } memcpy(blheli.buf, buf, len); uint16_t crc = BL_CRC(buf, len); @@ -608,7 +615,21 @@ bool AP_BLHeli::BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n) if (!BL_SendBuf(sCMD, 2)) { return false; } - return BL_ReadBuf(buf, n); + bool ret = BL_ReadBuf(buf, n); + if (ret && n == sizeof(esc_status) && blheli.address == esc_status_addr) { + // display esc_status structure if we see it + struct esc_status status; + memcpy(&status, buf, n); + debug("Prot %u Good %u Bad %u %x %x %x x%x\n", + (unsigned)status.protocol, + (unsigned)status.good_frames, + (unsigned)status.bad_frames, + (unsigned)status.unknown[0], + (unsigned)status.unknown[1], + (unsigned)status.unknown[2], + (unsigned)status.unknown2); + } + return ret; } return false; } @@ -618,6 +639,10 @@ bool AP_BLHeli::BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n) */ bool AP_BLHeli::BL_ConnectEx(void) { + if (blheli.connected[blheli.chan] != 0) { + debug("Using cached interface 0x%x for %u", blheli.interface_mode[blheli.chan], blheli.chan); + return true; + } debug("BL_ConnectEx %u/%u at %u", 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}; @@ -637,19 +662,19 @@ bool AP_BLHeli::BL_ConnectEx(void) } // extract device information - blheli.deviceInfo[2] = BootInfo[3]; - blheli.deviceInfo[1] = BootInfo[4]; - blheli.deviceInfo[0] = BootInfo[5]; + blheli.deviceInfo[blheli.chan][2] = BootInfo[3]; + blheli.deviceInfo[blheli.chan][1] = BootInfo[4]; + blheli.deviceInfo[blheli.chan][0] = BootInfo[5]; - blheli.interface_mode = 0; + blheli.interface_mode[blheli.chan] = 0; - uint16_t *devword = (uint16_t *)blheli.deviceInfo; + uint16_t *devword = (uint16_t *)blheli.deviceInfo[blheli.chan]; switch (*devword) { case 0x9307: case 0x930A: case 0x930F: case 0x940B: - blheli.interface_mode = imATM_BLB; + blheli.interface_mode[blheli.chan] = imATM_BLB; debug("Interface type imATM_BLB"); break; case 0xF310: @@ -659,14 +684,14 @@ bool AP_BLHeli::BL_ConnectEx(void) case 0xF850: case 0xE8B1: case 0xE8B2: - blheli.interface_mode = imSIL_BLB; + blheli.interface_mode[blheli.chan] = imSIL_BLB; debug("Interface type imSIL_BLB"); break; case 0x1F06: case 0x3306: case 0x3406: case 0x3506: - blheli.interface_mode = imARM_BLB; + blheli.interface_mode[blheli.chan] = imARM_BLB; debug("Interface type imARM_BLB"); break; default: @@ -674,7 +699,10 @@ bool AP_BLHeli::BL_ConnectEx(void) debug("Unknown interface type 0x%04x", *devword); break; } - blheli.deviceInfo[3] = blheli.interface_mode; + blheli.deviceInfo[blheli.chan][3] = blheli.interface_mode[blheli.chan]; + if (blheli.interface_mode[blheli.chan] != 0) { + blheli.connected[blheli.chan] = true; + } return true; } @@ -705,7 +733,7 @@ bool AP_BLHeli::BL_PageErase(void) void AP_BLHeli::BL_SendCMDRunRestartBootloader(void) { uint8_t sCMD[] = {RestartBootloader, 0}; - blheli.deviceInfo[0] = 1; + blheli.deviceInfo[blheli.chan][0] = 1; BL_SendBuf(sCMD, 2); } @@ -831,12 +859,19 @@ void AP_BLHeli::blheli_process_command(void) uart->lock_port(0); uart_locked = false; } + memset(blheli.connected, 0, sizeof(blheli.connected)); break; } case cmd_DeviceReset: { debug("cmd_DeviceReset(%u)", unsigned(blheli.buf[0])); + if (blheli.buf[0] >= num_motors) { + debug("bad reset channel %u", 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) { + switch (blheli.interface_mode[blheli.chan]) { case imSIL_BLB: case imATM_BLB: case imARM_BLB: @@ -852,26 +887,25 @@ void AP_BLHeli::blheli_process_command(void) case cmd_DeviceInitFlash: { debug("cmd_DeviceInitFlash(%u)", unsigned(blheli.buf[0])); - blheli.chan = blheli.buf[0]; - for (uint8_t tries=0; tries<5; tries++) { - blheli.ack = ACK_OK; - setDisconnected(); - if (BL_ConnectEx()) { - break; - } + if (blheli.buf[0] >= num_motors) { + debug("bad channel %u", blheli.buf[0]); + break; } - uint8_t buf[4] = {blheli.deviceInfo[0], - blheli.deviceInfo[1], - blheli.deviceInfo[2], - blheli.deviceInfo[3]}; // device ID + 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)", unsigned(blheli.buf[0])); - blheli.interface_mode = blheli.buf[0]; - blheli_send_reply(&blheli.interface_mode, 1); + blheli.interface_mode[blheli.chan] = blheli.buf[0]; + blheli_send_reply(&blheli.interface_mode[blheli.chan], 1); break; } @@ -879,7 +913,7 @@ void AP_BLHeli::blheli_process_command(void) uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256; debug("cmd_DeviceRead(%u) n=%u", blheli.chan, nbytes); uint8_t buf[nbytes]; - uint8_t cmd = blheli.interface_mode==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL; + 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; } @@ -889,11 +923,11 @@ void AP_BLHeli::blheli_process_command(void) case cmd_DevicePageErase: { uint8_t page = blheli.buf[0]; - debug("cmd_DevicePageErase(%u) im=%u", page, blheli.interface_mode); - switch (blheli.interface_mode) { + debug("cmd_DevicePageErase(%u) im=%u", page, blheli.interface_mode[blheli.chan]); + switch (blheli.interface_mode[blheli.chan]) { case imSIL_BLB: case imARM_BLB: { - if (blheli.interface_mode == imARM_BLB) { + if (blheli.interface_mode[blheli.chan] == imARM_BLB) { // Address =Page * 1024 blheli.address = page << 10; } else { @@ -916,10 +950,10 @@ void AP_BLHeli::blheli_process_command(void) case cmd_DeviceWrite: { uint16_t nbytes = blheli.param_len; - debug("cmd_DeviceWrite n=%u im=%u", nbytes, blheli.interface_mode); + debug("cmd_DeviceWrite n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]); uint8_t buf[nbytes]; memcpy(buf, blheli.buf, nbytes); - switch (blheli.interface_mode) { + switch (blheli.interface_mode[blheli.chan]) { case imSIL_BLB: case imATM_BLB: case imARM_BLB: { @@ -938,8 +972,8 @@ void AP_BLHeli::blheli_process_command(void) case cmd_DeviceVerify: { uint16_t nbytes = blheli.param_len; - debug("cmd_DeviceWrite n=%u im=%u", nbytes, blheli.interface_mode); - switch (blheli.interface_mode) { + debug("cmd_DeviceWrite n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]); + switch (blheli.interface_mode[blheli.chan]) { case imARM_BLB: { uint8_t buf[nbytes]; memcpy(buf, blheli.buf, nbytes); @@ -958,8 +992,8 @@ void AP_BLHeli::blheli_process_command(void) case cmd_DeviceReadEEprom: { uint16_t nbytes = blheli.buf[0]?blheli.buf[0]:256; uint8_t buf[nbytes]; - debug("cmd_DeviceReadEEprom n=%u im=%u", nbytes, blheli.interface_mode); - switch (blheli.interface_mode) { + debug("cmd_DeviceReadEEprom n=%u im=%u", 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; @@ -982,8 +1016,8 @@ void AP_BLHeli::blheli_process_command(void) uint16_t nbytes = blheli.param_len; uint8_t buf[nbytes]; memcpy(buf, blheli.buf, nbytes); - debug("cmd_DeviceWriteEEprom n=%u im=%u", nbytes, blheli.interface_mode); - switch (blheli.interface_mode) { + debug("cmd_DeviceWriteEEprom n=%u im=%u", nbytes, blheli.interface_mode[blheli.chan]); + switch (blheli.interface_mode[blheli.chan]) { case imATM_BLB: BL_WriteA(CMD_PROG_EEPROM, buf, nbytes, 1000); break; @@ -1024,6 +1058,7 @@ bool AP_BLHeli::process_input(uint8_t b) } if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') { debug("Change to BLHeli mode"); + memset(blheli.connected, 0, sizeof(blheli.connected)); msp.escMode = PROTOCOL_4WAY; } if (msp.escMode == PROTOCOL_4WAY) { @@ -1080,6 +1115,10 @@ void AP_BLHeli::run_connection_test(uint8_t chan) { debug_uart = hal.console; uint8_t saved_chan = blheli.chan; + if (blheli.buf[0] >= num_motors) { + debug("bad channel %u", chan); + return; + } blheli.chan = chan; debug("Running test on channel %u", blheli.chan); run_test.set_and_notify(0); @@ -1089,10 +1128,21 @@ void AP_BLHeli::run_connection_test(uint8_t chan) setDisconnected(); if (BL_ConnectEx()) { uint8_t buf[256]; - uint8_t cmd = blheli.interface_mode==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL; + uint8_t cmd = blheli.interface_mode[blheli.chan]==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL; passed = true; - blheli.address = blheli.interface_mode==imATM_BLB?0:0x7c00; + blheli.address = blheli.interface_mode[blheli.chan]==imATM_BLB?0:0x7c00; passed &= BL_ReadA(cmd, buf, sizeof(buf)); + if (blheli.interface_mode[blheli.chan]==imARM_BLB) { + if (passed) { + // read status structure + blheli.address = esc_status_addr; + passed &= BL_SendCMDSetAddress(); + } + if (passed) { + struct esc_status status; + passed &= BL_ReadA(CMD_READ_FLASH_SIL, (uint8_t *)&status, sizeof(status)); + } + } BL_SendCMDRunRestartBootloader(); break; } @@ -1139,11 +1189,16 @@ void AP_BLHeli::update(void) initialised = true; run_test.set_and_notify(0); - - if (gcs().install_alternative_protocol(MAVLINK_COMM_0, + + if (last_control_port > 0 && last_control_port != control_port) { + gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+last_control_port), nullptr); + last_control_port = -1; + } + if (gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+control_port), FUNCTOR_BIND_MEMBER(&AP_BLHeli::protocol_handler, bool, uint8_t, AP_HAL::UARTDriver *))) { - debug("BLHeli installed"); + debug("BLHeli installed on port %u", (unsigned)control_port); + last_control_port = control_port; } uint16_t mask = uint16_t(channel_mask.get()); diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index 475fb181d2..1ad3559f06 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -76,6 +76,7 @@ private: AP_Int16 telem_rate; AP_Int8 debug_level; AP_Int8 output_type; + AP_Int8 control_port; enum mspState { MSP_IDLE=0, @@ -183,12 +184,33 @@ private: uint8_t buf[256+3+8]; uint8_t crc1; uint16_t crc; - uint8_t interface_mode; - uint8_t deviceInfo[4]; + bool connected[AP_BLHELI_MAX_ESCS]; + uint8_t interface_mode[AP_BLHELI_MAX_ESCS]; + uint8_t deviceInfo[AP_BLHELI_MAX_ESCS][4]; uint8_t chan; uint8_t ack; } blheli; + const uint16_t esc_status_addr = 0xEB00; + + // protocol reported by ESC in esc_status + enum esc_protocol { + ESC_PROTOCOL_NONE=0, + ESC_PROTOCOL_NORMAL=1, + ESC_PROTOCOL_ONESHOT125=2, + ESC_PROTOCOL_DSHOT=5, + }; + + // ESC status structure at address 0xEB00 + struct PACKED esc_status { + uint8_t unknown[3]; + enum esc_protocol protocol; + uint32_t good_frames; + uint32_t bad_frames; + uint32_t unknown2; + }; + + AP_HAL::UARTDriver *uart; AP_HAL::UARTDriver *debug_uart; AP_HAL::UARTDriver *telem_uart; @@ -222,6 +244,7 @@ private: static const uint8_t telem_packet_size = 10; bool telem_uart_started; uint32_t last_telem_byte_read_us; + int8_t last_control_port; bool msp_process_byte(uint8_t c); void blheli_crc_update(uint8_t c);