Browse Source

HAL_ChibiOS: fixed attach_interrupt check

we can't have two handlers on the same pad
mission-4.1.18
Andrew Tridgell 7 years ago
parent
commit
64c8ca514c
  1. 13
      libraries/AP_HAL_ChibiOS/GPIO.cpp

13
libraries/AP_HAL_ChibiOS/GPIO.cpp

@ -135,10 +135,21 @@ bool GPIO::_attach_interrupt(ioline_t line, AP_HAL::Proc p, uint8_t mode) @@ -135,10 +135,21 @@ bool GPIO::_attach_interrupt(ioline_t line, AP_HAL::Proc p, uint8_t mode)
return false;
}
break;
}
if (p) {
osalSysLock();
palevent_t *pep = pal_lld_get_line_event(line);
if (pep->cb) {
// the pad is already being used for a callback
osalSysUnlock();
return false;
}
osalSysUnlock();
}
palDisableLineEvent(line);
palEnableLineEvent(line, chmode);
palSetLineCallback(line, pal_interrupt_cb, (void*)p);
palSetLineCallback(line, p?pal_interrupt_cb:nullptr, (void*)p);
return true;
}

Loading…
Cancel
Save