From b63d19533d2a284e06c9fd85b4e04c9076b64127 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 27 Feb 2021 17:46:03 +0000 Subject: [PATCH] AP_KDECAN: support ESC telemetry remove send_esc_telemetry_mavlink() log telemetry data in frontend remove datastructures and semaphore obsoleted by AP_ESC_Telem* () record volts, amps and consumption as floats scale rpm correctly --- libraries/AP_KDECAN/AP_KDECAN.cpp | 107 ++++-------------------------- libraries/AP_KDECAN/AP_KDECAN.h | 18 +---- 2 files changed, 15 insertions(+), 110 deletions(-) diff --git a/libraries/AP_KDECAN/AP_KDECAN.cpp b/libraries/AP_KDECAN/AP_KDECAN.cpp index 047800d7fa..f00a752777 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.cpp +++ b/libraries/AP_KDECAN/AP_KDECAN.cpp @@ -445,18 +445,20 @@ void AP_KDECAN::loop() break; } - if (!_telem_sem.take(1)) { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get telemetry semaphore on write"); - break; - } + const uint8_t idx = id.source_id - ESC_NODE_ID_FIRST; + const uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES; + update_rpm(idx, uint16_t(uint16_t(frame.data[4] << 8 | frame.data[5]) * 60UL * 2 / num_poles)); + + TelemetryData t { + .temperature_cdeg = int16_t(frame.data[6] * 100), + .voltage = float(uint16_t(frame.data[0] << 8 | frame.data[1])) * 0.01f, + .current = float(uint16_t(frame.data[2] << 8 | frame.data[3])) * 0.01f, + }; + update_telem_data(idx, t, + AP_ESC_Telem_Backend::TelemetryType::CURRENT + | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE + | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); - _telemetry[id.source_id - ESC_NODE_ID_FIRST].time = rx_time; - _telemetry[id.source_id - ESC_NODE_ID_FIRST].voltage = frame.data[0] << 8 | frame.data[1]; - _telemetry[id.source_id - ESC_NODE_ID_FIRST].current = frame.data[2] << 8 | frame.data[3]; - _telemetry[id.source_id - ESC_NODE_ID_FIRST].rpm = frame.data[4] << 8 | frame.data[5]; - _telemetry[id.source_id - ESC_NODE_ID_FIRST].temp = frame.data[6]; - _telemetry[id.source_id - ESC_NODE_ID_FIRST].new_data = true; - _telem_sem.give(); break; } default: @@ -581,41 +583,6 @@ void AP_KDECAN::update() } else { debug_can(AP_CANManager::LOG_DEBUG, "failed to get PWM semaphore on write"); } - - AP_Logger *logger = AP_Logger::get_singleton(); - - if (logger == nullptr || !logger->should_log(0xFFFFFFFF)) { - return; - } - - if (!_telem_sem.take(1)) { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get telemetry semaphore on DF read"); - return; - } - - telemetry_info_t telem_buffer[KDECAN_MAX_NUM_ESCS] {}; - - for (uint8_t i = 0; i < _esc_max_node_id; i++) { - if (_telemetry[i].new_data) { - telem_buffer[i] = _telemetry[i]; - _telemetry[i].new_data = false; - } - } - - _telem_sem.give(); - - uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES; - - // log ESC telemetry data - for (uint8_t i = 0; i < _esc_max_node_id; i++) { - if (telem_buffer[i].new_data) { - logger->Write_ESC(i, telem_buffer[i].time, - int32_t(telem_buffer[i].rpm * 60UL * 2 / num_poles * 100), - telem_buffer[i].voltage, - telem_buffer[i].current, - int16_t(telem_buffer[i].temp * 100U), 0, 0); - } - } } bool AP_KDECAN::pre_arm_check(char* reason, uint8_t reason_len) @@ -661,54 +628,6 @@ bool AP_KDECAN::pre_arm_check(char* reason, uint8_t reason_len) return true; } -void AP_KDECAN::send_mavlink(uint8_t chan) -{ - if (!_telem_sem.take(1)) { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get telemetry semaphore on MAVLink read"); - return; - } - - telemetry_info_t telem_buffer[KDECAN_MAX_NUM_ESCS]; - memcpy(telem_buffer, _telemetry, sizeof(telemetry_info_t) * KDECAN_MAX_NUM_ESCS); - _telem_sem.give(); - - uint16_t voltage[4] {}; - uint16_t current[4] {}; - uint16_t rpm[4] {}; - uint8_t temperature[4] {}; - uint16_t totalcurrent[4] {}; - uint16_t count[4] {}; - uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES; - uint64_t now = AP_HAL::micros64(); - - for (uint8_t i = 0; i < _esc_max_node_id && i < 8; i++) { - uint8_t idx = i % 4; - if (telem_buffer[i].time && (now - telem_buffer[i].time < 1000000)) { - voltage[idx] = telem_buffer[i].voltage; - current[idx] = telem_buffer[i].current; - rpm[idx] = uint16_t(telem_buffer[i].rpm * 60UL * 2 / num_poles); - temperature[idx] = telem_buffer[i].temp; - } else { - voltage[idx] = 0; - current[idx] = 0; - rpm[idx] = 0; - temperature[idx] = 0; - } - - if (idx == 3 || i == _esc_max_node_id - 1) { - if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)chan, ESC_TELEMETRY_1_TO_4)) { - return; - } - - if (i < 4) { - mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count); - } else { - mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count); - } - } - } -} - bool AP_KDECAN::run_enumeration(bool start_stop) { if (!_enum_sem.take(1)) { diff --git a/libraries/AP_KDECAN/AP_KDECAN.h b/libraries/AP_KDECAN/AP_KDECAN.h index ce852137d2..3664067f69 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.h +++ b/libraries/AP_KDECAN/AP_KDECAN.h @@ -27,13 +27,14 @@ #include #include +#include #include // there are 12 motor functions in SRV_Channel but CAN driver can't keep up #define KDECAN_MAX_NUM_ESCS 8 -class AP_KDECAN : public AP_CANDriver { +class AP_KDECAN : public AP_CANDriver, public AP_ESC_Telem_Backend { public: AP_KDECAN(); @@ -55,9 +56,6 @@ public: // check that arming can happen bool pre_arm_check(char* reason, uint8_t reason_len); - // send MAVLink telemetry packets - void send_mavlink(uint8_t chan); - // caller checks that vehicle isn't armed // start_stop: true to start, false to stop bool run_enumeration(bool start_stop); @@ -91,18 +89,6 @@ private: std::atomic _new_output; uint16_t _scaled_output[KDECAN_MAX_NUM_ESCS]; - // telemetry input - HAL_Semaphore _telem_sem; - struct telemetry_info_t { - uint64_t time; - uint16_t voltage; - uint16_t current; - uint16_t rpm; - uint8_t temp; - bool new_data; - } _telemetry[KDECAN_MAX_NUM_ESCS]; - - union frame_id_t { struct PACKED { uint8_t object_address;