/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ /* * AP_KDECAN.cpp * * Author: Francisco Ferreira */ #include #if HAL_WITH_UAVCAN #include #include #include #include #include #include #include #include #include #include #include "AP_KDECAN.h" extern const AP_HAL::HAL& hal; #define debug_can(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0) #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 AP_GROUPINFO("NPOLE", 1, AP_KDECAN, _num_poles, DEFAULT_NUM_POLES), 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(2, "KDECAN: constructed\n\r"); } AP_KDECAN *AP_KDECAN::get_kdecan(uint8_t driver_index) { if (driver_index >= AP::can().get_num_drivers() || AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_KDECAN) { return nullptr; } return static_cast(AP::can().get_driver(driver_index)); } void AP_KDECAN::init(uint8_t driver_index, bool enable_filters) { _driver_index = driver_index; debug_can(2, "KDECAN: starting init\n\r"); if (_initialized) { debug_can(1, "KDECAN: already initialized\n\r"); return; } // get CAN manager instance AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index]; if (can_mgr == nullptr) { debug_can(1, "KDECAN: no mgr for this driver\n\r"); return; } if (!can_mgr->is_initialized()) { debug_can(1, "KDECAN: mgr not initialized\n\r"); return; } // store pointer to CAN driver _can_driver = can_mgr->get_driver(); if (_can_driver == nullptr) { debug_can(1, "KDECAN: no CAN driver\n\r"); 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 } }; uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), nullptr, 0 }; if(!_can_driver->getIface(CAN_IFACE_INDEX)->send(frame, uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1000), 0)) { debug_can(1, "KDECAN: couldn't send discovery message\n\r"); return; } debug_can(2, "KDECAN: discovery message sent\n\r"); uint32_t start = AP_HAL::millis(); // wait 1 second for answers while (AP_HAL::millis() - start < 1000) { uavcan::CanFrame esc_id_frame {}; uavcan::MonotonicTime time {}; uavcan::UtcTime utc_time {}; uavcan::CanIOFlags flags {}; int16_t n = _can_driver->getIface(CAN_IFACE_INDEX)->receive(esc_id_frame, time, utc_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 & uavcan::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(2, "KDECAN: found ESC id %u\n\r", 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(1, "KDECAN: couldn't create thread\n\r"); return; } _initialized = true; debug_can(2, "KDECAN: init done\n\r"); return; } void AP_KDECAN::loop() { uavcan::MonotonicTime timeout; uavcan::CanFrame empty_frame { (0 | uavcan::CanFrame::FlagEFF), nullptr, 0 }; const uavcan::CanFrame* select_frames[uavcan::MaxCanIfaces] { }; select_frames[CAN_IFACE_INDEX] = &empty_frame; 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; while (true) { if (!_initialized) { debug_can(2, "KDECAN: not initialized\n\r"); hal.scheduler->delay_microseconds(2000); continue; } uavcan::CanSelectMasks inout_mask; uint64_t now = AP_HAL::micros64(); // get latest enumeration state set from GCS if (_enum_sem.take(1)) { enumeration_state = _enumeration_state; _enum_sem.give(); } else { debug_can(2, "KDECAN: failed to get enumeration semaphore on loop\n\r"); } 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 = uavcan::MonotonicTime::fromUSec(now + 1000); switch (enumeration_state) { case ENUMERATION_START: { inout_mask.write = 1 << CAN_IFACE_INDEX; // 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); uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &data, sizeof(data) }; uavcan::CanSelectMasks in_mask = inout_mask; select_frames[CAN_IFACE_INDEX] = &frame; // wait for write space to be available _can_driver->select(inout_mask, select_frames, timeout); select_frames[CAN_IFACE_INDEX] = &empty_frame; if (in_mask.write & inout_mask.write) { now = AP_HAL::micros64(); timeout = uavcan::MonotonicTime::fromUSec(now + ENUMERATION_TIMEOUT_MS * 1000); int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->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(1, "KDECAN: strange buffer full when starting ESC enumeration\n\r"); break; } else { debug_can(1, "KDECAN: error sending message to start ESC enumeration, result %d\n\r", res); break; } } else { break; } FALLTHROUGH; } case ENUMERATION_RUNNING: { inout_mask.read = 1 << CAN_IFACE_INDEX; inout_mask.write = 0; // wait for enumeration messages from ESCs uavcan::CanSelectMasks in_mask = inout_mask; _can_driver->select(inout_mask, select_frames, timeout); if (in_mask.read & inout_mask.read) { uavcan::CanFrame recv_frame; uavcan::MonotonicTime time; uavcan::UtcTime utc_time; uavcan::CanIOFlags flags {}; int16_t res = _can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags); if (res == 1) { if (time.toUSec() < enumeration_start) { // old message break; } frame_id_t id { .value = recv_frame.id & uavcan::CanFrame::MaskExtID }; 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); 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) { inout_mask.read = 0; inout_mask.write = 1 << CAN_IFACE_INDEX; // wait for write space to be available in_mask = inout_mask; _can_driver->select(inout_mask, select_frames, timeout); if (in_mask.write & inout_mask.write) { 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 } }; uavcan::CanFrame send_frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &recv_frame.data, recv_frame.dlc }; timeout = uavcan::MonotonicTime::fromUSec(enumeration_start + ENUMERATION_TIMEOUT_MS * 1000); res = _can_driver->getIface(CAN_IFACE_INDEX)->send(send_frame, timeout, 0); if (res == 1) { enumeration_esc_num++; break; } else if (res == 0) { debug_can(1, "KDECAN: strange buffer full when setting ESC node ID\n\r"); } else { debug_can(1, "KDECAN: error sending message to set ESC node ID, result %d\n\r", res); } } } } else if (res == 0) { debug_can(1, "KDECAN: strange failed read when getting ESC enumeration message\n\r"); } else { debug_can(1, "KDECAN: error receiving ESC enumeration message, result %d\n\r", res); } } break; } case ENUMERATION_STOP: { inout_mask.write = 1 << CAN_IFACE_INDEX; // 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); uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &data, sizeof(data) }; uavcan::CanSelectMasks in_mask = inout_mask; select_frames[CAN_IFACE_INDEX] = &frame; // wait for write space to be available _can_driver->select(inout_mask, select_frames, timeout); select_frames[CAN_IFACE_INDEX] = &empty_frame; if (in_mask.write & inout_mask.write) { timeout = uavcan::MonotonicTime::fromUSec(enumeration_start + ENUMERATION_TIMEOUT_MS * 1000); int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->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(1, "KDECAN: strange buffer full when stop ESC enumeration\n\r"); } else { debug_can(1, "KDECAN: error sending message to stop ESC enumeration, result %d\n\r", res); } } break; } case ENUMERATION_STOPPED: default: debug_can(2, "KDECAN: something wrong happened, shouldn't be here, enumeration state: %u\n\r", enumeration_state); break; } continue; } if (!_esc_present_bitmask) { debug_can(1, "KDECAN: no valid ESC present"); hal.scheduler->delay(1000); continue; } // always look for received frames inout_mask.read = 1 << CAN_IFACE_INDEX; timeout = uavcan::MonotonicTime::fromUSec(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 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)) { inout_mask.write = 1 << CAN_IFACE_INDEX; } 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 = uavcan::MonotonicTime::fromUSec(next_action); } // wait for write space or receive frame uavcan::CanSelectMasks in_mask = inout_mask; _can_driver->select(inout_mask, select_frames, timeout); if (in_mask.read & inout_mask.read) { uavcan::CanFrame frame; uavcan::MonotonicTime time; uavcan::UtcTime utc_time; uavcan::CanIOFlags flags {}; int16_t res = _can_driver->getIface(CAN_IFACE_INDEX)->receive(frame, time, utc_time, flags); if (res == 1) { frame_id_t id { .value = frame.id & uavcan::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; } if (!_telem_sem.take(1)) { debug_can(2, "KDECAN: failed to get telemetry semaphore on write\n\r"); break; } _telemetry[id.source_id - ESC_NODE_ID_FIRST].time = time.toUSec(); _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: // discard frame break; } } } } if (in_mask.write & inout_mask.write) { 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(2, "KDECAN: timed-out after sending frame to ESC with ID %d\n\r", sending_esc_num - 1); sending_esc_num = 0; } } if (sending_esc_num == 0 && new_output) { if (!_rc_out_sem.take(1)) { debug_can(2, "KDECAN: failed to get PWM semaphore on read\n\r"); 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 } }; uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &kde_pwm, sizeof(kde_pwm) }; if (esc_num == 0) { timeout = uavcan::MonotonicTime::fromUSec(now + SET_PWM_TIMEOUT_US); } else { timeout = uavcan::MonotonicTime::fromUSec(pwm_last_sent + SET_PWM_TIMEOUT_US); } int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->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 if (res == 0) { debug_can(1, "KDECAN: strange buffer full when sending message to ESC with ID %d\n\r", esc_num + ESC_NODE_ID_FIRST); } else { debug_can(1, "KDECAN: error sending message to ESC with ID %d, result %d\n\r", 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 } }; uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), nullptr, 0 }; timeout = uavcan::MonotonicTime::fromUSec(now + TELEMETRY_TIMEOUT_US); int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0); if (res == 1) { telemetry_last_request = now; } else if (res == 0) { debug_can(1, "KDECAN: strange buffer full when sending message requesting telemetry\n\r"); } else { debug_can(1, "KDECAN: error sending message requesting telemetry, result %d\n\r", res); } } } } } 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) { continue; } SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(i); if (SRV_Channels::function_assigned(motor_function)) { float norm_output = SRV_Channels::get_output_norm(motor_function); _scaled_output[i] = uint16_t((norm_output + 1.0f) / 2.0f * 2000.0f); } else { _scaled_output[i] = 0; } } _rc_out_sem.give(); _new_output.store(true, std::memory_order_release); } else { debug_can(2, "KDECAN: failed to get PWM semaphore on write\n\r"); } AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr || !logger->should_log(0xFFFFFFFF)) { return; } if (!_telem_sem.take(1)) { debug_can(2, "KDECAN: failed to get telemetry semaphore on DF read\n\r"); 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) { if (!_enum_sem.take(1)) { debug_can(2, "KDECAN: failed to get enumeration semaphore on read\n\r"); snprintf(reason, reason_len ,"Enumeration state unknown"); return false; } if (_enumeration_state != ENUMERATION_STOPPED) { snprintf(reason, reason_len ,"Enumeration running"); _enum_sem.give(); return false; } _enum_sem.give(); uint16_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"); return false; } if (num_present_escs > num_expected_motors) { snprintf(reason, reason_len, "Too many ESCs detected"); return false; } if (_esc_max_node_id != num_expected_motors) { snprintf(reason, reason_len, "Wrong node IDs, run enumeration"); return false; } return true; } void AP_KDECAN::send_mavlink(uint8_t chan) { if (!_telem_sem.take(1)) { debug_can(2, "KDECAN: failed to get telemetry semaphore on MAVLink read\n\r"); 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)) { debug_can(2, "KDECAN: failed to get enumeration semaphore on write\n\r"); return false; } if (start_stop) { _enumeration_state = ENUMERATION_START; } else if (_enumeration_state != ENUMERATION_STOPPED) { _enumeration_state = ENUMERATION_STOP; } _enum_sem.give(); return true; } #endif // HAL_WITH_UAVCAN