diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index f86fd034a0..90c5b30600 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -151,12 +151,23 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @RebootRequired: True AP_GROUPINFO("RVMASK", 12, AP_BLHeli, channel_reversed_mask, 0), +#if HAL_WITH_ESC_TELEM + // @Param: EXT_TLM + // @DisplayName: Enable extended BLHeli telemetry + // @Description: When set to 1, this listens for 13-byte extended telemetry packets from BLHeli ESCs + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO("EXTLM", 13, AP_BLHeli, extended_telemetry, 0), +#endif + AP_GROUPEND }; #define RPM_SLEW_RATE 50 AP_BLHeli *AP_BLHeli::_singleton; +uint8_t AP_BLHeli::telem_packet_size; // constructor AP_BLHeli::AP_BLHeli(void) @@ -1415,6 +1426,8 @@ void AP_BLHeli::init(void) telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0); } } + + telem_packet_size = extended_telemetry ? 13 : 10; } /* @@ -1423,7 +1436,7 @@ void AP_BLHeli::init(void) void AP_BLHeli::read_telemetry_packet(void) { #if HAL_WITH_ESC_TELEM - uint8_t buf[telem_packet_size]; + uint8_t buf[telem_packet_array_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; @@ -1447,18 +1460,28 @@ void AP_BLHeli::read_telemetry_packet(void) hal.rcout->set_active_escs_mask(1<= 2) { uint16_t trpm = new_rpm; @@ -1470,10 +1493,10 @@ void AP_BLHeli::read_telemetry_packet(void) } 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, + telemetryData.temperature_cdeg, + telemetryData.voltage, + telemetryData.current, + telemetryData.consumption_mah, trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis()); } #endif // HAL_WITH_ESC_TELEM diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index 2614cd39ae..b782351c14 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -76,6 +76,7 @@ class AP_BLHeli : public AP_ESC_Telem_Backend { AP_Int8 motor_poles; // mask of channels with bi-directional dshot enabled AP_Int32 channel_bidir_dshot_mask; + AP_Int8 extended_telemetry; enum mspState { MSP_IDLE=0, @@ -250,7 +251,8 @@ class AP_BLHeli : public AP_ESC_Telem_Backend { // when did we last request telemetry? uint32_t last_telem_request_us; uint8_t last_telem_esc; - static const uint8_t telem_packet_size = 10; + static const uint8_t telem_packet_array_size = 13; + static uint8_t telem_packet_size; bool telem_uart_started; uint32_t last_telem_byte_read_us; int8_t last_control_port;