Add support for using Safety switch pins on IOMCU to control ProfiLEDs via bit banging

Tools/ardupilotwaf/boards.py

@@ -177,7 +177,10 @@ def srcpath(path):
 
         if cfg.options.enable_networking_tests:
             env.CXXFLAGS += ['-DAP_NETWORKING_TESTS_ENABLED=1']
-
+
+        if cfg.options.enable_iomcu_profiled_support:
+            env.CXXFLAGS += ['-DAP_IOMCU_PROFILED_SUPPORT_ENABLED=1']
+
         d = env.get_merged_dict()
         # Always prepend so that arguments passed in the command line get
         # the priority.

Tools/scripts/build_iofirmware.py

@@ -29,12 +29,24 @@ def run_program(cmd_list):
 shutil.copy('build/iomcu/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_lowpolh.bin')
 shutil.copy('build/iomcu/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_highpolh.bin')
 
+run_program(["./waf", "configure", "--board", 'iomcu', '--enable-iomcu-profiled-support'])
+run_program(["./waf", "clean"])
+run_program(["./waf", "iofirmware"])
+shutil.copy('build/iomcu/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_lowpolh.bin')
+shutil.copy('build/iomcu/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_highpolh.bin')
+
 run_program(["./waf", "configure", "--board", 'iomcu-dshot'])
 run_program(["./waf", "clean"])
 run_program(["./waf", "iofirmware"])
 shutil.copy('build/iomcu-dshot/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin')
 shutil.copy('build/iomcu-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_dshot_highpolh.bin')
 
+run_program(["./waf", "configure", "--board", 'iomcu-dshot', '--enable-iomcu-profiled-support'])
+run_program(["./waf", "clean"])
+run_program(["./waf", "iofirmware"])
+shutil.copy('build/iomcu-dshot/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_dshot_lowpolh.bin')
+shutil.copy('build/iomcu-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin')
+
 run_program(["./waf", "configure", "--board", 'iomcu-f103'])
 run_program(["./waf", "clean"])
 run_program(["./waf", "iofirmware"])

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -281,7 +281,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Board options
     // @Description: Board specific option flags
-    // @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm
+    // @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm, 9:Use safety pins as profiled
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 19, AP_BoardConfig, _options, HAL_BRD_OPTIONS_DEFAULT),
 

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -158,7 +158,8 @@ class AP_BoardConfig {
         WRITE_PROTECT_FLASH = (1<<5),
         WRITE_PROTECT_BOOTLOADER = (1<<6),
         SKIP_BOARD_VALIDATION = (1<<7),
-        DISABLE_ARMING_GPIO = (1<<8)
+        DISABLE_ARMING_GPIO = (1<<8),
+        IO_SAFETY_PINS_AS_PROFILED = (1<<9),
     };
 
     //return true if arming gpio output is disabled
@@ -200,6 +201,12 @@ class AP_BoardConfig {
         return _singleton?(_singleton->_options & ALLOW_SET_INTERNAL_PARM)!=0:false;
     }
 
+#if HAL_WITH_IO_MCU
+    static bool use_safety_as_led(void) {
+        return _singleton?(_singleton->_options & IO_SAFETY_PINS_AS_PROFILED)!=0:false;
+    }
+#endif
+
     // handle press of safety button. Return true if safety state
     // should be toggled
     bool safety_button_handle_pressed(uint8_t press_count);

libraries/AP_HAL_ChibiOS/hwdef/CubeBlack+/hwdef.dat

@@ -19,4 +19,6 @@ IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180
 IMU Invensense SPI:mpu9250 ROTATION_YAW_270
 
 undef ROMFS
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
+
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1

libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat

@@ -102,8 +102,9 @@ define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5
 
 # default the 2nd interface to SLCAN
 define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
 # enable support for dshot on iomcu
 define HAL_WITH_IO_MCU_DSHOT 1

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc

@@ -321,12 +321,14 @@ define HAL_GPIO_PWM_VOLT_3v3 1
 # we can automatically update the IOMCU firmware on boot. The format
 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the
 # ardupilot root.
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
 # enable support for dshot on iomcu
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
 define HAL_WITH_IO_MCU_DSHOT 1
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
+
 DMA_NOSHARE SPI1* SPI4* USART6*
 
 # for users who have replaced their CubeSolo with a CubeOrange:

libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat

@@ -470,9 +470,10 @@ STORAGE_FLASH_PAGE 1
 # we can automatically update the IOMCU firmware on boot. The format
 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the
 # ardupilot root
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
 # enable support for dshot on iomcu
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
 define HAL_WITH_IO_MCU_DSHOT 1
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -43,6 +43,7 @@ enum ioevents {
     IOEVENT_SET_DSHOT_PERIOD,
     IOEVENT_SET_CHANNEL_MASK,
     IOEVENT_DSHOT,
+    IOEVENT_PROFILED,
 };
 
 // max number of consecutve protocol failures we accept before raising
@@ -89,7 +90,9 @@ void AP_IOMCU::init(void)
         crc_is_ok = true;
     }
 #endif
-
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    use_safety_as_led = boardconfig->use_safety_as_led();
+#endif
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
                                       1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
         AP_HAL::panic("Unable to allocate IOMCU thread");
@@ -300,6 +303,16 @@ void AP_IOMCU::thread_main(void)
         }
         mask &= ~EVENT_MASK(IOEVENT_DSHOT);
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+        if (mask & EVENT_MASK(IOEVENT_PROFILED)) {
+            if (!write_registers(PAGE_PROFILED, 0, sizeof(profiled)/sizeof(uint16_t), (const uint16_t*)&profiled)) {
+                event_failed(mask);
+                continue;
+            }
+        }
+        mask &= ~EVENT_MASK(IOEVENT_PROFILED);
+#endif
+
         // check for regular timed events
         uint32_t now = AP_HAL::millis();
         if (now - last_rc_read_ms > 20) {
@@ -1436,6 +1449,23 @@ void AP_IOMCU::toggle_GPIO(uint8_t pin)
     trigger_event(IOEVENT_GPIO);
 }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+// set profiled R G B values
+void AP_IOMCU::set_profiled(uint8_t r, uint8_t g, uint8_t b)
+{
+    if (!use_safety_as_led) {
+        return;
+    }
+    if (r == profiled.red && g == profiled.green && b == profiled.blue) {
+        return;
+    }
+    profiled.magic = PROFILED_ENABLE_MAGIC;
+    profiled.red = r;
+    profiled.green = g;
+    profiled.blue = b;
+    trigger_event(IOEVENT_PROFILED);
+}
+#endif
 
 namespace AP {
     AP_IOMCU *iomcu(void) {

libraries/AP_IOMCU/AP_IOMCU.h

@@ -177,6 +177,11 @@ class AP_IOMCU
     // toggle a output pin
     void toggle_GPIO(uint8_t pin);
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // set profiled values
+    void set_profiled(uint8_t r, uint8_t g, uint8_t b);
+#endif
+
     // channel group masks
     const uint8_t ch_masks[3] = { 0x03,0x0C,0xF0 };
 
@@ -298,6 +303,11 @@ class AP_IOMCU
     // output mode values
     struct page_mode_out mode_out;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // profiled control
+    struct page_profiled profiled {0, 255, 255, 255}; // by default, white
+#endif
+
     // IMU heater duty cycle
     uint8_t heater_duty_cycle;
 
@@ -311,6 +321,9 @@ class AP_IOMCU
     bool detected_io_reset;
     bool initialised;
     bool is_chibios_backend;
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    bool use_safety_as_led;
+#endif
 
     uint32_t protocol_fail_count;
     uint32_t protocol_count;

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -489,6 +489,10 @@ void AP_IOMCU_FW::update()
         last_status_ms = now;
         page_status_update();
     }
+
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    profiled_update();
+#endif
 #ifdef HAL_WITH_BIDIR_DSHOT
     // EDT updates are semt at ~1Hz per ESC, but we want to make sure
     // that we don't delay updates unduly so sample at 5Hz
@@ -1029,6 +1033,20 @@ bool AP_IOMCU_FW::handle_code_write()
         break;
     }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    case PAGE_PROFILED:
+        if (rx_io_packet.count != 2 || (rx_io_packet.regs[0] & 0xFF) != PROFILED_ENABLE_MAGIC) {
+            return false;
+        }
+        profiled_brg[0] = rx_io_packet.regs[0] >> 8;
+        profiled_brg[1] = rx_io_packet.regs[1] & 0xFF;
+        profiled_brg[2] = rx_io_packet.regs[1] >> 8;
+        // push new led data
+        profiled_num_led_pushed = 0;
+        profiled_control_enabled = true;
+        break;
+#endif
+
     default:
         break;
     }
@@ -1063,6 +1081,48 @@ void AP_IOMCU_FW::calculate_fw_crc(void)
     reg_setup.crc[1] = sum >> 16;
 }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+// bitbang profiled bitstream, 8-10 chunks at a time
+// Max time taken per call is ~7us
+void AP_IOMCU_FW::profiled_update(void)
+{
+    if (profiled_num_led_pushed > PROFILED_LED_LEN) {
+        profiled_byte_index = 0;
+        profiled_leading_zeros = PROFILED_LEADING_ZEROS;
+        return;
+    }
+
+    // push 10 zero leading bits at a time
+    if (profiled_leading_zeros != 0) {
+        for (uint8_t i = 0; i < 10; i++) {
+            palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+            palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+            profiled_leading_zeros--;
+        }
+        return;
+    }
+
+    if ((profiled_byte_index == 0) ||
+        (profiled_byte_index == PROFILED_OUTPUT_BYTE_LEN)) {
+        // start bit
+        palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        palSetLine(HAL_GPIO_PIN_SAFETY_LED);
+        palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        profiled_byte_index = 0;
+        profiled_num_led_pushed++;
+    }
+
+    uint8_t byte_val = profiled_brg[profiled_byte_index];
+    for (uint8_t i = 0; i < 8; i++) {
+        palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        palWriteLine(HAL_GPIO_PIN_SAFETY_LED, byte_val & 1);
+        byte_val >>= 1;
+        palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+    }
+
+    profiled_byte_index++;
+}
+#endif
 
 /*
   update safety state
@@ -1076,6 +1136,15 @@ void AP_IOMCU_FW::safety_update(void)
     }
     safety_update_ms = now;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    if (profiled_control_enabled) {
+        // set line mode to output for safety input pin
+        palSetLineMode(HAL_GPIO_PIN_SAFETY_INPUT, PAL_MODE_OUTPUT_PUSHPULL);
+        palSetLineMode(HAL_GPIO_PIN_SAFETY_LED, PAL_MODE_OUTPUT_PUSHPULL);
+        return;
+    }
+#endif
+
     bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
     if (safety_pressed) {
         if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -17,6 +17,10 @@
 #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
 #define SERVO_COUNT 8
 
+#define PROFILED_LED_LEN         2
+#define PROFILED_OUTPUT_BYTE_LEN 3
+#define PROFILED_LEADING_ZEROS   50
+
 class AP_IOMCU_FW {
 public:
     void process_io_packet();
@@ -36,6 +40,9 @@ class AP_IOMCU_FW {
     bool handle_code_write();
     bool handle_code_read();
     void schedule_reboot(uint32_t time_ms);
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    void profiled_update();
+#endif
     void safety_update();
     void rcout_config_update();
     void rcin_serial_init();
@@ -81,6 +88,14 @@ class AP_IOMCU_FW {
 
     uint16_t last_channel_mask;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    uint8_t profiled_byte_index;
+    uint8_t profiled_leading_zeros;
+    uint8_t profiled_num_led_pushed;
+    uint8_t profiled_brg[3];
+    bool profiled_control_enabled;
+#endif
+
     // CONFIG values
     struct page_config config;
 
@@ -117,6 +132,11 @@ class AP_IOMCU_FW {
     // output mode values
     struct page_mode_out mode_out;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // profiled control values
+    struct page_profiled profiled;
+#endif
+
     uint16_t last_output_mode_mask;
     uint16_t last_output_bdmask;
     uint16_t last_output_esc_type;