Browse Source

AP_IOMCU: remove safe PWM

gps-1.3.1
Iampete1 3 years ago committed by Andrew Tridgell
parent
commit
c1a2379504
  1. 27
      libraries/AP_IOMCU/AP_IOMCU.cpp
  2. 6
      libraries/AP_IOMCU/AP_IOMCU.h

27
libraries/AP_IOMCU/AP_IOMCU.cpp

@ -243,14 +243,6 @@ void AP_IOMCU::thread_main(void)
last_safety_option_check_ms = now; last_safety_option_check_ms = now;
} }
// update safety pwm
if (pwm_out.safety_pwm_set != pwm_out.safety_pwm_sent) {
uint8_t set = pwm_out.safety_pwm_set;
if (write_registers(PAGE_SAFETY_PWM, 0, IOMCU_MAX_CHANNELS, pwm_out.safety_pwm)) {
pwm_out.safety_pwm_sent = set;
}
}
// update failsafe pwm // update failsafe pwm
if (pwm_out.failsafe_pwm_set != pwm_out.failsafe_pwm_sent) { if (pwm_out.failsafe_pwm_set != pwm_out.failsafe_pwm_sent) {
uint8_t set = pwm_out.failsafe_pwm_set; uint8_t set = pwm_out.failsafe_pwm_set;
@ -852,25 +844,6 @@ bool AP_IOMCU::check_crc(void)
return false; return false;
} }
/*
set the pwm to use when safety is on
*/
void AP_IOMCU::set_safety_pwm(uint16_t chmask, uint16_t period_us)
{
bool changed = false;
for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) {
if (chmask & (1U<<i)) {
if (pwm_out.safety_pwm[i] != period_us) {
pwm_out.safety_pwm[i] = period_us;
changed = true;
}
}
}
if (changed) {
pwm_out.safety_pwm_set++;
}
}
/* /*
set the pwm to use when in FMU failsafe set the pwm to use when in FMU failsafe
*/ */

6
libraries/AP_IOMCU/AP_IOMCU.h

@ -46,9 +46,6 @@ public:
// force safety off // force safety off
void force_safety_off(void); void force_safety_off(void);
// set PWM of channels when safety is on
void set_safety_pwm(uint16_t chmask, uint16_t period_us);
// set mask of channels that ignore safety state // set mask of channels that ignore safety state
void set_safety_mask(uint16_t chmask); void set_safety_mask(uint16_t chmask);
@ -184,9 +181,6 @@ private:
struct { struct {
uint8_t num_channels; uint8_t num_channels;
uint16_t pwm[IOMCU_MAX_CHANNELS]; uint16_t pwm[IOMCU_MAX_CHANNELS];
uint8_t safety_pwm_set;
uint8_t safety_pwm_sent;
uint16_t safety_pwm[IOMCU_MAX_CHANNELS];
uint16_t safety_mask; uint16_t safety_mask;
uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS]; uint16_t failsafe_pwm[IOMCU_MAX_CHANNELS];
uint8_t failsafe_pwm_set; uint8_t failsafe_pwm_set;

Loading…
Cancel
Save