diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp
new file mode 100644
index 0000000000..24b2914ff3
--- /dev/null
+++ b/libraries/AP_BLHeli/AP_BLHeli.cpp
@@ -0,0 +1,998 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ implementation of MSP and BLHeli-4way protocols for pass-through ESC
+ calibration and firmware update
+
+ With thanks to betaflight for a great reference implementation
+ */
+
+#include "AP_BLHeli.h"
+#include
+#include
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+#if 0
+#define debug(fmt, args ...) do { printf("ESC: " fmt "\n", ## args); } while (0)
+#else
+#define debug(fmt, args ...)
+#endif
+
+const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
+ // @Param: MASK
+ // @DisplayName: Channel Bitmask
+ // @Description: Enable of BLHeli servo protocol support to specific channels
+ // @Bitmask: 0:Channel1,1:Channel2,2:Channel3,3:Channel4,4:Channel5,5:Channel6,6:Channel7,7:Channel8,8:Channel9,9:Channel10,10:Channel11,11:Channel12,12:Channel13,13:Channel14,14:Channel15,15:Channel16
+ // @User: Standard
+ AP_GROUPINFO("MASK", 1, AP_BLHeli, channel_mask, 0),
+
+ AP_GROUPEND
+};
+
+// constructor
+AP_BLHeli::AP_BLHeli(void)
+{
+ // set defaults from the parameter table
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+/*
+ process one byte of serial input for MSP protocol
+ */
+bool AP_BLHeli::msp_process_byte(uint8_t c)
+{
+ if (msp.state == MSP_IDLE) {
+ msp.escMode = PROTOCOL_NONE;
+ if (c == '$') {
+ msp.state = MSP_HEADER_START;
+ } else {
+ return false;
+ }
+ } else if (msp.state == MSP_HEADER_START) {
+ msp.state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
+ } else if (msp.state == MSP_HEADER_M) {
+ msp.state = MSP_IDLE;
+ switch (c) {
+ case '<': // COMMAND
+ msp.packetType = MSP_PACKET_COMMAND;
+ msp.state = MSP_HEADER_ARROW;
+ break;
+ case '>': // REPLY
+ msp.packetType = MSP_PACKET_REPLY;
+ msp.state = MSP_HEADER_ARROW;
+ break;
+ default:
+ break;
+ }
+ } else if (msp.state == MSP_HEADER_ARROW) {
+ if (c > sizeof(msp.buf)) {
+ msp.state = MSP_IDLE;
+ } else {
+ msp.dataSize = c;
+ msp.offset = 0;
+ msp.checksum = 0;
+ msp.checksum ^= c;
+ msp.state = MSP_HEADER_SIZE;
+ }
+ } else if (msp.state == MSP_HEADER_SIZE) {
+ msp.cmdMSP = c;
+ msp.checksum ^= c;
+ msp.state = MSP_HEADER_CMD;
+ } else if (msp.state == MSP_HEADER_CMD && msp.offset < msp.dataSize) {
+ msp.checksum ^= c;
+ msp.buf[msp.offset++] = c;
+ } else if (msp.state == MSP_HEADER_CMD && msp.offset >= msp.dataSize) {
+ if (msp.checksum == c) {
+ msp.state = MSP_COMMAND_RECEIVED;
+ } else {
+ msp.state = MSP_IDLE;
+ }
+ }
+ return true;
+}
+
+/*
+ update CRC state for blheli protocol
+ */
+void AP_BLHeli::blheli_crc_update(uint8_t c)
+{
+ blheli.crc = crc_xmodem_update(blheli.crc, c);
+}
+
+/*
+ process one byte of serial input for blheli 4way protocol
+ */
+bool AP_BLHeli::blheli_4way_process_byte(uint8_t c)
+{
+ if (blheli.state == BLHELI_IDLE) {
+ if (c == cmd_Local_Escape) {
+ blheli.state = BLHELI_HEADER_START;
+ blheli.crc = 0;
+ blheli_crc_update(c);
+ } else {
+ return false;
+ }
+ } else if (blheli.state == BLHELI_HEADER_START) {
+ blheli.command = c;
+ blheli_crc_update(c);
+ blheli.state = BLHELI_HEADER_CMD;
+ } else if (blheli.state == BLHELI_HEADER_CMD) {
+ blheli.address = c<<8;
+ blheli.state = BLHELI_HEADER_ADDR_HIGH;
+ blheli_crc_update(c);
+ } else if (blheli.state == BLHELI_HEADER_ADDR_HIGH) {
+ blheli.address |= c;
+ blheli.state = BLHELI_HEADER_ADDR_LOW;
+ blheli_crc_update(c);
+ } else if (blheli.state == BLHELI_HEADER_ADDR_LOW) {
+ blheli.state = BLHELI_HEADER_LEN;
+ blheli.param_len = c?c:256;
+ blheli.offset = 0;
+ blheli_crc_update(c);
+ } else if (blheli.state == BLHELI_HEADER_LEN) {
+ blheli.buf[blheli.offset++] = c;
+ blheli_crc_update(c);
+ if (blheli.offset == blheli.param_len) {
+ blheli.state = BLHELI_CRC1;
+ }
+ } else if (blheli.state == BLHELI_CRC1) {
+ blheli.crc1 = c;
+ blheli.state = BLHELI_CRC2;
+ } else if (blheli.state == BLHELI_CRC2) {
+ uint16_t crc = blheli.crc1<<8 | c;
+ if (crc == blheli.crc) {
+ blheli.state = BLHELI_COMMAND_RECEIVED;
+ } else {
+ blheli.state = BLHELI_IDLE;
+ }
+ }
+ return true;
+}
+
+/*
+ send a MSP protocol reply
+ */
+void AP_BLHeli::msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len)
+{
+ uint8_t *b = &msp.buf[0];
+ *b++ = '$';
+ *b++ = 'M';
+ *b++ = '>';
+ *b++ = len;
+ *b++ = cmd;
+ memcpy(b, buf, len);
+ b += len;
+ uint8_t c = 0;
+ for (uint8_t i=0; iwrite(&msp.buf[0], len+6);
+}
+
+void AP_BLHeli::putU16(uint8_t *b, uint16_t v)
+{
+ b[0] = v;
+ b[1] = v >> 8;
+}
+
+uint16_t AP_BLHeli::getU16(const uint8_t *b)
+{
+ return b[0] | (b[1]<<8);
+}
+
+void AP_BLHeli::putU32(uint8_t *b, uint32_t v)
+{
+ b[0] = v;
+ b[1] = v >> 8;
+ b[2] = v >> 16;
+ b[3] = v >> 24;
+}
+
+void AP_BLHeli::putU16_BE(uint8_t *b, uint16_t v)
+{
+ b[0] = v >> 8;
+ b[1] = v;
+}
+
+/*
+ process a MSP command from GCS
+ */
+void AP_BLHeli::msp_process_command(void)
+{
+ debug("MSP cmd %u len=%u", msp.cmdMSP, msp.dataSize);
+ switch (msp.cmdMSP) {
+ case MSP_API_VERSION: {
+ uint8_t buf[3] = { MSP_PROTOCOL_VERSION, API_VERSION_MAJOR, API_VERSION_MINOR };
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_FC_VARIANT:
+ msp_send_reply(msp.cmdMSP, (const uint8_t *)ARDUPILOT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
+ break;
+
+ case MSP_FC_VERSION: {
+ uint8_t version[3] = { 3, 3, 0 };
+ msp_send_reply(msp.cmdMSP, version, sizeof(version));
+ break;
+ }
+ case MSP_BOARD_INFO: {
+ // send a generic 'ArduPilot ChibiOS' board type
+ uint8_t buf[7] = { 'A', 'R', 'C', 'H', 0, 0, 0 };
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_BUILD_INFO: {
+ // build date, build time, git version
+ uint8_t buf[26] {
+ 0x4d, 0x61, 0x72, 0x20, 0x31, 0x36, 0x20, 0x32, 0x30,
+ 0x31, 0x38, 0x30, 0x38, 0x3A, 0x34, 0x32, 0x3a, 0x32, 0x39,
+ 0x62, 0x30, 0x66, 0x66, 0x39, 0x32, 0x38};
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_REBOOT:
+ debug("MSP: rebooting");
+ hal.scheduler->reboot(false);
+ break;
+
+ case MSP_UID:
+ // MCU identifer
+ msp_send_reply(msp.cmdMSP, (const uint8_t *)UDID_START, 12);
+ break;
+
+ case MSP_ADVANCED_CONFIG: {
+ uint8_t buf[10];
+ buf[0] = 1; // gyro sync denom
+ buf[1] = 4; // pid process denom
+ buf[2] = 0; // use unsynced pwm
+ buf[3] = (uint8_t)PWM_TYPE_DSHOT150; // motor PWM protocol
+ putU16(&buf[4], 480); // motor PWM Rate
+ putU16(&buf[6], 450); // idle offset value
+ buf[8] = 0; // use 32kHz
+ buf[9] = 0; // motor PWM inversion
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_FEATURE_CONFIG: {
+ uint8_t buf[4];
+ putU32(buf, 0); // from MSPFeatures enum
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_STATUS: {
+ uint8_t buf[21];
+ putU16(&buf[0], 2500); // loop time usec
+ putU16(&buf[2], 0); // i2c error count
+ putU16(&buf[4], 0x27); // available sensors
+ putU32(&buf[6], 0); // flight modes
+ buf[10] = 0; // pid profile index
+ putU16(&buf[11], 5); // system load percent
+ putU16(&buf[13], 0); // gyro cycle time
+ buf[15] = 0; // flight mode flags length
+ buf[16] = 18; // arming disable flags count
+ putU32(&buf[17], 0); // arming disable flags
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_MOTOR_3D_CONFIG: {
+ uint8_t buf[6];
+ putU16(&buf[0], 1406); // 3D deadband low
+ putU16(&buf[2], 1514); // 3D deadband high
+ putU16(&buf[4], 1460); // 3D neutral
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_MOTOR_CONFIG: {
+ uint8_t buf[6];
+ putU16(&buf[0], 1070); // min throttle
+ putU16(&buf[2], 2000); // max throttle
+ putU16(&buf[4], 1000); // min command
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_MOTOR: {
+ // get the output going to each motor
+ uint8_t buf[16];
+ for (uint8_t i = 0; i < 8; i++) {
+ putU16(&buf[2*i], hal.rcout->read(i));
+ }
+ msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
+ break;
+ }
+
+ case MSP_SET_MOTOR: {
+ // set the output to each motor
+ uint8_t nmotors = msp.dataSize / 2;
+ debug("MSP_SET_MOTOR %u", nmotors);
+ hal.rcout->cork();
+ for (uint8_t i = 0; i < nmotors; i++) {
+ if (i >= num_motors) {
+ break;
+ }
+ uint16_t v = getU16(&msp.buf[i*2]);
+ debug("MSP_SET_MOTOR %u %u", i, v);
+ hal.rcout->write(motor_map[i], v);
+ }
+ hal.rcout->push();
+ break;
+ }
+
+ case MSP_SET_4WAY_IF: {
+ if (msp.dataSize == 0) {
+ msp.escMode = PROTOCOL_4WAY;
+ } else if (msp.dataSize == 2) {
+ msp.escMode = (enum escProtocol)msp.buf[0];
+ msp.portIndex = msp.buf[1];
+ }
+ debug("escMode=%u portIndex=%u", msp.escMode, msp.portIndex);
+ uint8_t n = num_motors;
+ switch (msp.escMode) {
+ case PROTOCOL_4WAY:
+ break;
+ default:
+ n = 0;
+ hal.rcout->serial_end();
+ serial_started = false;
+ break;
+ }
+ msp_send_reply(msp.cmdMSP, &n, 1);
+ break;
+ }
+ default:
+ debug("Unknown MSP command %u", msp.cmdMSP);
+ break;
+ }
+}
+
+
+/*
+ send a blheli 4way protocol reply
+ */
+void AP_BLHeli::blheli_send_reply(const uint8_t *buf, uint16_t len)
+{
+ uint8_t *b = &blheli.buf[0];
+ *b++ = cmd_Remote_Escape;
+ *b++ = blheli.command;
+ putU16_BE(b, blheli.address); b += 2;
+ *b++ = len==256?0:len;
+ memcpy(b, buf, len);
+ b += len;
+ *b++ = blheli.ack;
+ putU16_BE(b, crc_xmodem(&blheli.buf[0], len+6));
+ uart->write(&blheli.buf[0], len+8);
+ debug("OutB(%u) 0x%02x ack=0x%02x", len+8, (unsigned)blheli.command, blheli.ack);
+}
+
+/*
+ CRC used when talking to ESCs
+ */
+uint16_t AP_BLHeli::BL_CRC(const uint8_t *buf, uint16_t len)
+{
+ uint16_t crc = 0;
+ while (len--) {
+ uint8_t xb = *buf++;
+ for (uint8_t i = 0; i < 8; i++) {
+ if (((xb & 0x01) ^ (crc & 0x0001)) !=0 ) {
+ crc = crc >> 1;
+ crc = crc ^ 0xA001;
+ } else {
+ crc = crc >> 1;
+ }
+ xb = xb >> 1;
+ }
+ }
+ return crc;
+}
+
+bool AP_BLHeli::isMcuConnected(void)
+{
+ return blheli.deviceInfo[0] > 0;
+}
+
+void AP_BLHeli::setDisconnected(void)
+{
+ blheli.deviceInfo[0] = 0;
+ blheli.deviceInfo[1] = 0;
+}
+
+/*
+ send a set of bytes to an RC output channel
+ */
+bool AP_BLHeli::BL_SendBuf(const uint8_t *buf, uint16_t len)
+{
+ bool send_crc = isMcuConnected();
+ if (blheli.chan >= num_motors) {
+ return false;
+ }
+ hal.scheduler->delay(10);
+ if (!hal.rcout->serial_setup_output(motor_map[blheli.chan], 19200)) {
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ serial_started = true;
+ memcpy(blheli.buf, buf, len);
+ uint16_t crc = BL_CRC(buf, len);
+ blheli.buf[len] = crc;
+ blheli.buf[len+1] = crc>>8;
+ if (!hal.rcout->serial_write_bytes(blheli.buf, len+(send_crc?2:0))) {
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ return true;
+}
+
+/*
+ read bytes from the ESC connection
+ */
+bool AP_BLHeli::BL_ReadBuf(uint8_t *buf, uint16_t len)
+{
+ bool check_crc = isMcuConnected() && len > 0;
+ uint16_t req_bytes = len+(check_crc?3:1);
+ uint16_t n = hal.rcout->serial_read_bytes(blheli.buf, req_bytes);
+ debug("BL_ReadBuf %u -> %u", len, n);
+ if (req_bytes != n) {
+ debug("short read");
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ if (check_crc) {
+ uint16_t crc = BL_CRC(blheli.buf, len);
+ if ((crc & 0xff) != blheli.buf[len] ||
+ (crc >> 8) != blheli.buf[len+1]) {
+ debug("bad CRC");
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ if (blheli.buf[len+2] != brSUCCESS) {
+ debug("bad ACK 0x%02x", blheli.buf[len+2]);
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ } else {
+ if (blheli.buf[len] != brSUCCESS) {
+ debug("bad ACK1 0x%02x", blheli.buf[len]);
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ }
+ if (len > 0) {
+ memcpy(buf, blheli.buf, len);
+ }
+ return true;
+}
+
+uint8_t AP_BLHeli::BL_GetACK(uint16_t timeout_ms)
+{
+ uint8_t ack;
+ uint32_t start_ms = AP_HAL::millis();
+ while (AP_HAL::millis() - start_ms < timeout_ms) {
+ if (hal.rcout->serial_read_bytes(&ack, 1) == 1) {
+ return ack;
+ }
+ }
+ // return brNONE, meaning no ACK received in the timeout
+ return brNONE;
+}
+
+bool AP_BLHeli::BL_SendCMDSetAddress()
+{
+ // skip if adr == 0xFFFF
+ if (blheli.address == 0xFFFF) {
+ return true;
+ }
+ debug("BL_SendCMDSetAddress 0x%04x", blheli.address);
+ uint8_t sCMD[] = {CMD_SET_ADDRESS, 0, uint8_t(blheli.address>>8), uint8_t(blheli.address)};
+ if (!BL_SendBuf(sCMD, 4)) {
+ return false;
+ }
+ return BL_GetACK() == brSUCCESS;
+}
+
+bool AP_BLHeli::BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n)
+{
+ if (BL_SendCMDSetAddress()) {
+ uint8_t sCMD[] = {cmd, uint8_t(n==256?0:n)};
+ if (!BL_SendBuf(sCMD, 2)) {
+ return false;
+ }
+ return BL_ReadBuf(buf, n);
+ }
+ return false;
+}
+
+/*
+ connect to a blheli ESC
+ */
+bool AP_BLHeli::BL_ConnectEx(void)
+{
+ debug("BL_ConnectEx start");
+ 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[2] = BootInfo[3];
+ blheli.deviceInfo[1] = BootInfo[4];
+ blheli.deviceInfo[0] = BootInfo[5];
+
+ blheli.interface_mode = 0;
+
+ uint16_t *devword = (uint16_t *)blheli.deviceInfo;
+ switch (*devword) {
+ case 0x9307:
+ case 0x930A:
+ case 0x930F:
+ case 0x940B:
+ blheli.interface_mode = imATM_BLB;
+ debug("Interface type imATM_BLB");
+ break;
+ case 0xF310:
+ case 0xF330:
+ case 0xF410:
+ case 0xF390:
+ case 0xF850:
+ case 0xE8B1:
+ case 0xE8B2:
+ blheli.interface_mode = imSIL_BLB;
+ debug("Interface type imSIL_BLB");
+ break;
+ case 0x1F06:
+ case 0x3306:
+ case 0x3406:
+ case 0x3506:
+ blheli.interface_mode = imARM_BLB;
+ debug("Interface type imARM_BLB");
+ break;
+ default:
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ debug("Unknown interface type 0x%04x", *devword);
+ break;
+ }
+ blheli.deviceInfo[3] = blheli.interface_mode;
+ return true;
+}
+
+bool AP_BLHeli::BL_SendCMDKeepAlive(void)
+{
+ uint8_t sCMD[] = {CMD_KEEP_ALIVE, 0};
+ if (!BL_SendBuf(sCMD, 2)) {
+ return false;
+ }
+ if (BL_GetACK() != brERRORCOMMAND) {
+ return false;
+ }
+ return true;
+}
+
+bool AP_BLHeli::BL_PageErase(void)
+{
+ if (BL_SendCMDSetAddress()) {
+ uint8_t sCMD[] = {CMD_ERASE_FLASH, 0x01};
+ if (!BL_SendBuf(sCMD, 2)) {
+ return false;
+ }
+ return BL_GetACK(1000) == brSUCCESS;
+ }
+ return false;
+}
+
+void AP_BLHeli::BL_SendCMDRunRestartBootloader(void)
+{
+ uint8_t sCMD[] = {RestartBootloader, 0};
+ blheli.deviceInfo[0] = 1;
+ BL_SendBuf(sCMD, 2);
+}
+
+uint8_t AP_BLHeli::BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes)
+{
+ uint8_t sCMD[] = {CMD_SET_BUFFER, 0, uint8_t(nbytes>>8), uint8_t(nbytes&0xff)};
+ if (!BL_SendBuf(sCMD, 4)) {
+ return false;
+ }
+ uint8_t ack;
+ if ((ack = BL_GetACK()) != brNONE) {
+ debug("BL_SendCMDSetBuffer ack failed 0x%02x", ack);
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ if (!BL_SendBuf(buf, nbytes)) {
+ debug("BL_SendCMDSetBuffer send failed");
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ return (BL_GetACK(40) == brSUCCESS);
+}
+
+bool AP_BLHeli::BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint32_t timeout_ms)
+{
+ if (BL_SendCMDSetAddress()) {
+ if (!BL_SendCMDSetBuffer(buf, nbytes)) {
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+ }
+ uint8_t sCMD[] = {cmd, 0x01};
+ if (!BL_SendBuf(sCMD, 2)) {
+ return false;
+ }
+ return (BL_GetACK(timeout_ms) == brSUCCESS);
+ }
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ return false;
+}
+
+uint8_t AP_BLHeli::BL_WriteFlash(const uint8_t *buf, uint16_t n)
+{
+ return BL_WriteA(CMD_PROG_FLASH, buf, n, 250);
+}
+
+bool AP_BLHeli::BL_VerifyFlash(const uint8_t *buf, uint16_t n)
+{
+ if (BL_SendCMDSetAddress()) {
+ if (!BL_SendCMDSetBuffer(buf, n)) {
+ return false;
+ }
+ uint8_t sCMD[] = {CMD_VERIFY_FLASH_ARM, 0x01};
+ if (!BL_SendBuf(sCMD, 2)) {
+ return false;
+ }
+ uint8_t ack = BL_GetACK(40);
+ switch (ack) {
+ case brSUCCESS:
+ blheli.ack = ACK_OK;
+ break;
+ case brERRORVERIFY:
+ blheli.ack = ACK_I_VERIFY_ERROR;
+ break;
+ default:
+ blheli.ack = ACK_D_GENERAL_ERROR;
+ break;
+ }
+ return true;
+ }
+ return false;
+}
+
+/*
+ process a blheli 4way command from GCS
+ */
+void AP_BLHeli::blheli_process_command(void)
+{
+ debug("BLHeli cmd 0x%02x len=%u", blheli.command, blheli.param_len);
+ blheli.ack = ACK_OK;
+ switch (blheli.command) {
+ case cmd_InterfaceTestAlive: {
+ debug("cmd_InterfaceTestAlive");
+ BL_SendCMDKeepAlive();
+ if (blheli.ack != ACK_OK) {
+ setDisconnected();
+ }
+ uint8_t b = 0;
+ blheli_send_reply(&b, 1);
+ break;
+ }
+ case cmd_ProtocolGetVersion: {
+ debug("cmd_ProtocolGetVersion");
+ uint8_t buf[1];
+ buf[0] = SERIAL_4WAY_PROTOCOL_VER;
+ blheli_send_reply(buf, sizeof(buf));
+ break;
+ }
+ case cmd_InterfaceGetName: {
+ debug("cmd_InterfaceGetName");
+ uint8_t buf[5] = { 4, 'A', 'R', 'D', 'U' };
+ blheli_send_reply(buf, sizeof(buf));
+ break;
+ }
+ case cmd_InterfaceGetVersion: {
+ debug("cmd_InterfaceGetVersion");
+ 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");
+ msp.escMode = PROTOCOL_NONE;
+ uint8_t b = 0;
+ blheli_send_reply(&b, 1);
+ hal.rcout->serial_end();
+ serial_started = false;
+ break;
+ }
+ case cmd_DeviceReset: {
+ debug("cmd_DeviceReset(%u)", unsigned(blheli.buf[0]));
+ blheli.chan = blheli.buf[0];
+ switch (blheli.interface_mode) {
+ case imSIL_BLB:
+ case imATM_BLB:
+ case imARM_BLB:
+ BL_SendCMDRunRestartBootloader();
+ break;
+ case imSK:
+ break;
+ }
+ blheli_send_reply(&blheli.chan, 1);
+ setDisconnected();
+ break;
+ }
+
+ 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;
+ }
+ }
+ uint8_t buf[4] = {blheli.deviceInfo[0],
+ blheli.deviceInfo[1],
+ blheli.deviceInfo[2],
+ blheli.deviceInfo[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);
+ break;
+ }
+
+ case cmd_DeviceRead: {
+ 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;
+ 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", page, blheli.interface_mode);
+ switch (blheli.interface_mode) {
+ case imSIL_BLB:
+ case imARM_BLB: {
+ if (blheli.interface_mode == imARM_BLB) {
+ // Address =Page * 1024
+ blheli.address = page << 10;
+ } else {
+ // Address =Page * 512
+ blheli.address = page << 9;
+ }
+ debug("ARM PageErase 0x%04x", 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", nbytes, blheli.interface_mode);
+ uint8_t buf[nbytes];
+ memcpy(buf, blheli.buf, nbytes);
+ switch (blheli.interface_mode) {
+ case imSIL_BLB:
+ case imATM_BLB:
+ case imARM_BLB: {
+ BL_WriteFlash(buf, nbytes);
+ break;
+ }
+ case imSK: {
+ debug("Unsupported flash mode imSK");
+ 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", nbytes, blheli.interface_mode);
+ switch (blheli.interface_mode) {
+ 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", nbytes, blheli.interface_mode);
+ switch (blheli.interface_mode) {
+ 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", nbytes, blheli.interface_mode);
+ switch (blheli.interface_mode) {
+ 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", 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");
+ msp.escMode = PROTOCOL_NONE;
+ hal.rcout->serial_end();
+ serial_started = false;
+ }
+ if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') {
+ debug("Change to BLHeli mode");
+ 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();
+ 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;
+ last_valid_ms = AP_HAL::millis();
+ msp_process_command();
+ }
+ msp.state = MSP_IDLE;
+ blheli.state = BLHELI_IDLE;
+ }
+
+ return valid_packet;
+}
+
+/*
+ protocol handler for detecting BLHeli input
+ */
+bool AP_BLHeli::protocol_handler(uint8_t b, AP_HAL::UARTDriver *_uart)
+{
+ uart = _uart;
+ return process_input(b);
+}
+
+/*
+ update BLHeli
+ Used to install protocol handler
+ */
+void AP_BLHeli::update(void)
+{
+ if (initialised && serial_started && AP_HAL::millis() - last_valid_ms > 4000) {
+ // we're not processing requests any more, shutdown serial
+ // output
+ hal.rcout->serial_end();
+ serial_started = false;
+ }
+ if (initialised || channel_mask.get() == 0) {
+ return;
+ }
+ initialised = true;
+
+ if (gcs().install_alternative_protocol(MAVLINK_COMM_0,
+ FUNCTOR_BIND_MEMBER(&AP_BLHeli::protocol_handler,
+ bool, uint8_t, AP_HAL::UARTDriver *))) {
+ debug("BLHeli installed");
+ }
+
+ // for testing without vehicle code
+ uint16_t mask = uint16_t(channel_mask.get());
+ for (uint8_t i=0; i<16 && num_motors < max_motors; i++) {
+ if (mask & (1U<.
+ */
+/*
+ implementation of MSP and BLHeli-4way protocols for pass-through ESC
+ calibration and firmware update
+
+ With thanks to betaflight for a great reference implementation
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include "msp_protocol.h"
+#include "blheli_4way_protocol.h"
+
+class AP_BLHeli {
+
+public:
+ AP_BLHeli();
+
+ void update(void);
+ bool process_input(uint8_t b);
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+ // mask of channels to use for BLHeli protocol
+ AP_Int32 channel_mask;
+
+ enum mspState {
+ MSP_IDLE=0,
+ MSP_HEADER_START,
+ MSP_HEADER_M,
+ MSP_HEADER_ARROW,
+ MSP_HEADER_SIZE,
+ MSP_HEADER_CMD,
+ MSP_COMMAND_RECEIVED
+ };
+
+ enum mspPacketType {
+ MSP_PACKET_COMMAND,
+ MSP_PACKET_REPLY
+ };
+
+ enum escProtocol {
+ PROTOCOL_SIMONK = 0,
+ PROTOCOL_BLHELI = 1,
+ PROTOCOL_KISS = 2,
+ PROTOCOL_KISSALL = 3,
+ PROTOCOL_CASTLE = 4,
+ PROTOCOL_MAX = 5,
+ PROTOCOL_NONE = 0xfe,
+ PROTOCOL_4WAY = 0xff
+ };
+
+ enum motorPwmProtocol {
+ PWM_TYPE_STANDARD = 0,
+ PWM_TYPE_ONESHOT125,
+ PWM_TYPE_ONESHOT42,
+ PWM_TYPE_MULTISHOT,
+ PWM_TYPE_BRUSHED,
+ PWM_TYPE_DSHOT150,
+ PWM_TYPE_DSHOT300,
+ PWM_TYPE_DSHOT600,
+ PWM_TYPE_DSHOT1200,
+ PWM_TYPE_PROSHOT1000,
+ };
+
+ enum MSPFeatures {
+ FEATURE_RX_PPM = 1 << 0,
+ FEATURE_INFLIGHT_ACC_CAL = 1 << 2,
+ FEATURE_RX_SERIAL = 1 << 3,
+ FEATURE_MOTOR_STOP = 1 << 4,
+ FEATURE_SERVO_TILT = 1 << 5,
+ FEATURE_SOFTSERIAL = 1 << 6,
+ FEATURE_GPS = 1 << 7,
+ FEATURE_RANGEFINDER = 1 << 9,
+ FEATURE_TELEMETRY = 1 << 10,
+ FEATURE_3D = 1 << 12,
+ FEATURE_RX_PARALLEL_PWM = 1 << 13,
+ FEATURE_RX_MSP = 1 << 14,
+ FEATURE_RSSI_ADC = 1 << 15,
+ FEATURE_LED_STRIP = 1 << 16,
+ FEATURE_DASHBOARD = 1 << 17,
+ FEATURE_OSD = 1 << 18,
+ FEATURE_CHANNEL_FORWARDING = 1 << 20,
+ FEATURE_TRANSPONDER = 1 << 21,
+ FEATURE_AIRMODE = 1 << 22,
+ FEATURE_RX_SPI = 1 << 25,
+ FEATURE_SOFTSPI = 1 << 26,
+ FEATURE_ESC_SENSOR = 1 << 27,
+ FEATURE_ANTI_GRAVITY = 1 << 28,
+ FEATURE_DYNAMIC_FILTER = 1 << 29,
+ };
+
+
+ /*
+ state of MSP command processing
+ */
+ struct {
+ enum mspState state;
+ enum mspPacketType packetType;
+ uint8_t offset;
+ uint8_t dataSize;
+ uint8_t checksum;
+ uint8_t buf[192];
+ uint8_t cmdMSP;
+ enum escProtocol escMode;
+ uint8_t portIndex;
+ } msp;
+
+ enum blheliState {
+ BLHELI_IDLE=0,
+ BLHELI_HEADER_START,
+ BLHELI_HEADER_CMD,
+ BLHELI_HEADER_ADDR_LOW,
+ BLHELI_HEADER_ADDR_HIGH,
+ BLHELI_HEADER_LEN,
+ BLHELI_CRC1,
+ BLHELI_CRC2,
+ BLHELI_COMMAND_RECEIVED
+ };
+
+ /*
+ state of blheli 4way protocol handling
+ */
+ struct {
+ enum blheliState state;
+ uint8_t command;
+ uint16_t address;
+ uint16_t param_len;
+ uint16_t offset;
+ uint8_t buf[256+3+8];
+ uint8_t crc1;
+ uint16_t crc;
+ uint8_t interface_mode;
+ uint8_t deviceInfo[4];
+ uint8_t chan;
+ uint8_t ack;
+ } blheli;
+
+ AP_HAL::UARTDriver *uart;
+
+ static const uint8_t max_motors = 8;
+ uint8_t num_motors;
+
+ // have we initialised the interface?
+ bool initialised;
+
+ // last valid packet timestamp
+ uint32_t last_valid_ms;
+
+ // have we started serial ESC output?
+ bool serial_started;
+
+ // mapping from BLHeli motor numbers to RC output channels
+ uint8_t motor_map[max_motors];
+
+ bool msp_process_byte(uint8_t c);
+ void blheli_crc_update(uint8_t c);
+ bool blheli_4way_process_byte(uint8_t c);
+ void msp_send_reply(uint8_t cmd, const uint8_t *buf, uint8_t len);
+ void putU16(uint8_t *b, uint16_t v);
+ uint16_t getU16(const uint8_t *b);
+ void putU32(uint8_t *b, uint32_t v);
+ void putU16_BE(uint8_t *b, uint16_t v);
+ void msp_process_command(void);
+ void blheli_send_reply(const uint8_t *buf, uint16_t len);
+ uint16_t BL_CRC(const uint8_t *buf, uint16_t len);
+ bool isMcuConnected(void);
+ void setDisconnected(void);
+ bool BL_SendBuf(const uint8_t *buf, uint16_t len);
+ bool BL_ReadBuf(uint8_t *buf, uint16_t len);
+ uint8_t BL_GetACK(uint16_t timeout_ms=2);
+ bool BL_SendCMDSetAddress();
+ bool BL_ReadA(uint8_t cmd, uint8_t *buf, uint16_t n);
+ bool BL_ConnectEx(void);
+ bool BL_SendCMDKeepAlive(void);
+ bool BL_PageErase(void);
+ void BL_SendCMDRunRestartBootloader(void);
+ uint8_t BL_SendCMDSetBuffer(const uint8_t *buf, uint16_t nbytes);
+ bool BL_WriteA(uint8_t cmd, const uint8_t *buf, uint16_t nbytes, uint32_t timeout);
+ uint8_t BL_WriteFlash(const uint8_t *buf, uint16_t n);
+ bool BL_VerifyFlash(const uint8_t *buf, uint16_t n);
+ void blheli_process_command(void);
+
+ // protocol handler hook
+ bool protocol_handler(uint8_t , AP_HAL::UARTDriver *);
+};
+
+
+// start of 12 byte CPU ID
+#ifndef UDID_START
+#define UDID_START 0x1FFF7A10
+#endif
diff --git a/libraries/AP_BLHeli/blheli_4way_protocol.h b/libraries/AP_BLHeli/blheli_4way_protocol.h
new file mode 100644
index 0000000000..08a6f6018b
--- /dev/null
+++ b/libraries/AP_BLHeli/blheli_4way_protocol.h
@@ -0,0 +1,171 @@
+/*
+ blheli 4way protocol. Based on serial_4way.c from betaflight
+ */
+/*
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ * Author: 4712
+*/
+
+// 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
+
+/*
+ local defines
+ */
+#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)
+
+#define brSUCCESS 0x30
+#define brERRORVERIFY 0xC0
+#define brERRORCOMMAND 0xC1
+#define brERRORCRC 0xC2
+#define brNONE 0xFF
+
+#define CMD_RUN 0x00
+#define CMD_PROG_FLASH 0x01
+#define CMD_ERASE_FLASH 0x02
+#define CMD_READ_FLASH_SIL 0x03
+#define CMD_VERIFY_FLASH 0x03
+#define CMD_VERIFY_FLASH_ARM 0x04
+#define CMD_READ_EEPROM 0x04
+#define CMD_PROG_EEPROM 0x05
+#define CMD_READ_SRAM 0x06
+#define CMD_READ_FLASH_ATM 0x07
+#define CMD_KEEP_ALIVE 0xFD
+#define CMD_SET_ADDRESS 0xFF
+#define CMD_SET_BUFFER 0xFE
+
+#define RestartBootloader 0
+#define ExitBootloader 1
+
+#define ACK_OK 0x00
+#define ACK_I_INVALID_CMD 0x02
+#define ACK_I_INVALID_CRC 0x03
+#define ACK_I_VERIFY_ERROR 0x04
+#define ACK_I_INVALID_CHANNEL 0x08
+#define ACK_I_INVALID_PARAM 0x09
+#define ACK_D_GENERAL_ERROR 0x0F
+
+// interface modes
+#define imC2 0
+#define imSIL_BLB 1
+#define imATM_BLB 2
+#define imSK 3
+#define imARM_BLB 4
diff --git a/libraries/AP_BLHeli/examples/BLHeliTest/BLHeliTest.cpp b/libraries/AP_BLHeli/examples/BLHeliTest/BLHeliTest.cpp
new file mode 100644
index 0000000000..462d342466
--- /dev/null
+++ b/libraries/AP_BLHeli/examples/BLHeliTest/BLHeliTest.cpp
@@ -0,0 +1,49 @@
+/*
+ example implementing MSP and BLHeli passthrough protocol in ArduPilot
+
+ With thanks to betaflight for a great reference implementation
+ */
+
+#include
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+void setup();
+void loop();
+
+static AP_BLHeli esc_serial;
+
+void setup(void) {
+ hal.console->begin(115200);
+ hal.scheduler->delay(1000);
+ hal.console->printf("ESCSerial Starting\n");
+
+ hal.uartA->begin(115200, 1024, 1024);
+ hal.uartC->begin(115200, 256, 256);
+
+ esc_serial.init(hal.uartA, hal.uartC);
+
+ hal.rcout->init();
+ hal.rcout->set_esc_scaling(1000, 2000);
+ hal.rcout->set_output_mode(0xF, AP_HAL::RCOutput::MODE_PWM_NORMAL);
+ hal.rcout->set_freq(0xF, 400);
+ hal.rcout->cork();
+ for (uint8_t i=0; i<4; i++) {
+ hal.rcout->enable_ch(i);
+ hal.rcout->write(i, 1000);
+ }
+ hal.rcout->push();
+}
+
+void loop(void)
+{
+ int16_t b = hal.uartA->read();
+ if (b == -1) {
+ return;
+ }
+ esc_serial.process_input(b);
+}
+
+AP_HAL_MAIN();
diff --git a/libraries/AP_BLHeli/examples/BLHeliTest/wscript b/libraries/AP_BLHeli/examples/BLHeliTest/wscript
new file mode 100644
index 0000000000..719ec151ba
--- /dev/null
+++ b/libraries/AP_BLHeli/examples/BLHeliTest/wscript
@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+ bld.ap_example(
+ use='ap',
+ )
diff --git a/libraries/AP_BLHeli/msp_protocol.h b/libraries/AP_BLHeli/msp_protocol.h
new file mode 100644
index 0000000000..cda3bcfbff
--- /dev/null
+++ b/libraries/AP_BLHeli/msp_protocol.h
@@ -0,0 +1,335 @@
+/*
+ msp_protocol.h imported from betaflight for use in ArduPilot
+
+ Many thanks to tbe Cleanflight and Betaflight developers for a great
+ reference implementation of this protocol
+ */
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/**
+ * MSP Guidelines, emphasis is used to clarify.
+ *
+ * Each FlightController (FC, Server) MUST change the API version when any MSP command is added, deleted, or changed.
+ *
+ * If you fork the FC source code and release your own version, you MUST change the Flight Controller Identifier.
+ *
+ * NEVER release a modified copy of this code that shares the same Flight controller IDENT and API version
+ * if the API doesn't match EXACTLY.
+ *
+ * Consumers of the API (API clients) SHOULD first attempt to get a response from the MSP_API_VERSION command.
+ * If no response is obtained then client MAY try the legacy MSP_IDENT command.
+ *
+ * API consumers should ALWAYS handle communication failures gracefully and attempt to continue
+ * without the information if possible. Clients MAY log/display a suitable message.
+ *
+ * API clients should NOT attempt any communication if they can't handle the returned API MAJOR VERSION.
+ *
+ * API clients SHOULD attempt communication if the API MINOR VERSION has increased from the time
+ * the API client was written and handle command failures gracefully. Clients MAY disable
+ * functionality that depends on the commands while still leaving other functionality intact.
+ * that the newer API version may cause problems before using API commands that change FC state.
+ *
+ * It is for this reason that each MSP command should be specific as possible, such that changes
+ * to commands break as little functionality as possible.
+ *
+ * API client authors MAY use a compatibility matrix/table when determining if they can support
+ * a given command from a given flight controller at a given api version level.
+ *
+ * Developers MUST NOT create new MSP commands that do more than one thing.
+ *
+ * Failure to follow these guidelines will likely invoke the wrath of developers trying to write tools
+ * that use the API and the users of those tools.
+ */
+
+#pragma once
+
+/* Protocol numbers used both by the wire format, config system, and
+ field setters.
+*/
+
+#define MSP_PROTOCOL_VERSION 0
+
+#define API_VERSION_MAJOR 1 // increment when major changes are made
+#define API_VERSION_MINOR 37 // increment after a release, to set the version for all changes to go into the following release (if no changes to MSP are made between the releases, this can be reverted before the release)
+
+#define API_VERSION_LENGTH 2
+
+#define MULTIWII_IDENTIFIER "MWII";
+#define BASEFLIGHT_IDENTIFIER "BAFL";
+#define BETAFLIGHT_IDENTIFIER "BTFL"
+#define CLEANFLIGHT_IDENTIFIER "CLFL"
+#define INAV_IDENTIFIER "INAV"
+#define RACEFLIGHT_IDENTIFIER "RCFL"
+#define ARDUPILOT_IDENTIFIER "ARDU"
+
+#define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
+#define FLIGHT_CONTROLLER_VERSION_LENGTH 3
+#define FLIGHT_CONTROLLER_VERSION_MASK 0xFFF
+
+#define BOARD_IDENTIFIER_LENGTH 4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
+#define BOARD_HARDWARE_REVISION_LENGTH 2
+
+// These are baseflight specific flags but they are useless now since MW 2.3 uses the upper 4 bits for the navigation version.
+#define CAP_PLATFORM_32BIT ((uint32_t)1 << 31)
+#define CAP_BASEFLIGHT_CONFIG ((uint32_t)1 << 30)
+
+// MW 2.3 stores NAVI_VERSION in the top 4 bits of the capability mask.
+#define CAP_NAVI_VERSION_BIT_4_MSB ((uint32_t)1 << 31)
+#define CAP_NAVI_VERSION_BIT_3 ((uint32_t)1 << 30)
+#define CAP_NAVI_VERSION_BIT_2 ((uint32_t)1 << 29)
+#define CAP_NAVI_VERSION_BIT_1_LSB ((uint32_t)1 << 28)
+
+#define CAP_DYNBALANCE ((uint32_t)1 << 2)
+#define CAP_FLAPS ((uint32_t)1 << 3)
+#define CAP_NAVCAP ((uint32_t)1 << 4)
+#define CAP_EXTAUX ((uint32_t)1 << 5)
+
+#define MSP_API_VERSION 1 //out message
+#define MSP_FC_VARIANT 2 //out message
+#define MSP_FC_VERSION 3 //out message
+#define MSP_BOARD_INFO 4 //out message
+#define MSP_BUILD_INFO 5 //out message
+
+#define MSP_NAME 10 //out message Returns user set board name - betaflight
+#define MSP_SET_NAME 11 //in message Sets board name - betaflight
+
+//
+// MSP commands for Cleanflight original features
+//
+#define MSP_BATTERY_CONFIG 32
+#define MSP_SET_BATTERY_CONFIG 33
+
+#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
+#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
+
+#define MSP_FEATURE_CONFIG 36
+#define MSP_SET_FEATURE_CONFIG 37
+
+#define MSP_BOARD_ALIGNMENT_CONFIG 38
+#define MSP_SET_BOARD_ALIGNMENT_CONFIG 39
+
+#define MSP_CURRENT_METER_CONFIG 40
+#define MSP_SET_CURRENT_METER_CONFIG 41
+
+#define MSP_MIXER_CONFIG 42
+#define MSP_SET_MIXER_CONFIG 43
+
+#define MSP_RX_CONFIG 44
+#define MSP_SET_RX_CONFIG 45
+
+#define MSP_LED_COLORS 46
+#define MSP_SET_LED_COLORS 47
+
+#define MSP_LED_STRIP_CONFIG 48
+#define MSP_SET_LED_STRIP_CONFIG 49
+
+#define MSP_RSSI_CONFIG 50
+#define MSP_SET_RSSI_CONFIG 51
+
+#define MSP_ADJUSTMENT_RANGES 52
+#define MSP_SET_ADJUSTMENT_RANGE 53
+
+// private - only to be used by the configurator, the commands are likely to change
+#define MSP_CF_SERIAL_CONFIG 54
+#define MSP_SET_CF_SERIAL_CONFIG 55
+
+#define MSP_VOLTAGE_METER_CONFIG 56
+#define MSP_SET_VOLTAGE_METER_CONFIG 57
+
+#define MSP_SONAR_ALTITUDE 58 //out message get sonar altitude [cm]
+
+#define MSP_PID_CONTROLLER 59
+#define MSP_SET_PID_CONTROLLER 60
+
+#define MSP_ARMING_CONFIG 61
+#define MSP_SET_ARMING_CONFIG 62
+
+//
+// Baseflight MSP commands (if enabled they exist in Cleanflight)
+//
+#define MSP_RX_MAP 64 //out message get channel map (also returns number of channels total)
+#define MSP_SET_RX_MAP 65 //in message set rx map, numchannels to set comes from MSP_RX_MAP
+
+// FIXME - Provided for backwards compatibility with configurator code until configurator is updated.
+// DEPRECATED - DO NOT USE "MSP_BF_CONFIG" and MSP_SET_BF_CONFIG. In Cleanflight, isolated commands already exist and should be used instead.
+#define MSP_BF_CONFIG 66 //out message baseflight-specific settings that aren't covered elsewhere
+#define MSP_SET_BF_CONFIG 67 //in message baseflight-specific settings save
+
+#define MSP_REBOOT 68 //in message reboot settings
+
+// Use MSP_BUILD_INFO instead
+// DEPRECATED - #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion
+
+#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip
+#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip
+#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip
+
+// No-longer needed
+// DEPRECATED - #define MSP_LOOP_TIME 73 //out message Returns FC cycle time i.e looptime parameter // DEPRECATED
+// DEPRECATED - #define MSP_SET_LOOP_TIME 74 //in message Sets FC cycle time i.e looptime parameter // DEPRECATED
+
+#define MSP_FAILSAFE_CONFIG 75 //out message Returns FC Fail-Safe settings
+#define MSP_SET_FAILSAFE_CONFIG 76 //in message Sets FC Fail-Safe settings
+
+#define MSP_RXFAIL_CONFIG 77 //out message Returns RXFAIL settings
+#define MSP_SET_RXFAIL_CONFIG 78 //in message Sets RXFAIL settings
+
+#define MSP_SDCARD_SUMMARY 79 //out message Get the state of the SD card
+
+#define MSP_BLACKBOX_CONFIG 80 //out message Get blackbox settings
+#define MSP_SET_BLACKBOX_CONFIG 81 //in message Set blackbox settings
+
+#define MSP_TRANSPONDER_CONFIG 82 //out message Get transponder settings
+#define MSP_SET_TRANSPONDER_CONFIG 83 //in message Set transponder settings
+
+#define MSP_OSD_CONFIG 84 //out message Get osd settings - betaflight
+#define MSP_SET_OSD_CONFIG 85 //in message Set osd settings - betaflight
+
+#define MSP_OSD_CHAR_READ 86 //out message Get osd settings - betaflight
+#define MSP_OSD_CHAR_WRITE 87 //in message Set osd settings - betaflight
+
+#define MSP_VTX_CONFIG 88 //out message Get vtx settings - betaflight
+#define MSP_SET_VTX_CONFIG 89 //in message Set vtx settings - betaflight
+
+// Betaflight Additional Commands
+#define MSP_ADVANCED_CONFIG 90
+#define MSP_SET_ADVANCED_CONFIG 91
+
+#define MSP_FILTER_CONFIG 92
+#define MSP_SET_FILTER_CONFIG 93
+
+#define MSP_PID_ADVANCED 94
+#define MSP_SET_PID_ADVANCED 95
+
+#define MSP_SENSOR_CONFIG 96
+#define MSP_SET_SENSOR_CONFIG 97
+
+#define MSP_CAMERA_CONTROL 98
+
+#define MSP_SET_ARMING_DISABLED 99
+
+//
+// OSD specific
+//
+#define MSP_OSD_VIDEO_CONFIG 180
+#define MSP_SET_OSD_VIDEO_CONFIG 181
+
+// External OSD displayport mode messages
+#define MSP_DISPLAYPORT 182
+
+#define MSP_COPY_PROFILE 183
+
+#define MSP_BEEPER_CONFIG 184
+#define MSP_SET_BEEPER_CONFIG 185
+
+#define MSP_SET_TX_INFO 186 // in message Used to send runtime information from TX lua scripts to the firmware
+#define MSP_TX_INFO 187 // out message Used by TX lua scripts to read information from the firmware
+
+//
+// Multwii original MSP commands
+//
+
+// See MSP_API_VERSION and MSP_MIXER_CONFIG
+//DEPRECATED - #define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
+
+
+#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
+#define MSP_RAW_IMU 102 //out message 9 DOF
+#define MSP_SERVO 103 //out message servos
+#define MSP_MOTOR 104 //out message motors
+#define MSP_RC 105 //out message rc channels and more
+#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
+#define MSP_COMP_GPS 107 //out message distance home, direction home
+#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
+#define MSP_ALTITUDE 109 //out message altitude, variometer
+#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
+#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
+#define MSP_PID 112 //out message P I D coeff (9 are used currently)
+// Legacy Multiicommand that was never used.
+//DEPRECATED - #define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
+// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead.
+//DEPRECATED - #define MSP_MISC 114 //out message powermeter trig
+// Legacy Multiicommand that was never used and always wrong
+//DEPRECATED - #define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
+#define MSP_BOXNAMES 116 //out message the aux switch names
+#define MSP_PIDNAMES 117 //out message the PID names
+#define MSP_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
+#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
+#define MSP_SERVO_CONFIGURATIONS 120 //out message All servo configurations.
+#define MSP_NAV_STATUS 121 //out message Returns navigation status
+#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
+#define MSP_MOTOR_3D_CONFIG 124 //out message Settings needed for reversible ESCs
+#define MSP_RC_DEADBAND 125 //out message deadbands for yaw alt pitch roll
+#define MSP_SENSOR_ALIGNMENT 126 //out message orientation of acc,gyro,mag
+#define MSP_LED_STRIP_MODECOLOR 127 //out message Get LED strip mode_color settings
+#define MSP_VOLTAGE_METERS 128 //out message Voltage (per meter)
+#define MSP_CURRENT_METERS 129 //out message Amperage (per meter)
+#define MSP_BATTERY_STATE 130 //out message Connected/Disconnected, Voltage, Current Used
+#define MSP_MOTOR_CONFIG 131 //out message Motor configuration (min/max throttle, etc)
+#define MSP_GPS_CONFIG 132 //out message GPS configuration
+#define MSP_COMPASS_CONFIG 133 //out message Compass configuration
+#define MSP_ESC_SENSOR_DATA 134 //out message Extra ESC data from 32-Bit ESCs (Temperature, RPM)
+
+#define MSP_SET_RAW_RC 200 //in message 8 rc chan
+#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
+#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
+// Legacy multiiwii command that was never used.
+//DEPRECATED - #define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
+#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID, yaw expo
+#define MSP_ACC_CALIBRATION 205 //in message no param
+#define MSP_MAG_CALIBRATION 206 //in message no param
+// Legacy command that was under constant change due to the naming vagueness, avoid at all costs - use more specific commands instead.
+//DEPRECATED - #define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
+#define MSP_RESET_CONF 208 //in message no param
+#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
+#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
+#define MSP_SET_HEADING 211 //in message define a new heading hold direction
+#define MSP_SET_SERVO_CONFIGURATION 212 //in message Servo settings
+#define MSP_SET_MOTOR 214 //in message PropBalance function
+#define MSP_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
+#define MSP_SET_MOTOR_3D_CONFIG 217 //in message Settings needed for reversible ESCs
+#define MSP_SET_RC_DEADBAND 218 //in message deadbands for yaw alt pitch roll
+#define MSP_SET_RESET_CURR_PID 219 //in message resetting the current pid profile to defaults
+#define MSP_SET_SENSOR_ALIGNMENT 220 //in message set the orientation of the acc,gyro,mag
+#define MSP_SET_LED_STRIP_MODECOLOR 221 //in message Set LED strip mode_color settings
+#define MSP_SET_MOTOR_CONFIG 222 //out message Motor configuration (min/max throttle, etc)
+#define MSP_SET_GPS_CONFIG 223 //out message GPS configuration
+#define MSP_SET_COMPASS_CONFIG 224 //out message Compass configuration
+
+// #define MSP_BIND 240 //in message no param
+// #define MSP_ALARMS 242
+
+#define MSP_EEPROM_WRITE 250 //in message no param
+#define MSP_RESERVE_1 251 //reserved for system usage
+#define MSP_RESERVE_2 252 //reserved for system usage
+#define MSP_DEBUGMSG 253 //out message debug string buffer
+#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
+#define MSP_RESERVE_3 255 //reserved for system usage
+
+// Additional commands that are not compatible with MultiWii
+#define MSP_STATUS_EX 150 //out message cycletime, errors_count, CPU load, sensor present etc
+#define MSP_UID 160 //out message Unique device ID
+#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
+#define MSP_GPSSTATISTICS 166 //out message get GPS debugging data
+#define MSP_ACC_TRIM 240 //out message get acc angle trim values
+#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
+#define MSP_SERVO_MIX_RULES 241 //out message Returns servo mixer configuration
+#define MSP_SET_SERVO_MIX_RULE 242 //in message Sets servo mixer configuration
+#define MSP_SET_4WAY_IF 245 //in message Sets 4way interface
+#define MSP_SET_RTC 246 //in message Sets the RTC clock
+#define MSP_RTC 247 //out message Gets the RTC clock