From 457d7bdc9d7e684e4262dc94b36c69f4b88aafc8 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 13 Aug 2024 13:59:08 +0100 Subject: [PATCH] AP_IOMCU: add iomcu support for reversible mask propagate armed state --- libraries/AP_IOMCU/AP_IOMCU.cpp | 25 +++++++++++++++----- libraries/AP_IOMCU/AP_IOMCU.h | 3 +++ libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 8 ++++++- libraries/AP_IOMCU/iofirmware/iofirmware.h | 1 + libraries/AP_IOMCU/iofirmware/ioprotocol.h | 1 + 5 files changed, 31 insertions(+), 7 deletions(-) diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index a228c977ae077e..8995d77c6b78ae 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/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,6 +990,13 @@ 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; @@ -1022,17 +1029,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)) { diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h index 5092bf26fa0ddf..107b2248ed70df 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.h +++ b/libraries/AP_IOMCU/AP_IOMCU.h @@ -110,6 +110,9 @@ 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; diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index f76b1e8ce91353..adac0faefe9236 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/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: diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index 6069b3cf2add1b..51dbe418bfd03d 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/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; diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index c6a804ad45c3c2..f13580ec47444b 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/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 {