Support reversible mask in iomcu

libraries/AP_HAL_ChibiOS/RCOutput.cpp

@@ -650,6 +650,11 @@ uint32_t RCOutput::get_disabled_channels(uint32_t digital_mask)
     }
 
     disabled_chan_mask <<= chan_offset;
+#if HAL_WITH_IO_MCU
+    if (iomcu_dshot) {
+       disabled_chan_mask |= iomcu.get_disabled_channels(digital_mask);
+    }
+#endif
     return disabled_chan_mask;
 }
 
@@ -1464,9 +1469,9 @@ void RCOutput::dshot_send_groups(rcout_timer_t cycle_start_us, rcout_timer_t tim
     }
 
     for (auto &group : pwm_group_list) {
-        bool pulse_sent;
+        bool pulse_sent = false;
         // send a dshot command
-        if (is_dshot_protocol(group.current_mode)
+        if (group.can_send_dshot_pulse()
             && dshot_command_is_active(group)) {
             command_sent = dshot_send_command(group, _dshot_current_command.command, _dshot_current_command.chan);
             pulse_sent = true;
@@ -1638,9 +1643,8 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
     bdshot_prepare_for_next_pulse(group);
 #endif
     bool safety_on = hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED;
-#if !defined(IOMCU_FW)
     bool armed = hal.util->get_soft_armed();
-#endif
+
     memset((uint8_t *)group.dma_buffer, 0, DSHOT_BUFFER_LENGTH);
 
     for (uint8_t i=0; i<4; i++) {
@@ -1651,7 +1655,9 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
                 bdshot_decode_telemetry_from_erpm(group.bdshot.erpm[i], chan);
             }
 #endif
-            if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) {
+            const uint32_t servo_chan_mask = 1U<<(chan+chan_offset);
+
+            if (safety_on && !(safety_mask & servo_chan_mask)) {
                 // safety is on, don't output anything
                 continue;
             }
@@ -1663,12 +1669,10 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
                 continue;
             }
 
-            const uint32_t chan_mask = (1U<<chan);
-
             pwm = constrain_int16(pwm, 1000, 2000);
             uint16_t value = MIN(2 * (pwm - 1000), 1999);
 
-            if ((chan_mask & _reversible_mask) != 0) {
+            if ((servo_chan_mask & _reversible_mask) != 0) {
                 // this is a DShot-3D output, map so that 1500 PWM is zero throttle reversed
                 if (value < 1000) {
                     value = 1999 - value;
@@ -1684,13 +1688,14 @@ void RCOutput::dshot_send(pwm_group &group, rcout_timer_t cycle_start_us, rcout_
             if (value != 0) {
                 value += DSHOT_ZERO_THROTTLE;
             }
-#if !defined(IOMCU_FW)
+
             if (!armed) {
                 // when disarmed we always send a zero value
                 value = 0;
             }
-#endif
+
             // according to sskaug requesting telemetry while trying to arm may interfere with the good frame calc
+            const uint32_t chan_mask = (1U<<chan);
             bool request_telemetry = telem_request_mask & chan_mask;
             uint16_t packet = create_dshot_packet(value, request_telemetry,
 #ifdef HAL_WITH_BIDIR_DSHOT

libraries/AP_HAL_ChibiOS/RCOutput_iofirmware.cpp

@@ -204,7 +204,7 @@ void RCOutput::bdshot_reset_pwm_f1(pwm_group& group, uint8_t telem_channel)
     // we need to switch every output on the same input channel to avoid
     // spurious line changes
     for (uint8_t i = 0; i<4; i++) {
-        if (group.chan[i] == CHAN_DISABLED) {
+        if (!group.is_chan_enabled(i)) {
             continue;
         }
         if (group.bdshot.telem_tim_ch[telem_channel] == group.bdshot.telem_tim_ch[i]) {
@@ -250,7 +250,7 @@ void RCOutput::bdshot_receive_pulses_DMAR_f1(pwm_group* group)
     // we need to switch every input on the same input channel to allow
     // the ESCs to drive the lines
     for (uint8_t i = 0; i<4; i++) {
-        if (group->chan[i] == CHAN_DISABLED) {
+        if (!group->is_chan_enabled(i)) {
             continue;
         }
         if (group->bdshot.telem_tim_ch[curr_ch] == group->bdshot.telem_tim_ch[i]) {

libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp

@@ -33,6 +33,8 @@ using namespace ChibiOS;
 
 extern const AP_HAL::HAL& hal;
 
+// send a dshot command to group
+// chan is the FMU channel to send the command to
 bool RCOutput::dshot_send_command(pwm_group& group, uint8_t command, uint8_t chan)
 {
     if (!group.can_send_dshot_pulse()) {
@@ -112,10 +114,11 @@ void RCOutput::send_dshot_command(uint8_t command, uint8_t chan, uint32_t comman
     DshotCommandPacket pkt;
     pkt.command = command;
     if (chan != ALL_CHANNELS) {
-        pkt.chan = chan - chan_offset;
+        pkt.chan = chan - chan_offset;  // normalize to FMU channel
     } else {
         pkt.chan = ALL_CHANNELS;
     }
+
     if (command_timeout_ms == 0) {
         pkt.cycle = MAX(10, repeat_count);
     } else {
@@ -140,6 +143,12 @@ void RCOutput::set_reversed_mask(uint32_t chanmask) {
 // The mask uses servo channel numbering
 void RCOutput::set_reversible_mask(uint32_t chanmask) {
     _reversible_mask |= chanmask;
+#if HAL_WITH_IO_MCU
+    const uint32_t iomcu_mask = ((1U<<chan_offset)-1);
+    if (iomcu_dshot && (chanmask & iomcu_mask)) {
+        iomcu.set_reversible_mask(chanmask & iomcu_mask);
+    }
+#endif
 }
 
 // Update the dshot outputs that should be reversible/3D at 1Hz
@@ -150,8 +159,9 @@ void RCOutput::update_channel_masks() {
         return;
     }
 
-#if HAL_PWM_COUNT > 0
-    for (uint8_t i=chan_offset; i<HAL_PWM_COUNT+chan_offset; i++) {
+    // The masks use servo channel numbering
+#if NUM_SERVO_CHANNELS > 0
+    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
         switch (_dshot_esc_type) {
             case DSHOT_ESC_BLHELI:
             case DSHOT_ESC_BLHELI_S:

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -156,10 +156,9 @@ void AP_IOMCU::thread_main(void)
 
             DEV_PRINTF("IOMCU: 0x%lx\n", config.mcuid);
 
-            // set IO_ARM_OK and FMU_ARMED
-            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, 0,
+            // set IO_ARM_OK and clear FMU_ARMED
+            if (!modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, P_SETUP_ARMING_FMU_ARMED,
                                  P_SETUP_ARMING_IO_ARM_OK |
-                                 P_SETUP_ARMING_FMU_ARMED |
                                  P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
                 event_failed(mask);
                 continue;
@@ -338,7 +337,8 @@ void AP_IOMCU::thread_main(void)
             last_telem_read_ms = AP_HAL::millis();
         }
 #endif
-        if (now - last_safety_option_check_ms > 1000) {
+        // update options at the same rate that the iomcu updates the state
+        if (now - last_safety_option_check_ms > 100) {
             update_safety_options();
             last_safety_option_check_ms = now;
         }
@@ -990,12 +990,29 @@ void AP_IOMCU::set_bidir_dshot_mask(uint16_t mask)
     trigger_event(IOEVENT_SET_OUTPUT_MODE);
 }
 
+// set reversible mask
+void AP_IOMCU::set_reversible_mask(uint16_t mask)
+{
+    mode_out.reversible_mask = mask;
+    trigger_event(IOEVENT_SET_OUTPUT_MODE);
+}
+
 AP_HAL::RCOutput::output_mode AP_IOMCU::get_output_mode(uint8_t& mask) const
 {
     mask = reg_status.rcout_mask;
     return AP_HAL::RCOutput::output_mode(reg_status.rcout_mode);
 }
 
+uint32_t AP_IOMCU::get_disabled_channels(uint32_t digital_mask) const
+{
+    uint32_t dig_out = reg_status.rcout_mask & (digital_mask & 0xFF);
+    if (dig_out > 0
+        && AP_HAL::RCOutput::is_dshot_protocol(AP_HAL::RCOutput::output_mode(reg_status.rcout_mode))) {
+        return ~dig_out & 0xFF;
+    }
+    return 0;
+}
+
 // setup channels
 void  AP_IOMCU::enable_ch(uint8_t ch)
 {
@@ -1022,17 +1039,23 @@ void AP_IOMCU::update_safety_options(void)
     }
     uint16_t desired_options = 0;
     uint16_t options = boardconfig->get_safety_button_options();
+    bool armed = hal.util->get_soft_armed();
     if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_OFF)) {
         desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_OFF;
     }
     if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_SAFETY_ON)) {
         desired_options |= P_SETUP_ARMING_SAFETY_DISABLE_ON;
     }
-    if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && AP::arming().is_armed()) {
+    if (!(options & AP_BoardConfig::BOARD_SAFETY_OPTION_BUTTON_ACTIVE_ARMED) && armed) {
         desired_options |= (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
     }
+    // update armed state
+    if (armed) {
+        desired_options |= P_SETUP_ARMING_FMU_ARMED;
+    }
+
     if (last_safety_options != desired_options) {
-        uint16_t mask = (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF);
+        uint16_t mask = (P_SETUP_ARMING_SAFETY_DISABLE_ON | P_SETUP_ARMING_SAFETY_DISABLE_OFF | P_SETUP_ARMING_FMU_ARMED);
         uint32_t bits_to_set = desired_options & mask;
         uint32_t bits_to_clear = (~desired_options) & mask;
         if (modify_register(PAGE_SETUP, PAGE_REG_SETUP_ARMING, bits_to_clear, bits_to_set)) {

libraries/AP_IOMCU/AP_IOMCU.h

@@ -110,9 +110,15 @@ class AP_IOMCU
     // set bi-directional mask
     void set_bidir_dshot_mask(uint16_t mask);
 
+    // set reversible mask
+    void set_reversible_mask(uint16_t mask);
+
     // get output mode
     AP_HAL::RCOutput::output_mode get_output_mode(uint8_t& mask) const;
 
+    // approximation to disabled channel
+    uint32_t get_disabled_channels(uint32_t digital_mask) const;
+
     // MCUID
     uint32_t get_mcu_id() const { return config.mcuid; }
 

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -920,6 +920,7 @@ bool AP_IOMCU_FW::handle_code_write()
             mode_out.mode = rx_io_packet.regs[1];
             mode_out.bdmask = rx_io_packet.regs[2];
             mode_out.esc_type = rx_io_packet.regs[3];
+            mode_out.reversible_mask = rx_io_packet.regs[4];
             break;
 
         case PAGE_REG_SETUP_HEATER_DUTY_CYCLE:
@@ -1097,6 +1098,8 @@ void AP_IOMCU_FW::safety_update(void)
             hal.rcout->force_safety_on();
         }
     }
+    // update the armed state
+    hal.util->set_soft_armed((reg_setup.arming & P_SETUP_ARMING_FMU_ARMED) != 0);
 
 #if IOMCU_ENABLE_RESET_TEST
     {
@@ -1175,6 +1178,7 @@ void AP_IOMCU_FW::rcout_config_update(void)
     // 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
         && (last_output_bdmask & mode_out.bdmask) == mode_out.bdmask
+        && (last_output_reversible_mask & mode_out.reversible_mask) == mode_out.reversible_mask
         && last_output_esc_type == mode_out.esc_type) {
         return;
     }
@@ -1187,13 +1191,15 @@ void AP_IOMCU_FW::rcout_config_update(void)
 #endif
 #ifdef HAL_WITH_BIDIR_DSHOT
         hal.rcout->set_bidir_dshot_mask(mode_out.bdmask);
-        hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type));
 #endif
+        hal.rcout->set_reversible_mask(mode_out.reversible_mask);
+        hal.rcout->set_dshot_esc_type(AP_HAL::RCOutput::DshotEscType(mode_out.esc_type));
         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();
         last_output_mode_mask |= mode_out.mask;
         last_output_bdmask |= mode_out.bdmask;
+        last_output_reversible_mask |= mode_out.reversible_mask;
         last_output_esc_type = mode_out.esc_type;
         break;
     case AP_HAL::RCOutput::MODE_PWM_ONESHOT:

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -120,6 +120,7 @@ class AP_IOMCU_FW {
     uint16_t last_output_mode_mask;
     uint16_t last_output_bdmask;
     uint16_t last_output_esc_type;
+    uint16_t last_output_reversible_mask;
 
     // MIXER values
     struct page_mixing mixing;

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -200,6 +200,7 @@ struct page_mode_out {
     uint16_t mode;
     uint16_t bdmask;
     uint16_t esc_type;
+    uint16_t reversible_mask;
 };
 
 struct page_dshot {