Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IOMCU: add iomcu support for reversible mask 

propagate armed state
This commit is contained in:
Andy Piper 2024-08-13 13:59:08 +01:00 committed by Andrew Tridgell
parent fc51829b63
commit 7f8e5aab85
5 changed files with 31 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_IOMCU/AP_IOMCU.cpp
View File

@ -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)) {

3
libraries/AP_IOMCU/AP_IOMCU.h
View File

@ -110,6 +110,9 @@ public:
// 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;

8
libraries/AP_IOMCU/iofirmware/iofirmware.cpp
View File

@ -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:

1
libraries/AP_IOMCU/iofirmware/iofirmware.h
View File

@ -120,6 +120,7 @@ public:
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;

1
libraries/AP_IOMCU/iofirmware/ioprotocol.h
View File

@ -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 {