From a1a83ab768901f2d98934fcc66dad682d1cd5d0b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Mar 2018 11:44:55 +1100 Subject: [PATCH] AP_BLHeli: added ESC serial protocol implementation this implements the MSP and BLHeli protocols for passthru control of BLHeli ESCs --- libraries/AP_BLHeli/AP_BLHeli.cpp | 998 ++++++++++++++++++ libraries/AP_BLHeli/AP_BLHeli.h | 210 ++++ libraries/AP_BLHeli/blheli_4way_protocol.h | 171 +++ .../examples/BLHeliTest/BLHeliTest.cpp | 49 + .../AP_BLHeli/examples/BLHeliTest/wscript | 7 + libraries/AP_BLHeli/msp_protocol.h | 335 ++++++ 6 files changed, 1770 insertions(+) create mode 100644 libraries/AP_BLHeli/AP_BLHeli.cpp create mode 100644 libraries/AP_BLHeli/AP_BLHeli.h create mode 100644 libraries/AP_BLHeli/blheli_4way_protocol.h create mode 100644 libraries/AP_BLHeli/examples/BLHeliTest/BLHeliTest.cpp create mode 100644 libraries/AP_BLHeli/examples/BLHeliTest/wscript create mode 100644 libraries/AP_BLHeli/msp_protocol.h 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