diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index 461457f726..5c74394275 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -607,15 +607,6 @@ bool AP_IOMCU_FW::handle_code_write() break; } - case PAGE_SAFETY_PWM: { - uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; - if (offset + num_values > sizeof(reg_safety_pwm.pwm)/2) { - return false; - } - memcpy((®_safety_pwm.pwm[0])+offset, &rx_io_packet.regs[0], num_values*2); - break; - } - case PAGE_FAILSAFE_PWM: { uint16_t offset = rx_io_packet.offset, num_values = rx_io_packet.count; if (offset + num_values > sizeof(reg_failsafe_pwm.pwm)/2) { @@ -750,7 +741,7 @@ void AP_IOMCU_FW::fill_failsafe_pwm(void) if (reg_status.flag_safety_off) { reg_direct_pwm.pwm[i] = reg_failsafe_pwm.pwm[i]; } else { - reg_direct_pwm.pwm[i] = reg_safety_pwm.pwm[i]; + reg_direct_pwm.pwm[i] = 0; } } if (mixing.enabled) { diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h index 0a45c6f37f..a4690d1144 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.h +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h @@ -94,11 +94,6 @@ public: uint16_t pwm[IOMCU_MAX_CHANNELS]; } reg_failsafe_pwm; - // PAGE_SAFETY_PWM - struct { - uint16_t pwm[IOMCU_MAX_CHANNELS]; - } reg_safety_pwm; - // output rates struct { uint16_t freq; diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index 4532f4369b..5d7819fc74 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -54,7 +54,6 @@ enum iopage { PAGE_SETUP = 50, PAGE_DIRECT_PWM = 54, PAGE_FAILSAFE_PWM = 55, - PAGE_SAFETY_PWM = 108, PAGE_MIXING = 200, };