diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index a1ffdd6f6b..cf1ee31983 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -32,6 +32,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -1352,6 +1353,7 @@ void AP_BLHeli::update(void) #ifdef HAL_WITH_BIDIR_DSHOT // possibly enable bi-directional dshot hal.rcout->set_bidir_dshot_mask(channel_bidir_dshot_mask.get() & mask); + hal.rcout->set_motor_poles(motor_poles); #endif // add motors from channel mask for (uint8_t i=0; i<16 && num_motors < max_motors; i++) { @@ -1375,70 +1377,6 @@ 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) -{ - if (esc_index >= max_motors) { - return false; - } - if (last_telem[esc_index].timestamp_ms == 0) { - return false; - } - td = last_telem[esc_index]; - return true; -} - -// return the average motor frequency in Hz for dynamic filtering -float AP_BLHeli::get_average_motor_frequency_hz() const -{ - float motor_freq = 0.0f; - const uint32_t now = AP_HAL::millis(); - uint8_t valid_escs = 0; - // average the rpm of each motor as reported by BLHeli and convert to Hz - for (uint8_t i = 0; i < num_motors; i++) { - if (has_bidir_dshot(i)) { - uint16_t erpm = hal.rcout->get_erpm(motor_map[i]); - if (erpm > 0 && erpm < 0xFFFF) { - valid_escs++; - motor_freq += (erpm * 200 / motor_poles) / 60.f; - } - } else if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) { - valid_escs++; - // slew the update - const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f); - motor_freq += (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f; - } - } - if (valid_escs > 0) { - motor_freq /= valid_escs; - } - - return motor_freq; -} - -// return all the motor frequencies in Hz for dynamic filtering -uint8_t AP_BLHeli::get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const -{ - const uint32_t now = AP_HAL::millis(); - uint8_t valid_escs = 0; - - // average the rpm of each motor as reported by BLHeli and convert to Hz - for (uint8_t i = 0; i < num_motors && i < nfreqs; i++) { - if (has_bidir_dshot(i)) { - uint16_t erpm = hal.rcout->get_erpm(motor_map[i]); - if (erpm > 0 && erpm < 0xFFFF) { - freqs[valid_escs++] = (erpm * 200 / motor_poles) / 60.f; - } - } else if (last_telem[i].timestamp_ms && (now - last_telem[i].timestamp_ms < 1000)) { - // slew the update - const float slew = MIN(1.0f, (now - last_telem[i].timestamp_ms) * telem_rate / 1000.0f); - freqs[valid_escs++] = (prev_motor_rpm[i] + (last_telem[i].rpm - prev_motor_rpm[i]) * slew) / 60.0f; - } - } - - return MIN(valid_escs, nfreqs); -} - /* implement the 8 bit CRC used by the BLHeli ESC telemetry protocol */ @@ -1476,68 +1414,41 @@ void AP_BLHeli::read_telemetry_packet(void) debug("Bad CRC on %u", last_telem_esc); return; } - struct telem_data td; - td.temperature = int8_t(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]) * 200 / motor_poles; - td.timestamp_ms = AP_HAL::millis(); // record the previous rpm so that we can slew to the new one - prev_motor_rpm[last_telem_esc] = last_telem[last_telem_esc].rpm; - - last_telem[last_telem_esc] = td; - last_telem[last_telem_esc].count++; - received_telem_data = true; - - AP_Logger *logger = AP_Logger::get_singleton(); - - uint16_t trpm = td.rpm; - float terr = 0.0f; - + 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<logging_enabled() - // log at 10Hz - && td.timestamp_ms - last_log_ms[last_telem_esc] > 100) { + 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])), + }; - if (has_bidir_dshot(last_telem_esc)) { - const uint16_t erpm = hal.rcout->get_erpm(motor_idx); - if (erpm != 0xFFFF) { // don't log invalid values - trpm = erpm * 200 / motor_poles; - } - terr = hal.rcout->get_erpm_error_rate(motor_idx); - } - - logger->Write_ESC(uint8_t(last_telem_esc), - AP_HAL::micros64(), - trpm*100U, - td.voltage, - td.current, - td.temperature * 100U, - td.consumption, - 0, - terr); - last_log_ms[last_telem_esc] = td.timestamp_ms; - } + update_telem_data(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); 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; } - terr = hal.rcout->get_erpm_error_rate(motor_idx); } - hal.console->printf("ESC[%u] T=%u V=%u C=%u con=%u RPM=%u e=%.1f t=%u\n", + hal.console->printf("ESC[%u] T=%u V=%f C=%f con=%f RPM=%u e=%.1f t=%u\n", last_telem_esc, - td.temperature, - td.voltage, - td.current, - td.consumption, - trpm, terr, (unsigned)AP_HAL::millis()); + t.temperature_cdeg, + t.voltage, + t.current, + t.consumption_mah, + trpm, hal.rcout->get_erpm_error_rate(motor_idx), (unsigned)AP_HAL::millis()); } } @@ -1546,32 +1457,19 @@ void AP_BLHeli::read_telemetry_packet(void) */ void AP_BLHeli::log_bidir_telemetry(void) { - AP_Logger *logger = AP_Logger::get_singleton(); - uint32_t now = AP_HAL::millis(); - if (logger && logger->logging_enabled() - // log at 10Hz - && now - last_log_ms[last_telem_esc] > 100) { - + 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); const float terr = hal.rcout->get_erpm_error_rate(motor_idx); if (trpm != 0xFFFF) { // don't log invalid values as they are never used trpm = trpm * 200 / motor_poles; - - logger->Write_ESC(uint8_t(last_telem_esc), - AP_HAL::micros64(), - trpm*100U, - 0, 0, 0, 0, - terr); - last_log_ms[last_telem_esc] = now; } - if (debug_level >= 2) { - hal.console->printf("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, terr, (unsigned)AP_HAL::millis()); - } + last_log_ms[last_telem_esc] = now; + hal.console->printf("ESC[%u] RPM=%u e=%.1f t=%u\n", last_telem_esc, trpm, terr, (unsigned)AP_HAL::millis()); } } @@ -1642,49 +1540,4 @@ void AP_BLHeli::update_telemetry(void) } } -/* - 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 #include @@ -35,7 +40,7 @@ #define AP_BLHELI_MAX_ESCS 8 -class AP_BLHeli { +class AP_BLHeli : public AP_ESC_Telem_Backend { public: AP_BLHeli(); @@ -46,30 +51,6 @@ public: static const struct AP_Param::GroupInfo var_info[]; - struct telem_data { - int8_t temperature; // degrees C, negative values allowed - uint16_t voltage; // volts * 100 - uint16_t current; // amps * 100 - uint16_t consumption;// mAh - uint16_t rpm; // eRPM - uint16_t count; - uint32_t timestamp_ms; - }; - - // number of ESCs configured as BLHeli in channel mask - uint8_t get_num_motors(void) { return num_motors;}; - // get the most recent telemetry data packet for a motor - bool get_telem_data(uint8_t esc_index, struct telem_data &td); - // return the average motor frequency in Hz for dynamic filtering - float get_average_motor_frequency_hz() const; - // return all of the motor frequencies in Hz for dynamic filtering - uint8_t get_motor_frequencies_hz(uint8_t nfreqs, float* freqs) const; - - // return true if we have received any telemetry data - bool have_telem_data(void) const { - return received_telem_data; - } - bool has_bidir_dshot(uint8_t esc_index) const { return channel_bidir_dshot_mask.get() & (1U << motor_map[esc_index]); } @@ -79,9 +60,6 @@ public: static AP_BLHeli *get_singleton(void) { return _singleton; } - - // send ESC telemetry messages over MAVLink - void send_esc_telemetry_mavlink(uint8_t mav_chan); private: static AP_BLHeli *_singleton; @@ -236,20 +214,14 @@ private: AP_HAL::UARTDriver *uart; AP_HAL::UARTDriver *debug_uart; - AP_HAL::UARTDriver *telem_uart; - + AP_HAL::UARTDriver *telem_uart; + static const uint8_t max_motors = AP_BLHELI_MAX_ESCS; uint8_t num_motors; - struct telem_data last_telem[max_motors]; - uint32_t received_telem_data; - // last log output to avoid beat frequencies uint32_t last_log_ms[max_motors]; - // previous motor rpm so that changes can be slewed - float prev_motor_rpm[max_motors]; - // have we initialised the interface? bool initialised;