diff --git a/libraries/AP_KDECAN/AP_KDECAN.cpp b/libraries/AP_KDECAN/AP_KDECAN.cpp
new file mode 100644
index 0000000000..0f538af0c1
--- /dev/null
+++ b/libraries/AP_KDECAN/AP_KDECAN.cpp
@@ -0,0 +1,754 @@
+/*
+ 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 "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
+};
+
+
+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)
+{
+ _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");
+ }
+
+ DataFlash_Class *df = DataFlash_Class::instance();
+
+ if (df == nullptr || !df->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) {
+ struct log_Esc pkt = {
+ LOG_PACKET_HEADER_INIT(uint8_t(LOG_ESC1_MSG+i)),
+ time_us : telem_buffer[i].time,
+ rpm : int32_t(telem_buffer[i].rpm * 60UL * 2 / num_poles * 100),
+ voltage : telem_buffer[i].voltage,
+ current : telem_buffer[i].current,
+ temperature : int16_t(telem_buffer[i].temp * 100U),
+ current_tot : 0
+ };
+ df->WriteBlock(&pkt, sizeof(pkt));
+ }
+ }
+}
+
+bool AP_KDECAN::pre_arm_check(const char* &reason)
+{
+ if (!_enum_sem.take(1)) {
+ debug_can(2, "KDECAN: failed to get enumeration semaphore on read\n\r");
+ reason = "KDECAN enumeration state unknown";
+ return false;
+ }
+
+ if (_enumeration_state != ENUMERATION_STOPPED) {
+ reason = "KDECAN enumeration running";
+ _enum_sem.give();
+ return false;
+ }
+
+ _enum_sem.give();
+
+ uint16_t motors_mask = 0;
+ AP_Motors *motors = AP_Motors::get_instance();
+
+ 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) {
+ reason = "Not enough KDECAN ESCs detected";
+ return false;
+ }
+
+ if (num_present_escs > num_expected_motors) {
+ reason = "Too many KDECAN ESCs detected";
+ return false;
+ }
+
+ if (_esc_max_node_id != num_expected_motors) {
+ reason = "Wrong KDECAN 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
diff --git a/libraries/AP_KDECAN/AP_KDECAN.h b/libraries/AP_KDECAN/AP_KDECAN.h
new file mode 100644
index 0000000000..bc598df599
--- /dev/null
+++ b/libraries/AP_KDECAN/AP_KDECAN.h
@@ -0,0 +1,137 @@
+/*
+ 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.h
+ *
+ * Author: Francisco Ferreira
+ */
+
+#pragma once
+
+#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_HAL::CANProtocol {
+public:
+ AP_KDECAN();
+
+ /* Do not allow copies */
+ AP_KDECAN(const AP_KDECAN &other) = delete;
+ AP_KDECAN &operator=(const AP_KDECAN&) = delete;
+
+ 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) override;
+
+ // called from SRV_Channels
+ void update();
+
+ // check that arming can happen
+ bool pre_arm_check(const char* &reason);
+
+ // 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);
+
+private:
+ void loop();
+
+ bool _initialized;
+ char _thread_name[9];
+ uint8_t _driver_index;
+ uavcan::ICanDriver* _can_driver;
+
+ AP_Int8 _num_poles;
+
+ // ESC detected information
+ uint16_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];
+
+ // 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 {
+ uint8_t object_address;
+ uint8_t destination_id;
+ uint8_t source_id;
+ uint8_t priority:5;
+ uint8_t unused:3;
+ };
+ uint32_t value;
+ };
+
+ static const uint8_t AUTOPILOT_NODE_ID = 0;
+ static const uint8_t BROADCAST_NODE_ID = 1;
+ static const uint8_t ESC_NODE_ID_FIRST = 2;
+
+ static const uint8_t ESC_INFO_OBJ_ADDR = 0;
+ static const uint8_t SET_PWM_OBJ_ADDR = 1;
+ static const uint8_t VOLTAGE_OBJ_ADDR = 2;
+ static const uint8_t CURRENT_OBJ_ADDR = 3;
+ static const uint8_t RPM_OBJ_ADDR = 4;
+ static const uint8_t TEMPERATURE_OBJ_ADDR = 5;
+ static const uint8_t GET_PWM_INPUT_OBJ_ADDR = 6;
+ static const uint8_t GET_PWM_OUTPUT_OBJ_ADDR = 7;
+ static const uint8_t MCU_ID_OBJ_ADDR = 8;
+ static const uint8_t UPDATE_NODE_ID_OBJ_ADDR = 9;
+ 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 uint16_t ENUMERATION_TIMEOUT_MS = 30000;
+
+ static const uint8_t CAN_IFACE_INDEX = 0;
+};
diff --git a/libraries/AP_KDECAN/examples/KDECAN_sniffer/KDECAN_sniffer.cpp b/libraries/AP_KDECAN/examples/KDECAN_sniffer/KDECAN_sniffer.cpp
new file mode 100644
index 0000000000..636f9280a1
--- /dev/null
+++ b/libraries/AP_KDECAN/examples/KDECAN_sniffer/KDECAN_sniffer.cpp
@@ -0,0 +1,358 @@
+/*
+ simple KDECAN network sniffer as an ArduPilot firmware
+ */
+#include
+#include
+
+#if (CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS || CONFIG_HAL_BOARD == HAL_BOARD_LINUX) && HAL_WITH_UAVCAN
+
+#include
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+#include
+#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#include
+#endif
+
+#include
+#include
+
+#include
+
+void setup();
+void loop();
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+#define debug_can(fmt, args...) do { hal.console->printf(fmt, ##args); } while (0)
+
+#define NUM_ESCS 8
+
+class KDECAN_sniffer {
+public:
+ KDECAN_sniffer() {
+ for (uint8_t i = 0; i < NUM_ESCS; i++) {
+ _esc_info[i].mcu_id = 0xA5961824E7BD3C00 | i;
+ }
+ }
+
+ void init(void);
+ void loop(void);
+ void print_stats(void);
+ void send_enumeration(uint8_t num);
+
+private:
+ uint8_t _driver_index = 0;
+ uint8_t _interface = 0;
+ uavcan::ICanDriver* _can_driver;
+ uint8_t _mask_received_pwm = 0;
+
+ struct esc_info {
+ uint8_t node_id;
+ uint64_t mcu_id;
+ uint64_t enum_timeout;
+
+ esc_info() : node_id(1), mcu_id(0), enum_timeout(0) {}
+ } _esc_info[NUM_ESCS];
+
+ uint8_t _max_node_id = 0;
+
+ static const uint8_t BROADCAST_NODE_ID = 1;
+
+ static const uint8_t ESC_INFO_OBJ_ADDR = 0;
+ static const uint8_t SET_PWM_OBJ_ADDR = 1;
+ static const uint8_t VOLTAGE_OBJ_ADDR = 2;
+ static const uint8_t CURRENT_OBJ_ADDR = 3;
+ static const uint8_t RPM_OBJ_ADDR = 4;
+ static const uint8_t TEMPERATURE_OBJ_ADDR = 5;
+ static const uint8_t GET_PWM_INPUT_OBJ_ADDR = 6;
+ static const uint8_t GET_PWM_OUTPUT_OBJ_ADDR = 7;
+ static const uint8_t MCU_ID_OBJ_ADDR = 8;
+ static const uint8_t UPDATE_NODE_ID_OBJ_ADDR = 9;
+ static const uint8_t START_ENUM_OBJ_ADDR = 10;
+ static const uint8_t TELEMETRY_OBJ_ADDR = 11;
+};
+
+static struct {
+ uint32_t frame_id;
+ uint32_t count;
+} counters[100];
+
+static void count_msg(uint32_t frame_id)
+{
+ for (uint16_t i=0; i (hal).can_mgr[_driver_index] = can_mgr;
+ can_mgr->begin(1000000, _interface);
+ can_mgr->initialized(true);
+
+ if (!can_mgr->is_initialized()) {
+ debug_can("Can not initialised\n");
+ return;
+ }
+
+ _can_driver = can_mgr->get_driver();
+
+ if (_can_driver == nullptr) {
+ debug_can("KDECAN: no CAN driver\n\r");
+ return;
+ }
+
+ debug_can("KDECAN: init done\n\r");
+}
+
+void KDECAN_sniffer::loop(void)
+{
+ if (_can_driver == nullptr) {
+ return;
+ }
+
+ uavcan::CanFrame empty_frame { (0 | uavcan::CanFrame::FlagEFF), nullptr, 0 };
+ const uavcan::CanFrame* select_frames[uavcan::MaxCanIfaces] { &empty_frame };
+ uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1);
+
+ uavcan::CanSelectMasks inout_mask;
+ inout_mask.read = 1 << _interface;
+
+ 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(_interface)->receive(frame, time, utc_time, flags);
+
+ if (res == 1) {
+ uint32_t id = frame.id & uavcan::CanFrame::MaskExtID;
+ uint8_t object_address = id & 0xFF;
+ uint8_t esc_num = uint8_t((id >> 8) & 0xFF);
+
+ count_msg(id);
+
+ uint8_t i = 0;
+ uint8_t n = NUM_ESCS;
+
+ if (esc_num != BROADCAST_NODE_ID) {
+ for (; i < NUM_ESCS; i++) {
+ if (object_address == UPDATE_NODE_ID_OBJ_ADDR) {
+ if (_esc_info[i].mcu_id == be64toh(*((be64_t*) &(frame.data[0])))) {
+ n = i + 1;
+ break;
+ }
+ } else if (_esc_info[i].node_id == esc_num) {
+ n = i + 1;
+ break;
+ }
+ }
+ }
+
+ while (i < n) {
+ uavcan::CanFrame res_frame;
+
+ switch (object_address) {
+ case ESC_INFO_OBJ_ADDR: {
+ uint8_t info[5] { 1, 2, 3, 4, 0 };
+
+ res_frame.dlc = 5;
+ memcpy(res_frame.data, info, 5);
+
+ break;
+ }
+ case SET_PWM_OBJ_ADDR: {
+ if ((1 << (esc_num - 2) & _mask_received_pwm) && _mask_received_pwm != ((1 << _max_node_id) - 1)) {
+ count_msg(0xFFFFFFF0);
+ _mask_received_pwm = 0;
+ }
+
+ _mask_received_pwm |= 1 << (esc_num - 2);
+
+ if (_mask_received_pwm == ((1 << _max_node_id) - 1)) {
+ count_msg(0xFFFFFFFF);
+ _mask_received_pwm = 0;
+ }
+
+ res_frame.dlc = 0;
+
+ break;
+ }
+ case UPDATE_NODE_ID_OBJ_ADDR: {
+ if (_esc_info[i].enum_timeout != 0 && _esc_info[i].enum_timeout >= AP_HAL::micros64()) {
+ _esc_info[i].node_id = esc_num;
+ _max_node_id = MAX(_max_node_id, esc_num - 2 + 1);
+ hal.console->printf("Set node ID %d for ESC %d\n", esc_num, i);
+ }
+
+ _esc_info[i].enum_timeout = 0;
+
+ res_frame.dlc = 1;
+ memcpy(res_frame.data, &(_esc_info[i].node_id), 1);
+
+ break;
+ }
+ case START_ENUM_OBJ_ADDR: {
+ _esc_info[i].enum_timeout = AP_HAL::micros64() + be16toh(*((be16_t*) &(frame.data[0]))) * 1000;
+ hal.console->printf("Starting enumeration for ESC %d, timeout %" PRIu64 "\n", i, _esc_info[i].enum_timeout);
+ i++;
+ continue;
+ }
+ case TELEMETRY_OBJ_ADDR: {
+ uint8_t data[7] {};
+ *((be16_t*) &data[0]) = htobe16(get_random16());
+ *((be16_t*) &data[2]) = htobe16(get_random16());
+ *((be16_t*) &data[4]) = htobe16(get_random16());
+ data[6] = uint8_t(float(rand()) / RAND_MAX * 40.0f + 15);
+
+ res_frame.dlc = 7;
+ memcpy(res_frame.data, data, 7);
+ break;
+ }
+ case VOLTAGE_OBJ_ADDR:
+ case CURRENT_OBJ_ADDR:
+ case RPM_OBJ_ADDR:
+ case TEMPERATURE_OBJ_ADDR:
+ case GET_PWM_INPUT_OBJ_ADDR:
+ case GET_PWM_OUTPUT_OBJ_ADDR:
+ case MCU_ID_OBJ_ADDR:
+ default:
+ // discard frame
+ return;
+ }
+
+ res_frame.id = (_esc_info[i].node_id << 16) | object_address | uavcan::CanFrame::FlagEFF;
+ timeout = uavcan::MonotonicTime::fromUSec(AP_HAL::millis() + 500);
+ int16_t res2 = _can_driver->getIface(_interface)->send(res_frame, timeout, 0);
+
+ if (res2 == 1) {
+ i++;
+ }
+ }
+ }
+ }
+}
+
+void KDECAN_sniffer::print_stats(void)
+{
+ hal.console->printf("%lu\n", AP_HAL::micros());
+ for (uint16_t i=0;i<100;i++) {
+ if (counters[i].frame_id == 0) {
+ break;
+ }
+ hal.console->printf("0x%08" PRIX32 ": %" PRIu32 "\n", counters[i].frame_id, counters[i].count);
+ counters[i].count = 0;
+ }
+ hal.console->printf("\n");
+}
+
+void KDECAN_sniffer::send_enumeration(uint8_t num)
+{
+ if (_esc_info[num].enum_timeout == 0 || AP_HAL::micros64() > _esc_info[num].enum_timeout) {
+ _esc_info[num].enum_timeout = 0;
+ hal.console->printf("Not running enumeration for ESC %d\n", num);
+ return;
+ }
+
+ while (true) {
+ uint8_t mcu[8] {};
+ *((be64_t*) mcu) = htobe64(_esc_info[num].mcu_id);
+ uavcan::CanFrame res_frame { (_esc_info[num].node_id << 16) | START_ENUM_OBJ_ADDR | uavcan::CanFrame::FlagEFF,
+ mcu,
+ 8 };
+ uavcan::MonotonicTime timeout = uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1);
+
+ int16_t res = _can_driver->getIface(_interface)->send(res_frame, timeout, 0);
+
+ if (res == 1) {
+ return;
+ }
+ }
+}
+
+static KDECAN_sniffer sniffer;
+
+void setup(void)
+{
+ hal.scheduler->delay(2000);
+ hal.console->printf("Starting KDECAN sniffer\n");
+ sniffer.init();
+}
+
+void loop(void)
+{
+ sniffer.loop();
+ static uint32_t last_print_ms;
+ uint32_t now = AP_HAL::millis();
+ if (now - last_print_ms >= 1000) {
+ last_print_ms = now;
+ sniffer.print_stats();
+ }
+
+ if (hal.console->available() >= 3) {
+ char c = hal.console->read();
+
+ if (c == 'e') {
+ c = hal.console->read();
+
+ if (c == ' ') {
+ c = hal.console->read();
+
+ if (c >= '0' && c < '9') {
+ uint8_t num = c - '0';
+ sniffer.send_enumeration(num);
+ }
+ }
+ } else if (c == 'r') {
+ hal.console->printf("rebooting\n");
+ hal.scheduler->reboot(false);
+ }
+ }
+
+ // auto-reboot for --upload
+ if (hal.console->available() > 50) {
+ hal.console->printf("rebooting\n");
+ hal.scheduler->reboot(false);
+ }
+}
+
+AP_HAL_MAIN();
+
+#else
+
+#include
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+static void loop() { }
+static void setup()
+{
+ printf("Board not currently supported\n");
+}
+
+AP_HAL_MAIN();
+#endif
diff --git a/libraries/AP_KDECAN/examples/KDECAN_sniffer/README.md b/libraries/AP_KDECAN/examples/KDECAN_sniffer/README.md
new file mode 100644
index 0000000000..c798462e66
--- /dev/null
+++ b/libraries/AP_KDECAN/examples/KDECAN_sniffer/README.md
@@ -0,0 +1,13 @@
+This is a KDECAN sniffer designed to run on an ArduPilot board. It can
+be used to watch traffic on a KDECAN bus as well as simulate a 8 ESCs (macro changeable).
+
+To build and upload for a Pixhawk style board run this:
+
+```
+ ./waf configure --board fmuv3
+ ./waf --target examples/KDECAN_sniffer --upload
+```
+
+then connect on the USB console. You will see 1Hz packet stats.
+
+You can respond to enumeration by writing 'e X', with X being the ESC number, zero index based.
diff --git a/libraries/AP_KDECAN/examples/KDECAN_sniffer/wscript b/libraries/AP_KDECAN/examples/KDECAN_sniffer/wscript
new file mode 100644
index 0000000000..719ec151ba
--- /dev/null
+++ b/libraries/AP_KDECAN/examples/KDECAN_sniffer/wscript
@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+ bld.ap_example(
+ use='ap',
+ )