zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_BLHeli: added ESC serial protocol implementation 

this implements the MSP and BLHeli protocols for passthru control of
BLHeli ESCs
master
Andrew Tridgell 7 years ago
parent
67bae2669b
commit
a1a83ab768
6 changed files with 1770 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 998
      libraries/AP_BLHeli/AP_BLHeli.cpp
  2. 210
      libraries/AP_BLHeli/AP_BLHeli.h
  3. 171
      libraries/AP_BLHeli/blheli_4way_protocol.h
  4. 49
      libraries/AP_BLHeli/examples/BLHeliTest/BLHeliTest.cpp
  5. 7
      libraries/AP_BLHeli/examples/BLHeliTest/wscript
  6. 335
      libraries/AP_BLHeli/msp_protocol.h

998
libraries/AP_BLHeli/AP_BLHeli.cpp
Unescape View File

@ -0,0 +1,998 @@ @@ -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
Unescape View File

@ -0,0 +1,210 @@ @@ -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
Unescape View File

@ -0,0 +1,171 @@ @@ -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
Unescape View File

@ -0,0 +1,49 @@ @@ -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
Unescape View File

@ -0,0 +1,7 @@ @@ -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
Unescape View File

@ -0,0 +1,335 @@ @@ -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
Write Preview
Loading…
Cancel
Save