ardupilot/libraries/AP_BLHeli/AP_BLHeli.cpp

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

1623 lines
54 KiB
C++
Raw Normal View History

/*
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. Several of the functions below are based on
betaflight equivalent functions
*/
#include "AP_BLHeli.h"
#if HAVE_AP_BLHELI_SUPPORT
2022-02-20 23:46:11 -04:00
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#include <hal.h>
#endif
#include <AP_Math/crc.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
2021-05-29 08:52:08 -03:00
#if APM_BUILD_TYPE(APM_BUILD_Rover)
2021-07-18 11:04:07 -03:00
#include <AR_Motors/AP_MotorsUGV.h>
2021-05-29 08:52:08 -03:00
#else
#include <AP_Motors/AP_Motors_Class.h>
2021-05-29 08:52:08 -03:00
#endif
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <SRV_Channel/SRV_Channel.h>
extern const AP_HAL::HAL& hal;
#define debug(fmt, args ...) do { if (debug_level) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: " fmt, ## args); } } while (0)
// key for locking UART for exclusive use. This prevents any other writes from corrupting
// the MSP protocol on hal.console
#define BLHELI_UART_LOCK_KEY 0x20180402
// if no packets are received for this time and motor control is active BLH will disconnect (stoping motors)
#define MOTOR_ACTIVE_TIMEOUT 1000
const AP_Param::GroupInfo AP_BLHeli::var_info[] = {
// @Param: MASK
// @DisplayName: BLHeli Channel Bitmask
// @Description: Enable of BLHeli pass-thru servo protocol support to specific channels. This mask is in addition to motors enabled using SERVO_BLH_AUTO (if any)
// @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, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("MASK", 1, AP_BLHeli, channel_mask, 0),
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
// @Param: AUTO
// @DisplayName: BLHeli pass-thru auto-enable for multicopter motors
// @Description: If set to 1 this auto-enables BLHeli pass-thru support for all multicopter motors
// @Values: 0:Disabled,1:Enabled
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO("AUTO", 2, AP_BLHeli, channel_auto, 0),
#endif
// @Param: TEST
// @DisplayName: BLHeli internal interface test
2018-04-01 20:26:55 -03:00
// @Description: Setting SERVO_BLH_TEST to a motor number enables an internal test of the BLHeli ESC protocol to the corresponding ESC. The debug output is displayed on the USB console.
// @Values: 0:Disabled,1:TestMotor1,2:TestMotor2,3:TestMotor3,4:TestMotor4,5:TestMotor5,6:TestMotor6,7:TestMotor7,8:TestMotor8
// @User: Advanced
AP_GROUPINFO("TEST", 3, AP_BLHeli, run_test, 0),
// @Param: TMOUT
// @DisplayName: BLHeli protocol timeout
// @Description: This sets the inactivity timeout for the BLHeli protocol in seconds. If no packets are received in this time normal MAVLink operations are resumed. A value of 0 means no timeout
// @Units: s
// @Range: 0 300
// @User: Standard
AP_GROUPINFO("TMOUT", 4, AP_BLHeli, timeout_sec, 0),
// @Param: TRATE
// @DisplayName: BLHeli telemetry rate
// @Description: This sets the rate in Hz for requesting telemetry from ESCs. It is the rate per ESC. Setting to zero disables telemetry requests
// @Units: Hz
// @Range: 0 500
// @User: Standard
AP_GROUPINFO("TRATE", 5, AP_BLHeli, telem_rate, 10),
// @Param: DEBUG
// @DisplayName: BLHeli debug level
// @Description: When set to 1 this enabled verbose debugging output over MAVLink when the blheli protocol is active. This can be used to diagnose failures.
// @Values: 0:Disabled,1:Enabled
// @User: Standard
AP_GROUPINFO("DEBUG", 6, AP_BLHeli, debug_level, 0),
// @Param: OTYPE
// @DisplayName: BLHeli output type override
// @Description: When set to a non-zero value this overrides the output type for the output channels given by SERVO_BLH_MASK. This can be used to enable DShot on outputs that are not part of the multicopter motors group.
// @Values: 0:None,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("OTYPE", 7, AP_BLHeli, output_type, 0),
// @Param: PORT
// @DisplayName: Control port
// @Description: This sets the mavlink channel to use for blheli pass-thru. The channel number is determined by the number of serial ports configured to use mavlink. So 0 is always the console, 1 is the next serial port using mavlink, 2 the next after that and so on.
// @Values: 0:Console,1:Mavlink Serial Channel1,2:Mavlink Serial Channel2,3:Mavlink Serial Channel3,4:Mavlink Serial Channel4,5:Mavlink Serial Channel5
// @User: Advanced
AP_GROUPINFO("PORT", 8, AP_BLHeli, control_port, 0),
2018-10-19 17:21:36 -03:00
// @Param: POLES
// @DisplayName: BLHeli Motor Poles
2018-10-19 17:21:36 -03:00
// @Description: This allows calculation of true RPM from ESC's eRPM. The default is 14.
// @Range: 1 127
2018-10-19 17:21:36 -03:00
// @User: Advanced
// @RebootRequired: True
2018-10-19 17:21:36 -03:00
AP_GROUPINFO("POLES", 9, AP_BLHeli, motor_poles, 14),
// @Param: 3DMASK
// @DisplayName: BLHeli bitmask of 3D channels
// @Description: Mask of channels which are dynamically reversible. This is used to configure ESCs in '3D' mode, allowing for the motor to spin in either direction. Do not use for channels selected with SERVO_BLH_RVMASK.
// @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, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("3DMASK", 10, AP_BLHeli, channel_reversible_mask, 0),
#if defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT
// @Param: BDMASK
// @DisplayName: BLHeli bitmask of bi-directional dshot channels
// @Description: Mask of channels which support bi-directional dshot telemetry. This is used for ESCs which have firmware that supports bi-directional dshot allowing fast rpm telemetry values to be returned for the harmonic notch.
// @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, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("BDMASK", 11, AP_BLHeli, channel_bidir_dshot_mask, 0),
#endif
// @Param: RVMASK
// @DisplayName: BLHeli bitmask of reversed channels
// @Description: Mask of channels which are reversed. This is used to configure ESCs to reverse motor direction for unidirectional rotation.Do not use for channels selected with SERVO_BLH_RVMASK.Do not use for channels selected with SERVO_BLH_3DMASK.
// @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, 16:Channel 17, 17: Channel 18, 18: Channel 19, 19: Channel 20, 20: Channel 21, 21: Channel 22, 22: Channel 23, 23: Channel 24, 24: Channel 25, 25: Channel 26, 26: Channel 27, 27: Channel 28, 28: Channel 29, 29: Channel 30, 30: Channel 31, 31: Channel 32
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("RVMASK", 12, AP_BLHeli, channel_reversed_mask, 0),
AP_GROUPEND
};
#define RPM_SLEW_RATE 50
AP_BLHeli *AP_BLHeli::_singleton;
// constructor
AP_BLHeli::AP_BLHeli(void)
{
// set defaults from the parameter table
AP_Param::setup_object_defaults(this, var_info);
_singleton = this;
last_control_port = -1;
}
/*
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 ack
*/
void AP_BLHeli::msp_send_ack(uint8_t cmd)
{
msp_send_reply(cmd, 0, 0);
}
/*
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;
// acks do not have a payload
if (len > 0) {
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_locked(&msp.buf[0], len+6, BLHELI_UART_LOCK_KEY);
}
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: {
2018-04-01 20:26:55 -03:00
debug("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;
}
2018-12-17 04:23:27 -04:00
case MSP_FC_VARIANT:
2018-04-01 20:26:55 -03:00
debug("MSP_FC_VARIANT");
msp_send_reply(msp.cmdMSP, (const uint8_t *)ARDUPILOT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
break;
2018-12-17 04:23:27 -04:00
/*
Notes:
version 3.3.1 adds a reply to MSP_SET_MOTOR which was missing
version 3.3.0 requires a workaround in blheli suite to handle MSP_SET_MOTOR without an ack
*/
case MSP_FC_VERSION: {
2018-04-01 20:26:55 -03:00
debug("MSP_FC_VERSION");
uint8_t version[3] = { 3, 3, 1 };
msp_send_reply(msp.cmdMSP, version, sizeof(version));
break;
}
case MSP_BOARD_INFO: {
2018-04-01 20:26:55 -03:00
debug("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: {
2018-04-01 20:26:55 -03:00
debug("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: ignoring reboot command, end serial comms");
hal.rcout->serial_end();
blheli.connected[blheli.chan] = false;
serial_start_ms = 0;
break;
case MSP_UID:
// MCU identifier
2018-04-01 20:26:55 -03:00
debug("MSP_UID");
msp_send_reply(msp.cmdMSP, (const uint8_t *)UDID_START, 12);
break;
// a literal "4" is used for the PWMType here to allow Rover
// to use the same number for the same protocol. At time of
// writing the AP_MotorsUGV::PWMType has not been unified with
// AP_Motors::PWMType.
case MSP_ADVANCED_CONFIG: {
2018-04-01 20:26:55 -03:00
debug("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] = 4; // (uint8_t)AP_Motors::PWMType::DSHOT150;
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: {
2018-04-01 20:26:55 -03:00
debug("MSP_FEATURE_CONFIG");
uint8_t buf[4];
putU32(buf, (channel_reversible_mask.get() != 0) ? FEATURE_3D : 0); // from MSPFeatures enum
msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
break;
}
case MSP_STATUS: {
2018-04-01 20:26:55 -03:00
debug("MSP_STATUS");
uint8_t buf[21];
putU16(&buf[0], 1000); // 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: {
2018-04-01 20:26:55 -03:00
debug("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_BATTERY_STATE: {
debug("MSP_BATTERY_STATE");
uint8_t buf[8];
buf[0] = 4; // cell count
putU16(&buf[1], 1500); // mAh
buf[3] = 16; // V
putU16(&buf[4], 1500); // mAh
putU16(&buf[6], 1); // A
msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
break;
}
case MSP_MOTOR_CONFIG: {
2018-04-01 20:26:55 -03:00
debug("MSP_MOTOR_CONFIG");
uint8_t buf[10];
putU16(&buf[0], 1030); // min throttle
putU16(&buf[2], 2000); // max throttle
putU16(&buf[4], 1000); // min command
// API 1.42
buf[6] = num_motors; // motorCount
buf[7] = motor_poles; // motorPoleCount
buf[8] = 0; // useDshotTelemetry
buf[9] = 0; // FEATURE_ESC_SENSOR
msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
break;
}
case MSP_MOTOR: {
2018-04-01 20:26:55 -03:00
debug("MSP_MOTOR");
// get the output going to each motor
2018-04-01 20:26:55 -03:00
uint8_t buf[16] {};
for (uint8_t i = 0; i < num_motors; i++) {
// if we have a mix of reversible and normal report a PWM of zero, this allows BLHeliSuite to conect
uint16_t v = mixed_type ? 0 : hal.rcout->read(motor_map[i]);
2018-04-01 20:26:55 -03:00
putU16(&buf[2*i], v);
debug("MOTOR %u val: %u",i,v);
}
msp_send_reply(msp.cmdMSP, buf, sizeof(buf));
break;
}
case MSP_SET_MOTOR: {
2018-04-01 20:26:55 -03:00
debug("MSP_SET_MOTOR");
if (!mixed_type) {
// set the output to each motor
uint8_t nmotors = msp.dataSize / 2;
debug("MSP_SET_MOTOR %u", nmotors);
motors_disabled_mask = SRV_Channels::get_disabled_channel_mask();
SRV_Channels::set_disabled_channel_mask(0xFFFF);
motors_disabled = true;
EXPECT_DELAY_MS(1000);
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);
// map from a MSP value to a value in the range 1000 to 2000
uint16_t pwm = (v < 1000)?0:v;
hal.rcout->write(motor_map[i], pwm);
}
hal.rcout->push();
} else {
debug("mixed type, Motors Disabled");
}
msp_send_ack(msp.cmdMSP);
break;
}
2018-12-17 04:23:27 -04:00
case MSP_SET_PASSTHROUGH: {
debug("MSP_SET_PASSTHROUGH");
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];
}
2018-04-02 03:16:37 -03:00
debug("escMode=%u portIndex=%u num_motors=%u", msp.escMode, msp.portIndex, num_motors);
uint8_t n = num_motors;
switch (msp.escMode) {
case PROTOCOL_4WAY:
break;
default:
n = 0;
hal.rcout->serial_end();
serial_start_ms = 0;
break;
}
// doing the serial setup here avoids delays when doing it on demand and makes
// BLHeliSuite considerably more reliable
EXPECT_DELAY_MS(1000);
if (!hal.rcout->serial_setup_output(motor_map[0], 19200, motor_mask)) {
msp_send_ack(ACK_D_GENERAL_ERROR);
break;
} else {
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_locked(&blheli.buf[0], len+8, BLHELI_UART_LOCK_KEY);
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.connected[blheli.chan];
}
void AP_BLHeli::setDisconnected(void)
{
blheli.connected[blheli.chan] = false;
blheli.deviceInfo[blheli.chan][0] = 0;
blheli.deviceInfo[blheli.chan][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;
}
EXPECT_DELAY_MS(1000);
if (!hal.rcout->serial_setup_output(motor_map[blheli.chan], 19200, motor_mask)) {
blheli.ack = ACK_D_GENERAL_ERROR;
return false;
}
if (serial_start_ms == 0) {
serial_start_ms = AP_HAL::millis();
}
uint32_t now = AP_HAL::millis();
if (serial_start_ms == 0 || now - serial_start_ms < 1000) {
/*
we've just started the interface. We want it idle for at
least 1 second before we start sending serial data.
*/
hal.scheduler->delay(1100);
}
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;
}
// 19200 baud is 52us per bit - wait for half a bit between sending and receiving to avoid reading
// the end of the last sent bit by accident
hal.scheduler->delay_microseconds(26);
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);
EXPECT_DELAY_MS(1000);
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();
EXPECT_DELAY_MS(1000);
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;
}
bool ret = BL_ReadBuf(buf, n);
if (ret && n == sizeof(esc_status) && blheli.address == esc_status_addr) {
// display esc_status structure if we see it
struct esc_status status;
memcpy(&status, buf, n);
debug("Prot %u Good %u Bad %u %x %x %x x%x\n",
(unsigned)status.protocol,
(unsigned)status.good_frames,
(unsigned)status.bad_frames,
(unsigned)status.unknown[0],
(unsigned)status.unknown[1],
(unsigned)status.unknown[2],
(unsigned)status.unknown2);
}
return ret;
}
return false;
}
/*
connect to a blheli ESC
*/
bool AP_BLHeli::BL_ConnectEx(void)
{
if (blheli.connected[blheli.chan] != 0) {
debug("Using cached interface 0x%x for %u", blheli.interface_mode[blheli.chan], blheli.chan);
return true;
}
debug("BL_ConnectEx %u/%u at %u", blheli.chan, num_motors, motor_map[blheli.chan]);
setDisconnected();
const uint8_t BootInit[] = {0,0,0,0,0,0,0,0,0,0,0,0,0x0D,'B','L','H','e','l','i',0xF4,0x7D};
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[blheli.chan][2] = BootInfo[3];
blheli.deviceInfo[blheli.chan][1] = BootInfo[4];
blheli.deviceInfo[blheli.chan][0] = BootInfo[5];
blheli.interface_mode[blheli.chan] = 0;
2018-12-17 04:23:27 -04:00
uint16_t devword;
memcpy(&devword, blheli.deviceInfo[blheli.chan], sizeof(devword));
switch (devword) {
case 0x9307:
case 0x930A:
case 0x930F:
case 0x940B:
blheli.interface_mode[blheli.chan] = 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[blheli.chan] = imSIL_BLB;
debug("Interface type imSIL_BLB");
break;
default:
// BLHeli_32 MCU ID hi > 0x00 and < 0x90 / lo always = 0x06
if ((blheli.deviceInfo[blheli.chan][1] > 0x00) && (blheli.deviceInfo[blheli.chan][1] < 0x90) && (blheli.deviceInfo[blheli.chan][0] == 0x06)) {
blheli.interface_mode[blheli.chan] = imARM_BLB;
debug("Interface type imARM_BLB");
} else {
blheli.ack = ACK_D_GENERAL_ERROR;
debug("Unknown interface type 0x%04x", devword);
break;
}
}
blheli.deviceInfo[blheli.chan][3] = blheli.interface_mode[blheli.chan];
if (blheli.interface_mode[blheli.chan] != 0) {
blheli.connected[blheli.chan] = true;
}
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(3000) == brSUCCESS;
}
return false;
}
void AP_BLHeli::BL_SendCMDRunRestartBootloader(void)
{
uint8_t sCMD[] = {RestartBootloader, 0};
blheli.deviceInfo[blheli.chan][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, 500);
}
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_start_ms = 0;
if (motors_disabled) {
motors_disabled = false;
SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
}
if (uart_locked) {
debug("Unlocked UART");
uart->lock_port(0, 0);
uart_locked = false;
}
memset(blheli.connected, 0, sizeof(blheli.connected));
break;
}
case cmd_DeviceReset: {
debug("cmd_DeviceReset(%u)", unsigned(blheli.buf[0]));
if (blheli.buf[0] >= num_motors) {
debug("bad reset channel %u", blheli.buf[0]);
blheli.ack = ACK_I_INVALID_CHANNEL;
blheli_send_reply(&blheli.buf[0], 1);
break;
}
blheli.chan = blheli.buf[0];
switch (blheli.interface_mode[blheli.chan]) {
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]));
if (blheli.buf[0] >= num_motors) {
debug("bad channel %u", blheli.buf[0]);
blheli.ack = ACK_I_INVALID_CHANNEL;
blheli_send_reply(&blheli.buf[0], 1);
break;
}
blheli.chan = blheli.buf[0];
blheli.ack = ACK_OK;
BL_ConnectEx();
uint8_t buf[4] = {blheli.deviceInfo[blheli.chan][0],
blheli.deviceInfo[blheli.chan][1],
blheli.deviceInfo[blheli.chan][2],
blheli.deviceInfo[blheli.chan][3]}; // device ID
blheli_send_reply(buf, sizeof(buf));
break;
}
case cmd_InterfaceSetMode: {
debug("cmd_InterfaceSetMode(%u)", unsigned(blheli.buf[0]));
blheli.interface_mode[blheli.chan] = blheli.buf[0];
blheli_send_reply(&blheli.interface_mode[blheli.chan], 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[blheli.chan]==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[blheli.chan]);
switch (blheli.interface_mode[blheli.chan]) {
case imSIL_BLB:
case imARM_BLB: {
if (blheli.interface_mode[blheli.chan] == 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[blheli.chan]);
uint8_t buf[nbytes];
memcpy(buf, blheli.buf, nbytes);
switch (blheli.interface_mode[blheli.chan]) {
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[blheli.chan]);
switch (blheli.interface_mode[blheli.chan]) {
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[blheli.chan]);
switch (blheli.interface_mode[blheli.chan]) {
case imATM_BLB: {
if (!BL_ReadA(CMD_READ_EEPROM, buf, nbytes)) {
blheli.ack = ACK_D_GENERAL_ERROR;
}
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[blheli.chan]);
switch (blheli.interface_mode[blheli.chan]) {
case imATM_BLB:
BL_WriteA(CMD_PROG_EEPROM, buf, nbytes, 3000);
break;
default:
blheli.ack = ACK_D_GENERAL_ERROR;
break;
}
uint8_t b = 0;
blheli_send_reply(&b, 1);
break;
}
2018-12-17 04:23:27 -04:00
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;
2018-12-17 04:23:27 -04:00
if (msp.escMode == PROTOCOL_4WAY && blheli.state == BLHELI_IDLE && b == '$') {
debug("Change to MSP mode");
msp.escMode = PROTOCOL_NONE;
hal.rcout->serial_end();
serial_start_ms = 0;
}
if (msp.escMode != PROTOCOL_4WAY && msp.state == MSP_IDLE && b == '/') {
debug("Change to BLHeli mode");
memset(blheli.connected, 0, sizeof(blheli.connected));
msp.escMode = PROTOCOL_4WAY;
}
if (msp.escMode == PROTOCOL_4WAY) {
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();
if (uart->lock_port(BLHELI_UART_LOCK_KEY, 0)) {
uart_locked = true;
}
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;
if (uart->lock_port(BLHELI_UART_LOCK_KEY, 0)) {
uart_locked = 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;
if (hal.util->get_soft_armed()) {
// don't allow MSP control when armed
return false;
}
return process_input(b);
}
2018-04-01 20:26:55 -03:00
/*
run a connection test to the ESCs. This is used to test the
operation of the BLHeli ESC protocol
*/
void AP_BLHeli::run_connection_test(uint8_t chan)
{
run_test.set_and_notify(0);
2018-04-01 20:26:55 -03:00
debug_uart = hal.console;
uint8_t saved_chan = blheli.chan;
if (chan >= num_motors) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: bad channel %u", chan);
return;
}
2018-04-01 20:26:55 -03:00
blheli.chan = chan;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Running test on channel %u", blheli.chan);
2018-04-01 20:26:55 -03:00
bool passed = false;
for (uint8_t tries=0; tries<5; tries++) {
EXPECT_DELAY_MS(3000);
2018-04-01 20:26:55 -03:00
blheli.ack = ACK_OK;
setDisconnected();
if (BL_ConnectEx()) {
uint8_t buf[256];
uint8_t cmd = blheli.interface_mode[blheli.chan]==imATM_BLB?CMD_READ_FLASH_ATM:CMD_READ_FLASH_SIL;
2018-04-01 20:26:55 -03:00
passed = true;
blheli.address = blheli.interface_mode[blheli.chan]==imATM_BLB?0:0x7c00;
2018-04-01 20:26:55 -03:00
passed &= BL_ReadA(cmd, buf, sizeof(buf));
if (blheli.interface_mode[blheli.chan]==imARM_BLB) {
if (passed) {
// read status structure
blheli.address = esc_status_addr;
passed &= BL_SendCMDSetAddress();
}
if (passed) {
struct esc_status status;
passed &= BL_ReadA(CMD_READ_FLASH_SIL, (uint8_t *)&status, sizeof(status));
}
}
2018-04-01 20:26:55 -03:00
BL_SendCMDRunRestartBootloader();
break;
}
}
hal.rcout->serial_end();
SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
2018-04-01 20:26:55 -03:00
motors_disabled = false;
serial_start_ms = 0;
blheli.chan = saved_chan;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Test %s", passed?"PASSED":"FAILED");
2018-04-01 20:26:55 -03:00
debug_uart = nullptr;
}
/*
update BLHeli
*/
void AP_BLHeli::update(void)
{
bool motor_control_active = false;
for (uint8_t i = 0; i < num_motors; i++) {
bool reversed = ((1U<< motor_map[i]) & channel_reversible_mask.get()) != 0;
if (hal.rcout->read( motor_map[i]) != (reversed ? 1500 : 1000)) {
motor_control_active = true;
}
}
uint32_t now = AP_HAL::millis();
if (initialised && uart_locked &&
((timeout_sec && now - last_valid_ms > uint32_t(timeout_sec.get())*1000U) ||
(motor_control_active && now - last_valid_ms > MOTOR_ACTIVE_TIMEOUT))) {
// we're not processing requests any more, shutdown serial
// output
if (serial_start_ms) {
hal.rcout->serial_end();
serial_start_ms = 0;
}
if (motors_disabled) {
motors_disabled = false;
SRV_Channels::set_disabled_channel_mask(motors_disabled_mask);
}
if (uart != nullptr) {
debug("Unlocked UART");
uart->lock_port(0, 0);
uart_locked = false;
}
if (motor_control_active) {
for (uint8_t i = 0; i < num_motors; i++) {
bool reversed = ((1U<<motor_map[i]) & channel_reversible_mask.get()) != 0;
hal.rcout->write(motor_map[i], reversed ? 1500 : 1000);
}
}
}
if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) {
2018-04-01 20:26:55 -03:00
if (initialised && run_test.get() > 0) {
run_connection_test(run_test.get() - 1);
}
}
}
/*
Initialize BLHeli, called by SRV_Channels::init()
Used to install protocol handler
The motor mask of enabled motors can be passed in
*/
void AP_BLHeli::init(uint32_t mask, AP_HAL::RCOutput::output_mode otype)
{
initialised = true;
run_test.set_and_notify(0);
#if HAL_GCS_ENABLED
// only install pass-thru protocol handler if either auto or the motor mask are set
if (channel_mask.get() != 0 || channel_auto.get() != 0) {
if (last_control_port > 0 && last_control_port != control_port) {
gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+last_control_port), nullptr);
last_control_port = -1;
}
if (gcs().install_alternative_protocol((mavlink_channel_t)(MAVLINK_COMM_0+control_port),
FUNCTOR_BIND_MEMBER(&AP_BLHeli::protocol_handler,
bool, uint8_t, AP_HAL::UARTDriver *))) {
debug("BLHeli installed on port %u", (unsigned)control_port);
last_control_port = control_port;
}
}
#endif // HAL_GCS_ENABLED
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_enabled()) {
// with IOMCU the local (FMU) channels start at 8
chan_offset = 8;
}
#endif
mask |= uint32_t(channel_mask.get());
/*
allow mode override - this makes it possible to use DShot for
rovers and subs, plus for quadplane fwd motors
*/
// +1 converts from AP_Motors::pwm_type to AP_HAL::RCOutput::output_mode and saves doing a param conversion
// this is the only use of the param, but this is still a bit of a hack
const int16_t type = output_type.get() + 1;
if (otype == AP_HAL::RCOutput::MODE_PWM_NONE) {
otype = ((type > AP_HAL::RCOutput::MODE_PWM_NONE) && (type < AP_HAL::RCOutput::MODE_NEOPIXEL)) ? AP_HAL::RCOutput::output_mode(type) : AP_HAL::RCOutput::MODE_PWM_NONE;
}
switch (otype) {
case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
case AP_HAL::RCOutput::MODE_PWM_ONESHOT125:
case AP_HAL::RCOutput::MODE_PWM_BRUSHED:
case AP_HAL::RCOutput::MODE_PWM_DSHOT150:
case AP_HAL::RCOutput::MODE_PWM_DSHOT300:
case AP_HAL::RCOutput::MODE_PWM_DSHOT600:
case AP_HAL::RCOutput::MODE_PWM_DSHOT1200:
if (mask) {
hal.rcout->set_output_mode(mask, otype);
}
break;
default:
break;
}
2018-12-17 04:23:27 -04:00
uint32_t digital_mask = 0;
// setting the digital mask changes the min/max PWM values
// it's important that this is NOT done for non-digital channels as otherwise
// PWM min can result in motors turning. set for individual overrides first
if (mask && hal.rcout->is_dshot_protocol(otype)) {
digital_mask = mask;
}
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_Rover)
/*
plane and copter can use AP_Motors to get an automatic mask
*/
2021-05-29 08:52:08 -03:00
#if APM_BUILD_TYPE(APM_BUILD_Rover)
AP_MotorsUGV *motors = AP::motors_ugv();
2021-05-29 08:52:08 -03:00
#else
AP_Motors *motors = AP::motors();
2021-05-29 08:52:08 -03:00
#endif
if (motors) {
uint32_t motormask = motors->get_motor_mask();
// set the rest of the digital channels
if (motors->is_digital_pwm_type()) {
digital_mask |= motormask;
2018-04-02 03:16:37 -03:00
}
mask |= motormask;
}
#endif
// tell SRV_Channels about ESC capabilities
SRV_Channels::set_digital_outputs(digital_mask, uint32_t(channel_reversible_mask.get()) & digital_mask);
// the dshot ESC type is required in order to send the reversed/reversible dshot command correctly
hal.rcout->set_dshot_esc_type(SRV_Channels::get_dshot_esc_type());
hal.rcout->set_reversible_mask(uint32_t(channel_reversible_mask.get()) & digital_mask);
hal.rcout->set_reversed_mask(uint32_t(channel_reversed_mask.get()) & digital_mask);
#ifdef HAL_WITH_BIDIR_DSHOT
// possibly enable bi-directional dshot
hal.rcout->set_motor_poles(motor_poles);
#endif
#if defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT
hal.rcout->set_bidir_dshot_mask(uint32_t(channel_bidir_dshot_mask.get()) & digital_mask);
#endif
// add motors from channel mask
for (uint8_t i=0; i<16 && num_motors < max_motors; i++) {
if (mask & (1U<<i)) {
motor_map[num_motors] = i;
num_motors++;
}
}
motor_mask = mask;
debug("ESC: %u motors mask=0x%08lx", num_motors, mask);
// check if we have a combination of reversible and normal
mixed_type = (mask != (mask & channel_reversible_mask.get())) && (channel_reversible_mask.get() != 0);
if (num_motors != 0 && telem_rate > 0) {
AP_SerialManager *serial_manager = AP_SerialManager::get_singleton();
if (serial_manager) {
telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0);
}
}
}
/*
read an ESC telemetry packet
*/
void AP_BLHeli::read_telemetry_packet(void)
{
#if HAL_WITH_ESC_TELEM
uint8_t buf[telem_packet_size];
if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) {
// short read, we should have 10 bytes ready when this function is called
return;
}
2018-12-17 04:23:27 -04:00
// calculate crc
uint8_t crc = 0;
for (uint8_t i=0; i<telem_packet_size-1; i++) {
crc = crc8_dvb(buf[i], crc, 0x07);
}
if (buf[telem_packet_size-1] != crc) {
// bad crc
debug("Bad CRC on %u", last_telem_esc);
return;
}
// record the previous rpm so that we can slew to the new one
uint16_t new_rpm = ((buf[7]<<8) | buf[8]) * 200 / motor_poles;
const uint8_t motor_idx = motor_map[last_telem_esc];
// we have received valid data, mark the ESC as now active
hal.rcout->set_active_escs_mask(1<<motor_idx);
uint8_t normalized_motor_idx = motor_idx - chan_offset;
#if HAL_WITH_IO_MCU
if (AP_BoardConfig::io_dshot()) {
normalized_motor_idx = motor_idx;
}
#endif
update_rpm(normalized_motor_idx, new_rpm);
TelemetryData t {
.temperature_cdeg = int16_t(buf[0] * 100),
.voltage = float(uint16_t((buf[1]<<8) | buf[2])) * 0.01,
.current = float(uint16_t((buf[3]<<8) | buf[4])) * 0.01,
.consumption_mah = float(uint16_t((buf[5]<<8) | buf[6])),
};
update_telem_data(normalized_motor_idx, t,
AP_ESC_Telem_Backend::TelemetryType::CURRENT
| AP_ESC_Telem_Backend::TelemetryType::VOLTAGE
| AP_ESC_Telem_Backend::TelemetryType::CONSUMPTION
| AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE);
2020-06-22 17:20:15 -03:00
if (debug_level >= 2) {
uint16_t trpm = new_rpm;
if (has_bidir_dshot(last_telem_esc)) {
trpm = hal.rcout->get_erpm(motor_idx);
if (trpm != 0xFFFF) {
trpm = trpm * 200 / motor_poles;
}
}
DEV_PRINTF("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n",
last_telem_esc,
t.temperature_cdeg,
t.voltage,
t.current,
t.consumption_mah,
trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
}
#endif // HAL_WITH_ESC_TELEM
}
/*
log bidir telemetry - only called if BLH telemetry is not active
*/
void AP_BLHeli::log_bidir_telemetry(void)
{
uint32_t now = AP_HAL::millis();
if (debug_level >= 2 && now - last_log_ms[last_telem_esc] > 100) {
if (has_bidir_dshot(last_telem_esc)) {
const uint8_t motor_idx = motor_map[last_telem_esc];
uint16_t trpm = hal.rcout->get_erpm(motor_idx);
if (trpm != 0xFFFF) { // don't log invalid values as they are never used
trpm = trpm * 200 / motor_poles;
}
if (trpm > 0) {
last_log_ms[last_telem_esc] = now;
DEV_PRINTF("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis());
}
}
}
if (!SRV_Channels::have_digital_outputs()) {
return;
}
// ask the next ESC for telemetry
uint8_t idx_pos = last_telem_esc;
uint8_t idx = (idx_pos + 1) % num_motors;
for (; idx != idx_pos; idx = (idx + 1) % num_motors) {
if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
break;
}
}
if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
last_telem_esc = idx;
}
}
/*
update BLHeli telemetry handling
This is called on push() in SRV_Channels
*/
void AP_BLHeli::update_telemetry(void)
{
#ifdef HAL_WITH_BIDIR_DSHOT
// we might only have bi-dir dshot
if (channel_bidir_dshot_mask.get() != 0 && !telem_uart) {
log_bidir_telemetry();
}
#endif
if (!telem_uart || !SRV_Channels::have_digital_outputs()) {
return;
}
uint32_t now = AP_HAL::micros();
uint32_t telem_rate_us = 1000000U / uint32_t(telem_rate.get() * num_motors);
if (telem_rate_us < 2000) {
// make sure we have a gap between frames
telem_rate_us = 2000;
}
if (!telem_uart_started) {
// we need to use begin() here to ensure the correct thread owns the uart
telem_uart->begin(115200);
telem_uart_started = true;
}
2018-12-17 04:23:27 -04:00
uint32_t nbytes = telem_uart->available();
2018-12-17 04:23:27 -04:00
if (nbytes > telem_packet_size) {
// if we have more than 10 bytes then we don't know which ESC
// they are from. Throw them all away
telem_uart->discard_input();
return;
}
if (nbytes > 0 &&
nbytes < telem_packet_size &&
(last_telem_byte_read_us == 0 ||
now - last_telem_byte_read_us < 1000)) {
// wait a bit longer, we don't have enough bytes yet
if (last_telem_byte_read_us == 0) {
last_telem_byte_read_us = now;
}
return;
}
if (nbytes > 0 && nbytes < telem_packet_size) {
// we've waited long enough, discard bytes if we don't have 10 yet
telem_uart->discard_input();
return;
}
if (nbytes == telem_packet_size) {
// we have a full packet ready to parse
read_telemetry_packet();
last_telem_byte_read_us = 0;
}
if (now - last_telem_request_us >= telem_rate_us) {
// ask the next ESC for telemetry
uint8_t idx_pos = last_telem_esc;
uint8_t idx = (idx_pos + 1) % num_motors;
for (; idx != idx_pos; idx = (idx + 1) % num_motors) {
if (SRV_Channels::have_digital_outputs(1U << motor_map[idx])) {
break;
}
}
uint32_t mask = 1U << motor_map[idx];
if (SRV_Channels::have_digital_outputs(mask)) {
hal.rcout->set_telem_request_mask(mask);
last_telem_esc = idx;
last_telem_request_us = now;
}
}
}
#endif // HAVE_AP_BLHELI_SUPPORT