diff --git a/libraries/AP_KDECAN/AP_KDECAN.cpp b/libraries/AP_KDECAN/AP_KDECAN.cpp
index 4761ad6b41..88cffb1157 100644
--- a/libraries/AP_KDECAN/AP_KDECAN.cpp
+++ b/libraries/AP_KDECAN/AP_KDECAN.cpp
@@ -30,6 +30,8 @@
 
 extern const AP_HAL::HAL& hal;
 
+#define AP_KDECAN_DEBUG 0
+
 // table of user settable CAN bus parameters
 const AP_Param::GroupInfo AP_KDECAN::var_info[] = {
 
@@ -79,7 +81,7 @@ AP_KDECAN_Driver::AP_KDECAN_Driver() : CANSensor("KDECAN")
 {
     register_driver(AP_CANManager::Driver_Type_KDECAN);
 
-    // start thread for receiving and sending CAN frames
+    // start thread for receiving and sending CAN frames. Tests show we use about 640 bytes of stack
     hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_KDECAN_Driver::loop, void), "kdecan", 2048, AP_HAL::Scheduler::PRIORITY_CAN, 0);
 }
 
@@ -92,33 +94,40 @@ void AP_KDECAN_Driver::handle_frame(AP_HAL::CANFrame &frame)
 
     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);
-    // }
+#if AP_KDECAN_DEBUG
+    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);
+    }
+#endif
+
+    if (id.destination_id != AUTOPILOT_NODE_ID || id.source_id < ESC_NODE_ID_FIRST) {
+        // not for us or invalid id (0 and 1 are invalid)
+        return;
+    }
 
     // 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))
+                (id.source_id < (ARRAY_SIZE(_output.pwm) + ESC_NODE_ID_FIRST)))
             {
-                const uint16_t bitmask = (1U << (id.source_id - ESC_NODE_ID_FIRST));
+                if (__builtin_popcount(_init.detected_bitmask) >= KDECAN_MAX_NUM_ESCS) {
+                    // we already have the maximum number of ESCs
+                    return;
+                }
+                const uint16_t bitmask = (1UL << (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);
+                    GCS_SEND_TEXT(MAV_SEVERITY_INFO,"KDECAN: Found ESC id %u mapped to SERVO%u", id.source_id, id.source_id-1);
                 }
             }
         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)
+            if (frame.dlc == 8 &&
+                (1UL << (id.source_id - ESC_NODE_ID_FIRST) & _init.detected_bitmask))
             {
                 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;
@@ -141,7 +150,8 @@ void AP_KDECAN_Driver::handle_frame(AP_HAL::CANFrame &frame)
 
 void AP_KDECAN_Driver::update(const uint8_t num_poles)
 {
-    if (!hal.util->get_soft_armed() || _init.detected_bitmask == 0) {
+    if (_init.detected_bitmask == 0) {
+        // nothing to do...
         return;
     }
 
@@ -149,30 +159,22 @@ void AP_KDECAN_Driver::update(const uint8_t num_poles)
     _telemetry.num_poles = num_poles;
 #endif
     
-    uint16_t pwm[KDECAN_MAX_NUM_ESCS] {};
-
-    uint8_t index = 0;
-    for (uint16_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
+    WITH_SEMAPHORE(_output.sem);
+    for (uint8_t i = 0; i < ARRAY_SIZE(_output.pwm); i++) {
         if ((_init.detected_bitmask & (1UL<<i)) == 0 || SRV_Channels::channel_function(i) <= SRV_Channel::Aux_servo_function_t::k_none) {
+            _output.pwm[i] = 0;
             continue;
         }
 
         const SRV_Channel *c = SRV_Channels::srv_channel(i);
         if (c == nullptr) {
+            _output.pwm[i] = 0;
             continue;
         }
-        if (index >= ARRAY_SIZE(pwm)) {
-            break;
-        }
-        pwm[index++] = c->get_output_pwm();
+        _output.pwm[i] = c->get_output_pwm();
     }
 
-    {
-        // queue the PWMs for loop()
-        WITH_SEMAPHORE(_output.sem);
-        memcpy(&_output.pwm, &pwm, sizeof(_output.pwm));
-        _output.is_new = true;
-    }
+    _output.is_new = true;
 
 #if AP_KDECAN_USE_EVENTS
     if (_output.thread_ctx != nullptr) {
@@ -180,11 +182,23 @@ void AP_KDECAN_Driver::update(const uint8_t num_poles)
         chEvtSignal(_output.thread_ctx, 1);
     }
 #endif
+
+#if AP_KDECAN_DEBUG
+    static uint32_t last_send_ms = 0;
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - last_send_ms > 1000) {
+        last_send_ms = now_ms;
+        GCS_SEND_TEXT(MAV_SEVERITY_INFO,"%u: %u, %u, %u, %u, %u, %u, %u, %u",
+        (unsigned)_init.detected_bitmask,
+        (unsigned)_output.pwm[0], (unsigned)_output.pwm[1], (unsigned)_output.pwm[2], (unsigned)_output.pwm[3],
+        (unsigned)_output.pwm[4], (unsigned)_output.pwm[5], (unsigned)_output.pwm[6], (unsigned)_output.pwm[7]);
+    }
+#endif
 }
 
 void AP_KDECAN_Driver::loop()
 {
-    uint16_t pwm[KDECAN_MAX_NUM_ESCS] {};
+    uint16_t pwm[ARRAY_SIZE(_output.pwm)] {};
 
 #if AP_KDECAN_USE_EVENTS
     _output.thread_ctx = chThdGetSelfX();
@@ -219,22 +233,11 @@ void AP_KDECAN_Driver::loop()
             }
         }
 
-        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);
+        for (uint8_t i=0; i<ARRAY_SIZE(_output.pwm); i++) {
+            if ((_init.detected_bitmask & (1UL<<i)) != 0) {
+                send_packet_uint16(SET_PWM_OBJ_ADDR, (i + ESC_NODE_ID_FIRST), 1, pwm[i]);
             }
-        } // while index
+        }
 
 #if HAL_WITH_ESC_TELEM
         // broadcast as request-telemetry msg to everyone
@@ -286,25 +289,6 @@ bool AP_KDECAN_Driver::send_packet(const uint8_t address, const uint8_t dest_id,
     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++;
-        }
-    }
-
-    const uint8_t num_present_escs = __builtin_popcount(_init.detected_bitmask);
-
-    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;
-}
-
 // singleton instance
 AP_KDECAN *AP_KDECAN::_singleton;
 
diff --git a/libraries/AP_KDECAN/AP_KDECAN.h b/libraries/AP_KDECAN/AP_KDECAN.h
index 33d49f3a4f..635b100bbd 100644
--- a/libraries/AP_KDECAN/AP_KDECAN.h
+++ b/libraries/AP_KDECAN/AP_KDECAN.h
@@ -51,9 +51,6 @@ public:
     // called from SRV_Channels
     void update(const uint8_t num_poles);
 
-    // check that arming can happen
-    bool pre_arm_check(char* reason, const uint8_t reason_len);
-
 private:
 
     // handler for incoming frames
@@ -65,7 +62,7 @@ private:
     void loop();
 
     struct {
-        uint16_t detected_bitmask;
+        uint32_t detected_bitmask;
         uint32_t detected_bitmask_ms;
     } _init;
 
@@ -73,11 +70,10 @@ private:
         HAL_Semaphore sem;
         bool is_new;
         uint32_t last_new_ms;
-        uint16_t pwm[KDECAN_MAX_NUM_ESCS];
+        uint16_t pwm[NUM_SERVO_CHANNELS];
 #if AP_KDECAN_USE_EVENTS
         thread_t *thread_ctx;
 #endif
-        uint16_t max_node_id;
     } _output;
 
 #if HAL_WITH_ESC_TELEM
@@ -116,10 +112,7 @@ private:
     static const uint8_t TELEMETRY_OBJ_ADDR = 11;
 
 
-    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;
 
 };
 
@@ -137,9 +130,6 @@ public:
 
     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;