AP_IOMCU: reduce latency for oneshot

correctly update outmode modes when requested
andyp1per · Jun 24, 2023 · b2695ca · b2695ca
1 parent e87a71d
commit b2695ca
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 26 deletions.
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -140,8 +140,7 @@ static void dma_tx_end_cb(hal_uart_driver *uart)
     TOGGLE_PIN_DEBUG(108);
     TOGGLE_PIN_DEBUG(108);
 
-    chEvtSignalI(iomcu.thread_ctx, iomcu.fmu_events | IOEVENT_TX_END);
-    iomcu.fmu_events = 0;
+    chEvtSignalI(iomcu.thread_ctx, IOEVENT_TX_END);
 }
 
 /* replacement for ChibiOS uart_lld_serve_interrupt() */
@@ -877,7 +876,7 @@ bool AP_IOMCU_FW::handle_code_write()
             i++;
         }
         fmu_data_received_time = last_ms;
-        fmu_events |= IOEVENT_PWM;
+        chEvtSignalI(thread_ctx, IOEVENT_PWM);
         break;
     }
 
@@ -1062,30 +1061,36 @@ void AP_IOMCU_FW::rcout_config_update(void)
         last_channel_mask = reg_setup.channel_mask;
     }
 
-    bool use_dshot = mode_out.mode >= AP_HAL::RCOutput::MODE_PWM_DSHOT150 
-        && mode_out.mode <= AP_HAL::RCOutput::MODE_PWM_DSHOT300;
-    if (use_dshot && !dshot_enabled) {
-        dshot_enabled = true;
+    // see if there is anything to do, we only support setting the mode for a particular channel once
+    if ((last_output_mode_mask & ~mode_out.mask) == mode_out.mask) {
+        return;
+    }
+
+    switch (mode_out.mode) {
+    case AP_HAL::RCOutput::MODE_PWM_DSHOT150:
+    case AP_HAL::RCOutput::MODE_PWM_DSHOT300:
         hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
         // enabling dshot changes the memory allocation
         reg_status.freemem = hal.util->available_memory();
-    }
-    bool use_oneshot = (mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT
-        || mode_out.mode == AP_HAL::RCOutput::MODE_PWM_ONESHOT125);
-    if (use_oneshot && !oneshot_enabled) {
-        oneshot_enabled = true;
+        last_output_mode_mask |= mode_out.mask;
+        break;
+    case AP_HAL::RCOutput::MODE_PWM_ONESHOT:
+    case AP_HAL::RCOutput::MODE_PWM_ONESHOT125:
         // setup to use a 1Hz frequency, so we only get output when we trigger
         hal.rcout->set_freq(mode_out.mask, 1);
         hal.rcout->set_output_mode(mode_out.mask, (AP_HAL::RCOutput::output_mode)mode_out.mode);
-    }
-    bool use_brushed = mode_out.mode == AP_HAL::RCOutput::MODE_PWM_BRUSHED;
-    if (use_brushed && !brushed_enabled) {
-        brushed_enabled = true;
+        last_output_mode_mask |= mode_out.mask;
+        break;
+    case AP_HAL::RCOutput::MODE_PWM_BRUSHED:
         // default to 2kHz for all channels for brushed output
         hal.rcout->set_freq(mode_out.mask, 2000);
         hal.rcout->set_esc_scaling(1000, 2000);
         hal.rcout->set_output_mode(mode_out.mask, AP_HAL::RCOutput::MODE_PWM_BRUSHED);
         hal.rcout->set_freq(mode_out.mask, reg_setup.pwm_altrate);
+        last_output_mode_mask |= mode_out.mask;
+        break;
+    default:
+        break;
     }
 
     uint32_t output_mask = 0;

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -112,10 +112,12 @@ class AP_IOMCU_FW {
     } rate;
 
     // output mode values
-     struct {
-         uint16_t mask;
-         uint16_t mode;
-     } mode_out;
+    struct {
+        uint16_t mask;
+        uint16_t mode;
+    } mode_out;
+
+    uint16_t last_output_mode_mask;
 
     // MIXER values
     struct page_mixing mixing;
@@ -144,9 +146,6 @@ class AP_IOMCU_FW {
 
     uint32_t fmu_data_received_time;
 
-    // events that will be signaled on transaction completion;
-    eventmask_t fmu_events;
-
     bool pwm_update_pending;
     uint32_t last_heater_ms;
     uint32_t reboot_time;
@@ -161,9 +160,6 @@ class AP_IOMCU_FW {
     uint8_t led_counter;
     uint32_t last_slow_loop_ms;
     uint32_t last_fast_loop_us;
-    bool dshot_enabled;
-    bool oneshot_enabled;
-    bool brushed_enabled;
     thread_t *thread_ctx;
     bool last_safety_off;
     uint32_t last_status_ms;