From b7bcee69a37cbe719cf9aa6fe9c58c972de35389 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 2 Apr 2018 14:22:26 +1000 Subject: [PATCH] AP_BLHeli: added telemetry logging support allow telemetry feedback from BLHeli_32 ESCs --- libraries/AP_BLHeli/AP_BLHeli.cpp | 134 +++++++++++++++++++++++++++++- libraries/AP_BLHeli/AP_BLHeli.h | 10 ++- 2 files changed, 142 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index f671dc264c..aff05c3e52 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include extern const AP_HAL::HAL& hal; @@ -67,8 +69,16 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { // @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: Advanced + // @User: Standard AP_GROUPINFO("TMOUT", 4, AP_BLHeli, timeout_sec, 60), + + // @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, 0), AP_GROUPEND }; @@ -1116,6 +1126,80 @@ void AP_BLHeli::update(void) } } debug("ESC: mapped %u motors with mask 0x%04x", num_motors, mask); + + if (telem_rate > 0) { + AP_SerialManager *serial_manager = AP_SerialManager::get_instance(); + if (serial_manager) { + telem_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_ESCTelemetry,0); + } + } +} + +static uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed) +{ + uint8_t crc_u = crc; + crc_u ^= crc_seed; + + for (int i=0; i<8; i++) { + crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 ); + } + + return (crc_u); +} + +/* + read an ESC telemetry packet + */ +void AP_BLHeli::read_telemetry_packet(void) +{ + uint8_t buf[telem_packet_size]; + uint8_t crc = 0; + for (uint8_t i=0; iread(); + if (v < 0) { + // short read, we should have 10 bytes ready when this function is called + return; + } + buf[i] = uint8_t(v); + } + + // calculate crc + for (uint8_t i=0; ilogging_enabled()) { + struct log_Esc pkt = { + LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC1_MSG+last_telem_esc)), + time_us : AP_HAL::micros64(), + rpm : int32_t(rpm*100U), + voltage : uint16_t(voltage), + current : uint16_t(current), + temperature : int16_t(temperature * 100U), + current_tot : consumption + }; + df->WriteBlock(&pkt, sizeof(pkt)); + } +#if 0 + hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u\n", + last_telem_esc, + temperature, + voltage, + current, + consumption, + rpm); +#endif } /* @@ -1124,6 +1208,54 @@ void AP_BLHeli::update(void) */ void AP_BLHeli::update_telemetry(void) { + if (!telem_uart) { + return; + } + uint32_t now = AP_HAL::micros(); + uint32_t nbytes = telem_uart->available(); + 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 (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 + if (nbytes > telem_packet_size*2) { + // something badly wrong with this uart, disable ESC + // telemetry so we don't chew lots of CPU reading garbage + telem_uart = nullptr; + return; + } + while (nbytes--) { + telem_uart->read(); + } + return; + } + if (nbytes > 0 && + nbytes < telem_packet_size && + now - last_telem_request_us < telem_rate_us) { + // wait a bit longer, we don't have enough bytes yet + return; + } + if (nbytes > 0 && nbytes < telem_packet_size) { + // we've waited long enough, discard bytes if we don't have 10 yet + while (nbytes--) { + telem_uart->read(); + } + return; + } + if (nbytes == telem_packet_size) { + // we have a full packet ready to parse + read_telemetry_packet(); + } + if (now - last_telem_request_us >= telem_rate_us) { + last_telem_esc = (last_telem_esc + 1) % num_motors; + uint16_t mask = 1U << motor_map[last_telem_esc]; + hal.rcout->set_telem_request_mask(mask); + last_telem_request_us = now; + } } #endif // HAL_SUPPORT_RCOUT_SERIAL diff --git a/libraries/AP_BLHeli/AP_BLHeli.h b/libraries/AP_BLHeli/AP_BLHeli.h index d8fd043701..652ef1a3ca 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.h +++ b/libraries/AP_BLHeli/AP_BLHeli.h @@ -48,6 +48,7 @@ private: AP_Int8 channel_auto; AP_Int8 run_test; AP_Int16 timeout_sec; + AP_Int16 telem_rate; enum mspState { MSP_IDLE=0, @@ -163,6 +164,7 @@ private: AP_HAL::UARTDriver *uart; AP_HAL::UARTDriver *debug_uart; + AP_HAL::UARTDriver *telem_uart; static const uint8_t max_motors = 8; uint8_t num_motors; @@ -182,6 +184,11 @@ private: // mapping from BLHeli motor numbers to RC output channels uint8_t motor_map[max_motors]; + // 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; + bool msp_process_byte(uint8_t c); void blheli_crc_update(uint8_t c); bool blheli_4way_process_byte(uint8_t c); @@ -210,7 +217,8 @@ private: bool BL_VerifyFlash(const uint8_t *buf, uint16_t n); void blheli_process_command(void); void run_connection_test(uint8_t chan); - + void read_telemetry_packet(void); + // protocol handler hook bool protocol_handler(uint8_t , AP_HAL::UARTDriver *); };