From 34bbd29f254c5d7a527a6828f047c7bbe8a115c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 31 May 2018 07:33:26 +1000 Subject: [PATCH] AP_BLHeli: send ESC telem packets over MAVLink --- libraries/AP_BLHeli/AP_BLHeli.cpp | 133 +++++++++++++++++++++++------- libraries/AP_BLHeli/AP_BLHeli.h | 29 ++++++- 2 files changed, 132 insertions(+), 30 deletions(-) diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index 7eb196ac2c..490163d35f 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -23,7 +23,7 @@ #include "AP_BLHeli.h" -#if HAL_SUPPORT_RCOUT_SERIAL +#ifdef HAVE_AP_BLHELI_SUPPORT #include #include @@ -97,11 +97,14 @@ const AP_Param::GroupInfo AP_BLHeli::var_info[] = { AP_GROUPEND }; +AP_BLHeli *AP_BLHeli::instance; + // constructor AP_BLHeli::AP_BLHeli(void) { // set defaults from the parameter table AP_Param::setup_object_defaults(this, var_info); + instance = this; } /* @@ -1209,6 +1212,20 @@ void AP_BLHeli::update(void) } +// get the most recent telemetry data packet for a motor +bool AP_BLHeli::get_telem_data(uint8_t esc_index, struct telem_data &td, uint32_t ×tamp_ms) +{ + if (esc_index >= max_motors) { + return false; + } + if (last_telem_ms[esc_index] == 0) { + return false; + } + timestamp_ms = last_telem_ms[esc_index]; + td = last_telem[esc_index]; + return true; +} + /* implement the 8 bit CRC used by the BLHeli ESC telemetry protocol */ @@ -1250,34 +1267,39 @@ void AP_BLHeli::read_telemetry_packet(void) debug("Bad CRC on %u\n", last_telem_esc); return; } - uint8_t temperature = buf[0]; - uint16_t voltage = (buf[1]<<8) | buf[2]; - uint16_t current = (buf[3]<<8) | buf[4]; - uint16_t consumption = (buf[5]<<8) | buf[6]; - uint16_t rpm = (buf[7]<<8) | buf[8]; + struct telem_data td; + td.temperature = buf[0]; + td.voltage = (buf[1]<<8) | buf[2]; + td.current = (buf[3]<<8) | buf[4]; + td.consumption = (buf[5]<<8) | buf[6]; + td.rpm = (buf[7]<<8) | buf[8]; + last_telem[last_telem_esc] = td; + last_telem[last_telem_esc].count++; + last_telem_ms[last_telem_esc] = AP_HAL::millis(); + DataFlash_Class *df = DataFlash_Class::instance(); if (df && df->logging_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 + rpm : int32_t(td.rpm*100U), + voltage : td.voltage, + current : td.current, + temperature : int16_t(td.temperature * 100U), + current_tot : td.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 + if (debug_level >= 2) { + hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u t=%u\n", + last_telem_esc, + td.temperature, + td.voltage, + td.current, + td.consumption, + td.rpm, (unsigned)AP_HAL::millis()); + } } /* @@ -1290,22 +1312,22 @@ void AP_BLHeli::update_telemetry(void) 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 (!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 - 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(); } @@ -1313,8 +1335,12 @@ void AP_BLHeli::update_telemetry(void) } if (nbytes > 0 && nbytes < telem_packet_size && - now - last_telem_request_us < telem_rate_us) { + (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) { @@ -1327,8 +1353,10 @@ void AP_BLHeli::update_telemetry(void) 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 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); @@ -1336,4 +1364,51 @@ void AP_BLHeli::update_telemetry(void) } } -#endif // HAL_SUPPORT_RCOUT_SERIAL + +/* + send ESC telemetry messages over MAVLink + */ +void AP_BLHeli::send_esc_telemetry_mavlink(uint8_t mav_chan) +{ + if (num_motors == 0) { + return; + } + uint8_t temperature[4] {}; + uint16_t voltage[4] {}; + uint16_t current[4] {}; + uint16_t totalcurrent[4] {}; + uint16_t rpm[4] {}; + uint16_t count[4] {}; + uint32_t now = AP_HAL::millis(); + for (uint8_t i=0; i #include "msp_protocol.h" #include "blheli_4way_protocol.h" @@ -40,9 +42,29 @@ public: bool process_input(uint8_t b); static const struct AP_Param::GroupInfo var_info[]; + + struct telem_data { + uint8_t temperature; // degrees C + uint16_t voltage; // volts * 100 + uint16_t current; // amps * 100 + uint16_t consumption;// mAh + uint16_t rpm; // eRPM + uint16_t count; + }; + + // get the most recent telemetry data packet for a motor + bool get_telem_data(uint8_t esc_index, struct telem_data &td, uint32_t ×tamp_ms); + + static AP_BLHeli *get_instance(void) { + return instance; + } + + // send ESC telemetry messages over MAVLink + void send_esc_telemetry_mavlink(uint8_t mav_chan); private: - + static AP_BLHeli *instance; + // mask of channels to use for BLHeli protocol AP_Int32 channel_mask; AP_Int8 channel_auto; @@ -171,6 +193,9 @@ private: static const uint8_t max_motors = 8; uint8_t num_motors; + struct telem_data last_telem[max_motors]; + uint32_t last_telem_ms[max_motors]; + // have we initialised the interface? bool initialised; @@ -193,6 +218,8 @@ private: uint32_t last_telem_request_us; uint8_t last_telem_esc; static const uint8_t telem_packet_size = 10; + bool telem_uart_started; + uint32_t last_telem_byte_read_us; bool msp_process_byte(uint8_t c); void blheli_crc_update(uint8_t c);