/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* implementation of MSP and BLHeli-4way protocols for pass-through ESC calibration and firmware update With thanks to betaflight for a great reference implementation. Several of the functions below are based on betaflight equivalent functions */ #include "AP_BLHeli.h" #if HAVE_AP_BLHELI_SUPPORT #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS #include #endif #include #include #if APM_BUILD_TYPE(APM_BUILD_Rover) #include #else #include #endif #include #include #include #include #include #include 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 // @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), // @Param: POLES // @DisplayName: BLHeli Motor Poles // @Description: This allows calculation of true RPM from ESC's eRPM. The default is 14. // @Range: 1 127 // @User: Advanced // @RebootRequired: True 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; iwrite_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: { 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; } case MSP_FC_VARIANT: debug("MSP_FC_VARIANT"); msp_send_reply(msp.cmdMSP, (const uint8_t *)ARDUPILOT_IDENTIFIER, FLIGHT_CONTROLLER_IDENTIFIER_LENGTH); break; /* 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: { debug("MSP_FC_VERSION"); uint8_t version[3] = { 3, 3, 1 }; msp_send_reply(msp.cmdMSP, version, sizeof(version)); break; } case MSP_BOARD_INFO: { 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: { 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 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: { 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: { 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: { 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: { 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: { 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: { debug("MSP_MOTOR"); // get the output going to each motor 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]); 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: { 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; } 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]; } 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; 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; } 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_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); } /* 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); 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; } blheli.chan = chan; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Running test on channel %u", blheli.chan); bool passed = false; for (uint8_t tries=0; tries<5; tries++) { EXPECT_DELAY_MS(3000); 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; passed = true; blheli.address = blheli.interface_mode[blheli.chan]==imATM_BLB?0:0x7c00; 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)); } } BL_SendCMDRunRestartBootloader(); break; } } hal.rcout->serial_end(); SRV_Channels::set_disabled_channel_mask(motors_disabled_mask); motors_disabled = false; serial_start_ms = 0; blheli.chan = saved_chan; GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ESC: Test %s", passed?"PASSED":"FAILED"); 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<write(motor_map[i], reversed ? 1500 : 1000); } } } if (initialised || (channel_mask.get() == 0 && channel_auto.get() == 0)) { 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; } 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 */ #if APM_BUILD_TYPE(APM_BUILD_Rover) AP_MotorsUGV *motors = AP::motors_ugv(); #else AP_Motors *motors = AP::motors(); #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; } 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< 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; } // calculate crc uint8_t crc = 0; for (uint8_t i=0; iset_active_escs_mask(1<= 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; } uint32_t nbytes = telem_uart->available(); 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