From f513b1d1c21becf3e4c5c229479db98df8eecc8f Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 27 Feb 2021 16:44:01 +0000 Subject: [PATCH] AP_UAVCAN: add esc telemetry updates remove send_esc_telemetry_mavlink() remove datastructures and semaphore obsoleted by AP_ESC_Telem* () record volts, amps and consumption as floats --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 105 +++--------------------------- libraries/AP_UAVCAN/AP_UAVCAN.h | 23 ++----- 2 files changed, 14 insertions(+), 114 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index bff766f043..00d103ff84 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -137,9 +137,6 @@ static uavcan::Subscriber *esc_stat UC_REGISTRY_BINDER(DebugCb, uavcan::protocol::debug::LogMessage); static uavcan::Subscriber *debug_listener[HAL_MAX_CAN_PROTOCOL_DRIVERS]; -AP_UAVCAN::esc_data AP_UAVCAN::_escs_data[]; -HAL_Semaphore AP_UAVCAN::_telem_sem; - AP_UAVCAN::AP_UAVCAN() : _node_allocator() @@ -349,79 +346,6 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters) } #pragma GCC diagnostic pop -// send ESC telemetry messages over MAVLink -void AP_UAVCAN::send_esc_telemetry_mavlink(uint8_t mav_chan) -{ -#ifndef HAL_NO_GCS - static const uint8_t MAV_ESC_GROUPS = 3; - static const uint8_t MAV_ESC_PER_GROUP = 4; - - for (uint8_t i = 0; i < MAV_ESC_GROUPS; i++) { - - // arrays to hold output - uint8_t temperature[MAV_ESC_PER_GROUP] {}; - uint16_t voltage[MAV_ESC_PER_GROUP] {}; - uint16_t current[MAV_ESC_PER_GROUP] {}; - uint16_t current_tot[MAV_ESC_PER_GROUP] {}; - uint16_t rpm[MAV_ESC_PER_GROUP] {}; - uint16_t count[MAV_ESC_PER_GROUP] {}; - - // if at least one of the ESCs in the group is availabe, the group - // is considered to be available too, and will be sent over MAVlink - bool group_available = false; - - // fill in output arrays of ESCs sensors with available data. - for (uint8_t j = 0; j < MAV_ESC_PER_GROUP; j++) { - const uint8_t esc_idx = i * MAV_ESC_PER_GROUP + j; - - if (!is_esc_data_index_valid(esc_idx)) { - return; - } - - WITH_SEMAPHORE(_telem_sem); - - if (!_escs_data[esc_idx].available) { - continue; - } - - _escs_data[esc_idx].available = false; - - temperature[j] = _escs_data[esc_idx].temp; - voltage[j] = _escs_data[esc_idx].voltage; - current[j] = _escs_data[esc_idx].current; - current_tot[j] = 0; // currently not implemented - rpm[j] = _escs_data[esc_idx].rpm; - count[j] = _escs_data[esc_idx].count; - - group_available = true; - } - - if (!group_available) { - continue; - } - - if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t) mav_chan, ESC_TELEMETRY_1_TO_4)) { - return; - } - - // send messages - switch (i) { - case 0: - mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count); - break; - case 1: - mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count); - break; - case 2: - mavlink_msg_esc_telemetry_9_to_12_send((mavlink_channel_t)mav_chan, temperature, voltage, current, current_tot, rpm, count); - break; - default: - break; - } - } -#endif // HAL_NO_GCS -} - void AP_UAVCAN::loop(void) { while (true) { @@ -866,31 +790,22 @@ void AP_UAVCAN::handle_actuator_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, co void AP_UAVCAN::handle_ESC_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ESCStatusCb &cb) { const uint8_t esc_index = cb.msg->esc_index; - - // log as CESC message - AP::logger().Write_ESCStatus(AP_HAL::native_micros64(), - cb.msg->esc_index, - cb.msg->error_count, - cb.msg->voltage, - cb.msg->current, - cb.msg->temperature - C_TO_KELVIN, - cb.msg->rpm, - cb.msg->power_rating_pct); - - WITH_SEMAPHORE(_telem_sem); if (!is_esc_data_index_valid(esc_index)) { return; } - esc_data &esc = _escs_data[esc_index]; - esc.available = true; - esc.temp = (cb.msg->temperature - C_TO_KELVIN); - esc.voltage = cb.msg->voltage*100; - esc.current = cb.msg->current*100; - esc.rpm = cb.msg->rpm; - esc.count++; + TelemetryData t { + .temperature_cdeg = int16_t((cb.msg->temperature - C_TO_KELVIN) * 100), + .voltage = cb.msg->voltage, + .current = cb.msg->current, + }; + ap_uavcan->update_rpm(esc_index, cb.msg->rpm); + ap_uavcan->update_telem_data(esc_index, t, + AP_ESC_Telem_Backend::TelemetryType::CURRENT + | AP_ESC_Telem_Backend::TelemetryType::VOLTAGE + | AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); } bool AP_UAVCAN::is_esc_data_index_valid(const uint8_t index) { diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 9147248351..82272fa8b2 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -27,6 +27,7 @@ #include #include #include +#include #include @@ -71,7 +72,7 @@ class DebugCb; /* Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received, - the Callback will invoke registery to register the node as separate backend. + the Callback will invoke registry to register the node as separate backend. */ #define UC_REGISTRY_BINDER(ClassName_, DataType_) \ class ClassName_ : public AP_UAVCAN::RegistryBinder { \ @@ -84,7 +85,7 @@ class DebugCb; DISABLE_W_CAST_FUNCTION_TYPE_POP \ } -class AP_UAVCAN : public AP_CANDriver { +class AP_UAVCAN : public AP_CANDriver, public AP_ESC_Telem_Backend { public: AP_UAVCAN(); ~AP_UAVCAN(); @@ -96,9 +97,6 @@ public: void init(uint8_t driver_index, bool enable_filters) override; bool add_interface(AP_HAL::CANIface* can_iface) override; - - // send ESC telemetry messages over MAVLink - void send_esc_telemetry_mavlink(uint8_t mav_chan); uavcan::Node<0>* get_node() { return _node; } uint8_t get_driver_index() const { return _driver_index; } @@ -125,7 +123,7 @@ public: public: RegistryBinder() : - _uc(), + _uc(), _ffunc(), msg() {} @@ -226,19 +224,6 @@ private: static HAL_Semaphore _telem_sem; - struct esc_data { - uint8_t temp; - uint16_t voltage; - uint16_t current; - uint16_t total_current; - uint16_t rpm; - uint16_t count; //count of telemetry packets received (wraps at 65535). - bool available; - }; - - static esc_data _escs_data[UAVCAN_SRV_NUMBER]; - - // safety status send state uint32_t _last_safety_state_ms;