Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_BLHeli: added ESC serial protocol implementation 

this implements the MSP and BLHeli protocols for passthru control of
BLHeli ESCs
This commit is contained in:
Andrew Tridgell 2018-03-22 11:44:55 +11:00
parent 67bae2669b
commit a1a83ab768
6 changed files with 1770 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

998
libraries/AP_BLHeli/AP_BLHeli.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
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 <AP_Math/crc.h>
#include <AP_Motors/AP_Motors_Class.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
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; i<len+2; i++) {
c ^= msp.buf[i+3];
}
*b++ = c;
uart->write(&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<<i)) {
motor_map[num_motors] = i;
num_motors++;
}
}
debug("ESC: mapped %u motors", num_motors);
}

210
libraries/AP_BLHeli/AP_BLHeli.h Normal file
View File

@ -0,0 +1,210 @@
/*
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 <http://www.gnu.org/licenses/>.
*/
/*
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 <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#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

171
libraries/AP_BLHeli/blheli_4way_protocol.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
* 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

49
libraries/AP_BLHeli/examples/BLHeliTest/BLHeliTest.cpp Normal file
View File

@ -0,0 +1,49 @@
/*
example implementing MSP and BLHeli passthrough protocol in ArduPilot
With thanks to betaflight for a great reference implementation
*/
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_BLHeli/AP_BLHeli.h>
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();

7
libraries/AP_BLHeli/examples/BLHeliTest/wscript Normal file
View File

@ -0,0 +1,7 @@
#!/usr/bin/env python
# encoding: utf-8
def build(bld):
bld.ap_example(
use='ap',
)

335
libraries/AP_BLHeli/msp_protocol.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/**
* 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