From c585b0286d00d9b08b05960ad42df0873bb6c012 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Mon, 10 Apr 2023 16:36:52 -0700 Subject: [PATCH] AP_KDECAN: total re-write using CANSensor and remove Enum --- libraries/AP_KDECAN/AP_KDECAN.cpp | 841 ++++++++----------------- libraries/AP_KDECAN/AP_KDECAN.h | 135 ++-- libraries/AP_KDECAN/AP_KDECAN_config.h | 9 + 3 files changed, 344 insertions(+), 641 deletions(-) create mode 100644 libraries/AP_KDECAN/AP_KDECAN_config.h diff --git a/libraries/AP_KDECAN/AP_KDECAN.cpp b/libraries/AP_KDECAN/AP_KDECAN.cpp index 344b69f822..8b8e061f12 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.cpp +++ b/libraries/AP_KDECAN/AP_KDECAN.cpp @@ -15,36 +15,24 @@ /* * AP_KDECAN.cpp * - * Author: Francisco Ferreira + * Author: Francisco Ferreira and Tom Pittenger */ -#include -#include -#include +#include "AP_KDECAN.h" +#if AP_KDECAN_ENABLED +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include "AP_KDECAN.h" -#include +#include // for MIN,MAX -#if HAL_MAX_CAN_PROTOCOL_DRIVERS extern const AP_HAL::HAL& hal; -#if HAL_CANMANAGER_ENABLED -#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "KDECAN", fmt, ##args); } while (0) -#else -#define debug_can(level_debug, fmt, args...) -#endif - -#define DEFAULT_NUM_POLES 14 - // table of user settable CAN bus parameters const AP_Param::GroupInfo AP_KDECAN::var_info[] = { + // @Param: NPOLE // @DisplayName: Number of motor poles // @Description: Sets the number of motor poles to calculate the correct RPM value @@ -53,603 +41,282 @@ const AP_Param::GroupInfo AP_KDECAN::var_info[] = { AP_GROUPEND }; -const uint16_t AP_KDECAN::SET_PWM_MIN_INTERVAL_US; - AP_KDECAN::AP_KDECAN() { AP_Param::setup_object_defaults(this, var_info); - - debug_can(AP_CANManager::LOG_INFO, "constructed"); +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + if (_singleton != nullptr) { + AP_HAL::panic("AP_KDECAN must be singleton"); + } +#endif + _singleton = this; } -AP_KDECAN *AP_KDECAN::get_kdecan(uint8_t driver_index) +void AP_KDECAN::init() { - if (driver_index >= AP::can().get_num_drivers() || - AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_KDECAN) { - return nullptr; - } - return static_cast(AP::can().get_driver(driver_index)); -} - -bool AP_KDECAN::add_interface(AP_HAL::CANIface* can_iface) { - - if (_can_iface != nullptr) { - debug_can(AP_CANManager::LOG_ERROR, "Multiple Interface not supported"); - return false; - } - - _can_iface = can_iface; - - if (_can_iface == nullptr) { - debug_can(AP_CANManager::LOG_ERROR, "CAN driver not found"); - return false; - } - - if (!_can_iface->is_initialized()) { - debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized"); - return false; - } - - if (!_can_iface->set_event_handle(&_event_handle)) { - debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle"); - return false; - } - return true; -} - -void AP_KDECAN::init(uint8_t driver_index, bool enable_filters) -{ - _driver_index = driver_index; - - debug_can(AP_CANManager::LOG_INFO, "starting init"); - - if (_initialized) { - debug_can(AP_CANManager::LOG_ERROR, "already initialized"); + if (_driver != nullptr) { + // only allow one instance return; } - if (_can_iface == nullptr) { - debug_can(AP_CANManager::LOG_ERROR, "Interface not found"); - return; - } - - // find available KDE ESCs - frame_id_t id = { { .object_address = ESC_INFO_OBJ_ADDR, - .destination_id = BROADCAST_NODE_ID, - .source_id = AUTOPILOT_NODE_ID, - .priority = 0, - .unused = 0 } }; - - AP_HAL::CANFrame frame { (id.value | AP_HAL::CANFrame::FlagEFF), nullptr, 0 }; - - if(!_can_iface->send(frame, AP_HAL::micros() + 1000000, 0)) { - debug_can(AP_CANManager::LOG_DEBUG, "couldn't send discovery message"); - return; - } - - debug_can(AP_CANManager::LOG_DEBUG, "discovery message sent"); - - uint32_t start = AP_HAL::millis(); - - // wait 1 second for answers - while (AP_HAL::millis() - start < 1000) { - AP_HAL::CANFrame esc_id_frame {}; - uint64_t rx_time; - AP_HAL::CANIface::CanIOFlags flags = 0; - - int16_t n = _can_iface->receive(esc_id_frame, rx_time, flags); - - if (n != 1) { - continue; - } - - if (!esc_id_frame.isExtended()) { - continue; - } - - if (esc_id_frame.dlc != 5) { - continue; - } - - id.value = esc_id_frame.id & AP_HAL::CANFrame::MaskExtID; - - if (id.source_id == BROADCAST_NODE_ID || - id.source_id >= (KDECAN_MAX_NUM_ESCS + ESC_NODE_ID_FIRST) || - id.destination_id != AUTOPILOT_NODE_ID || - id.object_address != ESC_INFO_OBJ_ADDR) { - continue; - } - - _esc_present_bitmask |= (1 << (id.source_id - ESC_NODE_ID_FIRST)); - _esc_max_node_id = id.source_id - ESC_NODE_ID_FIRST + 1; - - debug_can(AP_CANManager::LOG_DEBUG, "found ESC id %u", id.source_id); - } - - snprintf(_thread_name, sizeof(_thread_name), "kdecan_%u", driver_index); - - // start thread for receiving and sending CAN frames - if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_KDECAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) { - debug_can(AP_CANManager::LOG_ERROR, "couldn't create thread"); - return; - } - - _initialized = true; - - debug_can(AP_CANManager::LOG_DEBUG, "init done"); - - return; -} - -void AP_KDECAN::loop() -{ - uint64_t timeout; - - uint16_t output_buffer[KDECAN_MAX_NUM_ESCS] {}; - - enumeration_state_t enumeration_state = _enumeration_state; - uint64_t enumeration_start = 0; - uint8_t enumeration_esc_num = 0; - - const uint32_t LOOP_INTERVAL_US = MIN(AP::scheduler().get_loop_period_us(), SET_PWM_MIN_INTERVAL_US); - uint64_t pwm_last_sent = 0; - uint8_t sending_esc_num = 0; - - uint64_t telemetry_last_request = 0; - AP_HAL::CANFrame empty_frame { (0 | AP_HAL::CANFrame::FlagEFF), nullptr, 0 }; - - while (true) { - if (!_initialized) { - debug_can(AP_CANManager::LOG_ERROR, "not initialized"); - hal.scheduler->delay_microseconds(2000); - continue; - } - - uint64_t now = AP_HAL::micros64(); - bool read_select; - bool write_select; - bool select_ret; - // get latest enumeration state set from GCS - if (_enum_sem.take(1)) { - enumeration_state = _enumeration_state; - _enum_sem.give(); - } else { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get enumeration semaphore on loop"); - } - - if (enumeration_state != ENUMERATION_STOPPED) { - // check if enumeration timed out - if (enumeration_start != 0 && now - enumeration_start >= ENUMERATION_TIMEOUT_MS * 1000) { - enumeration_start = 0; - - WITH_SEMAPHORE(_enum_sem); - - // check if enumeration state didn't change or was set to stop - if (enumeration_state == _enumeration_state || _enumeration_state == ENUMERATION_STOP) { - enumeration_state = _enumeration_state = ENUMERATION_STOPPED; - } - - continue; - } - - timeout = now + 1000; - - switch (enumeration_state) { - case ENUMERATION_START: { - - // send broadcast frame to start enumeration - frame_id_t id = { { .object_address = ENUM_OBJ_ADDR, - .destination_id = BROADCAST_NODE_ID, - .source_id = AUTOPILOT_NODE_ID, - .priority = 0, - .unused = 0 } }; - be16_t data = htobe16((uint16_t) ENUMERATION_TIMEOUT_MS); - AP_HAL::CANFrame frame { (id.value | AP_HAL::CANFrame::FlagEFF), (uint8_t*) &data, sizeof(data) }; - - - // wait for write space to be available - read_select = false; - write_select = true; - select_ret = _can_iface->select(read_select, write_select, &frame, timeout); - - if (select_ret && write_select) { - now = AP_HAL::micros64(); - timeout = now + ENUMERATION_TIMEOUT_MS * 1000; - - int8_t res = _can_iface->send(frame, timeout, 0); - - if (res == 1) { - enumeration_start = now; - enumeration_esc_num = 0; - _esc_present_bitmask = 0; - _esc_max_node_id = 0; - - WITH_SEMAPHORE(_enum_sem); - - if (enumeration_state == _enumeration_state) { - enumeration_state = _enumeration_state = ENUMERATION_RUNNING; - } - } else if (res == 0) { - debug_can(AP_CANManager::LOG_ERROR, "strange buffer full when starting ESC enumeration"); - break; - } else { - debug_can(AP_CANManager::LOG_ERROR, "error sending message to start ESC enumeration, result %d", res); - break; - } - } else { - break; - } - FALLTHROUGH; - } - case ENUMERATION_RUNNING: { - // wait for enumeration messages from ESCs - // wait for read data to be available - read_select = true; - write_select = false; - select_ret = _can_iface->select(read_select, write_select, nullptr, timeout); - - if (select_ret && read_select) { - AP_HAL::CANFrame recv_frame; - uint64_t rx_time; - AP_HAL::CANIface::CanIOFlags flags {}; - - int16_t res = _can_iface->receive(recv_frame, rx_time, flags); - - if (res == 1) { - frame_id_t id { .value = recv_frame.id & AP_HAL::CANFrame::MaskExtID }; - if (rx_time < enumeration_start) { - // old message - debug_can(AP_CANManager::LOG_DEBUG, "Received old message from ESC id %u", id.source_id); - break; - } - if (id.object_address == UPDATE_NODE_ID_OBJ_ADDR) { - // reply from setting new node ID - _esc_present_bitmask |= 1 << (id.source_id - ESC_NODE_ID_FIRST); - _esc_max_node_id = MAX(_esc_max_node_id, id.source_id - ESC_NODE_ID_FIRST + 1); - debug_can(AP_CANManager::LOG_DEBUG, "found ESC id %u", id.source_id); - break; - } else if (id.object_address != ENUM_OBJ_ADDR) { - // discardable frame, only looking for enumeration - break; - } - - // try to set node ID for the received ESC - while (AP_HAL::micros64() - enumeration_start < ENUMERATION_TIMEOUT_MS * 1000) { - // wait for write space to be available - id = { { .object_address = UPDATE_NODE_ID_OBJ_ADDR, - .destination_id = uint8_t(enumeration_esc_num + ESC_NODE_ID_FIRST), - .source_id = AUTOPILOT_NODE_ID, - .priority = 0, - .unused = 0 } }; - AP_HAL::CANFrame send_frame { (id.value | AP_HAL::CANFrame::FlagEFF), (uint8_t*) &recv_frame.data, recv_frame.dlc }; - read_select = false; - write_select = true; - select_ret = _can_iface->select(read_select, write_select, &send_frame, timeout); - - if (select_ret && write_select) { - timeout = enumeration_start + ENUMERATION_TIMEOUT_MS * 1000; - - res = _can_iface->send(send_frame, timeout, 0); - - if (res == 1) { - enumeration_esc_num++; - break; - } else if (res == 0) { - debug_can(AP_CANManager::LOG_ERROR, "strange buffer full when setting ESC node ID"); - } else { - debug_can(AP_CANManager::LOG_ERROR, "error sending message to set ESC node ID, result %d", res); - } - } - } - } else if (res == 0) { - debug_can(AP_CANManager::LOG_ERROR, "strange failed read when getting ESC enumeration message"); - } else { - debug_can(AP_CANManager::LOG_ERROR, "error receiving ESC enumeration message, result %d", res); - } - } - break; - } - case ENUMERATION_STOP: { - - // send broadcast frame to stop enumeration - frame_id_t id = { { .object_address = ENUM_OBJ_ADDR, - .destination_id = BROADCAST_NODE_ID, - .source_id = AUTOPILOT_NODE_ID, - .priority = 0, - .unused = 0 } }; - le16_t data = htole16((uint16_t) ENUMERATION_TIMEOUT_MS); - AP_HAL::CANFrame frame { (id.value | AP_HAL::CANFrame::FlagEFF), (uint8_t*) &data, sizeof(data) }; - - - // wait for write space to be available - read_select = false; - write_select = true; - select_ret = _can_iface->select(read_select, read_select, &frame, timeout); - - if (select_ret && write_select) { - timeout = enumeration_start + ENUMERATION_TIMEOUT_MS * 1000; - - int8_t res = _can_iface->send(frame, timeout, 0); - - if (res == 1) { - enumeration_start = 0; - - WITH_SEMAPHORE(_enum_sem); - - if (enumeration_state == _enumeration_state) { - enumeration_state = _enumeration_state = ENUMERATION_STOPPED; - } - } else if (res == 0) { - debug_can(AP_CANManager::LOG_ERROR, "strange buffer full when stop ESC enumeration"); - } else { - debug_can(AP_CANManager::LOG_ERROR, "error sending message to stop ESC enumeration, result %d", res); - } - } - break; - } - case ENUMERATION_STOPPED: - default: - debug_can(AP_CANManager::LOG_DEBUG, "something wrong happened, shouldn't be here, enumeration state: %u", enumeration_state); - break; - } - - continue; - } - - if (!_esc_present_bitmask) { - hal.scheduler->delay(1000); - continue; - } - - // always look for received frames - timeout = now + LOOP_INTERVAL_US; - - // check if: - // - is currently sending throttle frames, OR - // - there are new output values and, a throttle frame was never sent or it's no longer in CAN queue, OR - // - it is time to send throttle frames again, regardless of new output values, OR - // - it is time to ask for telemetry information - bool try_write = false; - if (sending_esc_num > 0 || - (_new_output.load(std::memory_order_acquire) && (pwm_last_sent == 0 || now - pwm_last_sent > SET_PWM_TIMEOUT_US)) || - (pwm_last_sent != 0 && (now - pwm_last_sent > SET_PWM_MIN_INTERVAL_US)) || - (now - telemetry_last_request > TELEMETRY_INTERVAL_US)) { - // wait for write space or receive frame - try_write = true; - } else { // don't need to send frame, choose the maximum time we'll wait for receiving a frame - uint64_t next_action = MIN(now + LOOP_INTERVAL_US, telemetry_last_request + TELEMETRY_INTERVAL_US); - - if (pwm_last_sent != 0) { - next_action = MIN(next_action, pwm_last_sent + SET_PWM_MIN_INTERVAL_US); - } - timeout = next_action; - } - - read_select = true; - write_select = try_write; - // Immediately check if rx buffer not empty - select_ret = _can_iface->select(read_select, write_select, &empty_frame, timeout); - if (select_ret && read_select) { - AP_HAL::CANFrame frame; - uint64_t rx_time; - AP_HAL::CANIface::CanIOFlags flags {}; - - int16_t res = _can_iface->receive(frame, rx_time, flags); - - if (res == 1) { -#if HAL_WITH_ESC_TELEM - frame_id_t id { .value = frame.id & AP_HAL::CANFrame::MaskExtID }; - - // check if frame is valid: directed at autopilot, doesn't come from broadcast and ESC was detected before - if (id.destination_id == AUTOPILOT_NODE_ID && - id.source_id != BROADCAST_NODE_ID && - (1 << (id.source_id - ESC_NODE_ID_FIRST) & _esc_present_bitmask)) { - switch (id.object_address) { - case TELEMETRY_OBJ_ADDR: { - if (frame.dlc != 8) { - 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); - - break; - } - default: - // discard frame - break; - } - } -#endif // HAL_WITH_ESC_TELEM - } - } - - if (select_ret && try_write && write_select) { - now = AP_HAL::micros64(); - - bool new_output = _new_output.load(std::memory_order_acquire); - - if (sending_esc_num > 0) { - // currently sending throttle frames, check it didn't timeout - if (now - pwm_last_sent > SET_PWM_TIMEOUT_US) { - debug_can(AP_CANManager::LOG_DEBUG, "timed-out after sending frame to ESC with ID %d", sending_esc_num - 1); - sending_esc_num = 0; - } - } - - if (sending_esc_num == 0 && new_output) { - if (!_rc_out_sem.take(1)) { - debug_can(AP_CANManager::LOG_ERROR, "failed to get PWM semaphore on read"); - continue; - } - - memcpy(output_buffer, _scaled_output, KDECAN_MAX_NUM_ESCS * sizeof(uint16_t)); - - _rc_out_sem.give(); - } - - // check if: - // - is currently sending throttle frames, OR - // - there are new output values and, a throttle frame was never sent or it's no longer in CAN queue, OR - // - it is time to send throttle frames again, regardless of new output values - if (sending_esc_num > 0 || - (new_output && (pwm_last_sent == 0 || now - pwm_last_sent > SET_PWM_TIMEOUT_US)) || - (pwm_last_sent != 0 && (now - pwm_last_sent > SET_PWM_MIN_INTERVAL_US))) { - - for (uint8_t esc_num = sending_esc_num; esc_num < _esc_max_node_id; esc_num++) { - - if ((_esc_present_bitmask & (1 << esc_num)) == 0) { - continue; - } - - be16_t kde_pwm = htobe16(output_buffer[esc_num]); - - if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { - kde_pwm = 0; - } - - frame_id_t id = { { .object_address = SET_PWM_OBJ_ADDR, - .destination_id = uint8_t(esc_num + ESC_NODE_ID_FIRST), - .source_id = AUTOPILOT_NODE_ID, - .priority = 0, - .unused = 0 } }; - - AP_HAL::CANFrame frame { (id.value | AP_HAL::CANFrame::FlagEFF), (uint8_t*) &kde_pwm, sizeof(kde_pwm) }; - - if (esc_num == 0) { - timeout = now + SET_PWM_TIMEOUT_US; - } else { - timeout = pwm_last_sent + SET_PWM_TIMEOUT_US; - } - - int16_t res = _can_iface->send(frame, timeout, 0); - if (res == 1) { - if (esc_num == 0) { - pwm_last_sent = now; - - if (new_output) { - _new_output.store(false, std::memory_order_release); - } - } - - sending_esc_num = (esc_num + 1) % _esc_max_node_id; - } else { - debug_can(AP_CANManager::LOG_ERROR, "error sending message to ESC with ID %d, result %d", esc_num + ESC_NODE_ID_FIRST, res); - } - - break; - } - } else if (now - telemetry_last_request > TELEMETRY_INTERVAL_US) { - // broadcast telemetry request frame - frame_id_t id = { { .object_address = TELEMETRY_OBJ_ADDR, - .destination_id = BROADCAST_NODE_ID, - .source_id = AUTOPILOT_NODE_ID, - .priority = 0, - .unused = 0 } }; - - AP_HAL::CANFrame frame { (id.value | AP_HAL::CANFrame::FlagEFF), nullptr, 0 }; - timeout = now + TELEMETRY_TIMEOUT_US; - int16_t res = _can_iface->send(frame, timeout, 0); - if (res == 1) { - telemetry_last_request = now; - } else if (res == 0) { - debug_can(AP_CANManager::LOG_ERROR, "strange buffer full when sending message requesting telemetry"); - } else { - debug_can(AP_CANManager::LOG_ERROR, "error sending message requesting telemetry, result %d", res); - } - } + AP_CANManager::Driver_Type protocol = AP_CANManager::Driver_Type_None; + + for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) { +#if HAL_CANMANAGER_ENABLED + protocol = AP::can().get_driver_type(i); +#elif defined(HAL_BUILD_AP_PERIPH) + protocol = CANSensor::get_driver_type(i); +#endif + if (protocol == AP_CANManager::Driver_Type_KDECAN) { + _driver = new AP_KDECAN_Driver(); + return; } } } void AP_KDECAN::update() { - if (_rc_out_sem.take(1)) { - for (uint8_t i = 0; i < KDECAN_MAX_NUM_ESCS; i++) { - if ((_esc_present_bitmask & (1 << i)) == 0) { - _scaled_output[i] = 0; - continue; + if (_driver == nullptr) { + return; + } + _driver->update((uint8_t)_num_poles.get()); +} + +AP_KDECAN_Driver::AP_KDECAN_Driver() : CANSensor("KDECAN") +{ + register_driver(AP_CANManager::Driver_Type_KDECAN); + + // start thread for receiving and sending CAN frames + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_KDECAN_Driver::loop, void), "kdecan", 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0); +} + +// parse inbound frames +void AP_KDECAN_Driver::handle_frame(AP_HAL::CANFrame &frame) +{ + if (!frame.isExtended()) { + return; + } + + const frame_id_t id { .value = frame.id & AP_HAL::CANFrame::MaskExtID }; + + // if (id.object_address != TELEMETRY_OBJ_ADDR) { + // GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"KDECAN: rx id:%d, src:%d, dest:%d, len:%d", (int)id.object_address, (int)id.source_id, (int)id.destination_id, (int)frame.dlc); + // } + + // check if frame is valid: directed at autopilot, doesn't come from broadcast and ESC was detected before + switch (id.object_address) { + case ESC_INFO_OBJ_ADDR: + if (frame.dlc == 5 && + id.destination_id == AUTOPILOT_NODE_ID && + id.source_id >= ESC_NODE_ID_FIRST && + id.source_id < (KDECAN_MAX_NUM_ESCS + ESC_NODE_ID_FIRST)) + { + const uint16_t bitmask = (1U << (id.source_id - ESC_NODE_ID_FIRST)); + + if ((bitmask & _init.detected_bitmask) != bitmask) { + _init.detected_bitmask |= bitmask; + GCS_SEND_TEXT(MAV_SEVERITY_INFO,"KDECAN: Found ESC id %u", id.source_id); + } + } + break; + +#if HAL_WITH_ESC_TELEM + case TELEMETRY_OBJ_ADDR: + if (id.destination_id == AUTOPILOT_NODE_ID && + id.source_id != BROADCAST_NODE_ID && + (1U << (id.source_id - ESC_NODE_ID_FIRST) & _init.detected_bitmask) && + frame.dlc == 8) + { + const uint8_t idx = id.source_id - ESC_NODE_ID_FIRST; + const uint8_t num_poles = _telemetry.num_poles > 0 ? _telemetry.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); + } + break; +#endif // HAL_WITH_ESC_TELEM + + case UPDATE_NODE_ID_OBJ_ADDR: + // reply from setting new node ID + _init.detected_bitmask |= 1U << (id.source_id - ESC_NODE_ID_FIRST); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "KDECAN: Found ESC id %u", id.source_id); + break; + } +} + +void AP_KDECAN_Driver::update(const uint8_t num_poles) +{ + if (!hal.util->get_soft_armed() || _init.detected_bitmask == 0) { + return; + } + +#if HAL_WITH_ESC_TELEM + _telemetry.num_poles = num_poles; +#endif + + uint16_t pwm[KDECAN_MAX_NUM_ESCS] {}; + + uint8_t index = 0; + for (uint16_t i = 0; i < ARRAY_SIZE(pwm); i++) { + if ((_init.detected_bitmask & (1UL< SRV_Channel::Aux_servo_function_t::k_none) { + pwm[index++] = SRV_Channels::srv_channel(i)->get_output_pwm(); + } + } + + { + // queue the PWMs for loop() + WITH_SEMAPHORE(_output.sem); + memcpy(&_output.pwm, &pwm, sizeof(_output.pwm)); + _output.is_new = true; + } + +#if AP_KDECAN_USE_EVENTS + if (_output.thread_ctx != nullptr) { + // trigger the thread to wake up immediately + chEvtSignal(_output.thread_ctx, 1); + } +#endif +} + +void AP_KDECAN_Driver::loop() +{ + uint16_t pwm[KDECAN_MAX_NUM_ESCS] {}; + +#if AP_KDECAN_USE_EVENTS + _output.thread_ctx = chThdGetSelfX(); +#endif + + uint8_t broadcast_esc_info_boot_spam_count = 3; + uint32_t broadcast_esc_info_next_interval_ms = 100; // spam a few at boot at 5Hz + + while (true) { +#if AP_KDECAN_USE_EVENTS + // sleep until we get new data, but also wake up at 400Hz to send the old data again + chEvtWaitAnyTimeout(ALL_EVENTS, chTimeUS2I(2500)); + #else + hal.scheduler->delay_microseconds(2500); // 400Hz +#endif + + const uint32_t now_ms = AP_HAL::millis(); + + // This should run at 400Hz + { + WITH_SEMAPHORE(_output.sem); + if (_output.is_new) { + _output.last_new_ms = now_ms; + _output.is_new = false; + memcpy(&pwm, &_output.pwm, sizeof(pwm)); + + } else if (_output.last_new_ms && now_ms - _output.last_new_ms > 1000) { + // if we haven't gotten any PWM updates for a bit, zero it + // out so we don't just keep sending the same values forever + memset(&pwm, 0, sizeof(pwm)); + _output.last_new_ms = 0; } - _scaled_output[i] = SRV_Channels::srv_channel(i)->get_output_pwm(); } - _rc_out_sem.give(); - _new_output.store(true, std::memory_order_release); - } else { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get PWM semaphore on write"); - } + uint8_t index = 0; + uint8_t retry = 0; + + while (index < KDECAN_MAX_NUM_ESCS) { + if ((_init.detected_bitmask & (1 << index)) == 0) { + // we're not sending this index so skip it + index++; + } else if (send_packet_uint16(SET_PWM_OBJ_ADDR, (index + ESC_NODE_ID_FIRST), 1, pwm[index]) || retry++ >= 10) { + // sent successfully or we've retried too many times, move on to the next + index++; + retry = 0; + } else { + // send failed, likely due to CAN TX buffer full. Delay a tiny bit and try again but only a few times + hal.scheduler->delay_microseconds(10); + } + } // while index + +#if HAL_WITH_ESC_TELEM + // broadcast as request-telemetry msg to everyone + if (_init.detected_bitmask != 0 && now_ms - _telemetry.timer_ms >= TELEMETRY_INTERVAL_MS) { + if (send_packet(TELEMETRY_OBJ_ADDR, BROADCAST_NODE_ID, 10)) { + _telemetry.timer_ms = now_ms; + } + } +#endif // HAL_WITH_ESC_TELEM + + if ((_init.detected_bitmask == 0 || broadcast_esc_info_boot_spam_count > 0) && (now_ms - _init.detected_bitmask_ms >= broadcast_esc_info_next_interval_ms)) { + // broadcast an "anyone there?" quick at boot but then 1Hz forever until we see at least 1 esc respond + if (broadcast_esc_info_boot_spam_count > 0) { + broadcast_esc_info_boot_spam_count--; + } else { + broadcast_esc_info_next_interval_ms = 1000; + } + + if (send_packet(ESC_INFO_OBJ_ADDR, BROADCAST_NODE_ID, 100)) { + _init.detected_bitmask_ms = now_ms; + } + } + + } // while true } -bool AP_KDECAN::pre_arm_check(char* reason, uint8_t reason_len) +bool AP_KDECAN_Driver::send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint16_t data) { - if (!_enum_sem.take(1)) { - snprintf(reason, reason_len ,"enumeration state unknown"); - return false; + const uint16_t data_be16 = htobe16(data); + return send_packet(address, dest_id, timeout_ms, (uint8_t*)&data_be16, 2); +} + +bool AP_KDECAN_Driver::send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint8_t *data, const uint8_t data_len) +{ + // broadcast telemetry request frame + frame_id_t id = { { .object_address = address, + .destination_id = dest_id, + .source_id = AUTOPILOT_NODE_ID, + .priority = 0, + .unused = 0 } }; + + AP_HAL::CANFrame frame = AP_HAL::CANFrame((id.value | AP_HAL::CANFrame::FlagEFF), data, data_len, false); + + const uint64_t timeout_us = uint64_t(timeout_ms) * 1000UL; + return write_frame(frame, timeout_us); +} + +bool AP_KDECAN_Driver::pre_arm_check(char* reason, const uint8_t reason_len) +{ + uint16_t configured_servo_motors_count = 0; + for (uint16_t i = 0; i < NUM_SERVO_CHANNELS; i++) { + if (SRV_Channel::is_motor(SRV_Channels::channel_function(i))) { + configured_servo_motors_count++; + } } - if (_enumeration_state != ENUMERATION_STOPPED) { - snprintf(reason, reason_len, "enumeration running"); - _enum_sem.give(); - return false; - } + const uint8_t num_present_escs = __builtin_popcount(_init.detected_bitmask); - _enum_sem.give(); - - uint32_t motors_mask = 0; - AP_Motors *motors = AP_Motors::get_singleton(); - - if (motors) { - motors_mask = motors->get_motor_mask(); - } - - uint8_t num_expected_motors = __builtin_popcount(motors_mask); - uint8_t num_present_escs = __builtin_popcount(_esc_present_bitmask); - - if (num_present_escs < num_expected_motors) { - snprintf(reason, reason_len, "too few ESCs detected (%u of %u)", (int)num_present_escs, (int)num_expected_motors); - return false; - } - - if (num_present_escs > num_expected_motors) { - snprintf(reason, reason_len, "too many ESCs detected (%u > %u)", (int)num_present_escs, (int)num_expected_motors); - return false; - } - - if (_esc_max_node_id != num_expected_motors) { - snprintf(reason, reason_len, "wrong node IDs (%u!=%u), run enumeration", (int)_esc_max_node_id, (int)num_expected_motors); + if (configured_servo_motors_count != num_present_escs) { + snprintf(reason, reason_len, "ESC count error: Srv:%u, Detected:%u", (unsigned)configured_servo_motors_count, (unsigned)num_present_escs); return false; } return true; } -bool AP_KDECAN::run_enumeration(bool start_stop) +// singleton instance +AP_KDECAN *AP_KDECAN::_singleton; + +namespace AP { +AP_KDECAN *kdecan() { - if (!_enum_sem.take(1)) { - debug_can(AP_CANManager::LOG_DEBUG, "failed to get enumeration semaphore on write"); - return false; - } - - if (start_stop) { - _enumeration_state = ENUMERATION_START; - } else if (_enumeration_state != ENUMERATION_STOPPED) { - _enumeration_state = ENUMERATION_STOP; - } - - _enum_sem.give(); - - return true; + return AP_KDECAN::get_singleton(); } +}; + +#endif // AP_KDECAN_ENABLED -#endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_KDECAN/AP_KDECAN.h b/libraries/AP_KDECAN/AP_KDECAN.h index a3a5af4e62..33d49f3a4f 100644 --- a/libraries/AP_KDECAN/AP_KDECAN.h +++ b/libraries/AP_KDECAN/AP_KDECAN.h @@ -15,78 +15,77 @@ /* * AP_KDECAN.h * - * Author: Francisco Ferreira + * Author: Francisco Ferreira and Tom Pittenger */ #pragma once -#include +#include -#if HAL_MAX_CAN_PROTOCOL_DRIVERS - -#include +#if AP_KDECAN_ENABLED +#include +#include #include #include -#include +#define AP_KDECAN_USE_EVENTS (defined(CH_CFG_USE_EVENTS) && CH_CFG_USE_EVENTS == TRUE) + +#if AP_KDECAN_USE_EVENTS +#include +#endif + +#define DEFAULT_NUM_POLES 14 -// 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, public AP_ESC_Telem_Backend { +class AP_KDECAN_Driver : public CANSensor +#if HAL_WITH_ESC_TELEM +, public AP_ESC_Telem_Backend +#endif +{ public: - AP_KDECAN(); - /* Do not allow copies */ - CLASS_NO_COPY(AP_KDECAN); - - static const struct AP_Param::GroupInfo var_info[]; - - // Return KDECAN from @driver_index or nullptr if it's not ready or doesn't exist - static AP_KDECAN *get_kdecan(uint8_t driver_index); - - void init(uint8_t driver_index, bool enable_filters) override; - bool add_interface(AP_HAL::CANIface* can_iface) override; + AP_KDECAN_Driver(); // called from SRV_Channels - void update(); - - // check that arming can happen - bool pre_arm_check(char* reason, uint8_t reason_len); + void update(const uint8_t num_poles); - // caller checks that vehicle isn't armed - // start_stop: true to start, false to stop - bool run_enumeration(bool start_stop); + // check that arming can happen + bool pre_arm_check(char* reason, const uint8_t reason_len); private: + + // handler for incoming frames + void handle_frame(AP_HAL::CANFrame &frame) override; + + bool send_packet_uint16(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint16_t data); + bool send_packet(const uint8_t address, const uint8_t dest_id, const uint32_t timeout_ms, const uint8_t *data = nullptr, const uint8_t data_len = 0); + void loop(); - bool _initialized; - char _thread_name[11]; - uint8_t _driver_index; - AP_HAL::CANIface* _can_iface; - HAL_EventHandle _event_handle; + struct { + uint16_t detected_bitmask; + uint32_t detected_bitmask_ms; + } _init; - AP_Int8 _num_poles; + struct { + HAL_Semaphore sem; + bool is_new; + uint32_t last_new_ms; + uint16_t pwm[KDECAN_MAX_NUM_ESCS]; +#if AP_KDECAN_USE_EVENTS + thread_t *thread_ctx; +#endif + uint16_t max_node_id; + } _output; - // ESC detected information - uint32_t _esc_present_bitmask; - uint8_t _esc_max_node_id; - - // enumeration - HAL_Semaphore _enum_sem; - enum enumeration_state_t : uint8_t { - ENUMERATION_STOPPED, - ENUMERATION_START, - ENUMERATION_STOP, - ENUMERATION_RUNNING - } _enumeration_state = ENUMERATION_STOPPED; - - // PWM output - HAL_Semaphore _rc_out_sem; - std::atomic _new_output; - uint16_t _scaled_output[KDECAN_MAX_NUM_ESCS]; +#if HAL_WITH_ESC_TELEM + struct { + uint8_t num_poles; + uint32_t timer_ms; + } _telemetry; +#endif union frame_id_t { struct PACKED { @@ -116,11 +115,39 @@ private: static const uint8_t ENUM_OBJ_ADDR = 10; static const uint8_t TELEMETRY_OBJ_ADDR = 11; - static const uint16_t SET_PWM_MIN_INTERVAL_US = 2500; - static const uint32_t TELEMETRY_INTERVAL_US = 100000; - static const uint32_t SET_PWM_TIMEOUT_US = 2000; - static const uint16_t TELEMETRY_TIMEOUT_US = 500; + static const uint32_t PWM_MIN_INTERVAL_MS = 3; + static const uint32_t PWM_IS_NEW_TIMEOUT_MS = 1000; + static const uint32_t TELEMETRY_INTERVAL_MS = 100; static const uint16_t ENUMERATION_TIMEOUT_MS = 30000; + }; -#endif //HAL_NUM_CAN_IFACES + +class AP_KDECAN { +public: + AP_KDECAN(); + + /* Do not allow copies */ + CLASS_NO_COPY(AP_KDECAN); + + static const struct AP_Param::GroupInfo var_info[]; + + void init(); + void update(); + + static AP_KDECAN *get_singleton() { return _singleton; } + + // check that arming can happen + bool pre_arm_check(char* reason, const uint8_t reason_len) { return (_driver == nullptr) ? true : _driver->pre_arm_check(reason, reason_len); } + +private: + static AP_KDECAN *_singleton; + + AP_Int8 _num_poles; + AP_KDECAN_Driver *_driver; +}; +namespace AP { + AP_KDECAN *kdecan(); +}; + +#endif // AP_KDECAN_ENABLED diff --git a/libraries/AP_KDECAN/AP_KDECAN_config.h b/libraries/AP_KDECAN/AP_KDECAN_config.h new file mode 100644 index 0000000000..e2f647c136 --- /dev/null +++ b/libraries/AP_KDECAN/AP_KDECAN_config.h @@ -0,0 +1,9 @@ + +#pragma once + +#include + +#ifndef AP_KDECAN_ENABLED + #define AP_KDECAN_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && BOARD_FLASH_SIZE > 1024) +#endif +