mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_ChibiOS: GPIO: Check IOMCU after local pins for speed
This commit is contained in:
parent
06a61f5dd9
commit
bafaffb07e
|
@ -246,12 +246,6 @@ uint8_t GPIO::read(uint8_t pin)
|
||||||
|
|
||||||
void GPIO::write(uint8_t pin, uint8_t value)
|
void GPIO::write(uint8_t pin, uint8_t value)
|
||||||
{
|
{
|
||||||
#if HAL_WITH_IO_MCU
|
|
||||||
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
|
||||||
iomcu.write_GPIO(pin, value);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
struct gpio_entry *g = gpio_by_pin_num(pin);
|
struct gpio_entry *g = gpio_by_pin_num(pin);
|
||||||
if (g) {
|
if (g) {
|
||||||
if (g->is_input) {
|
if (g->is_input) {
|
||||||
|
@ -263,37 +257,43 @@ void GPIO::write(uint8_t pin, uint8_t value)
|
||||||
} else {
|
} else {
|
||||||
palSetLine(g->pal_line);
|
palSetLine(g->pal_line);
|
||||||
}
|
}
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
#if HAL_WITH_IO_MCU
|
||||||
|
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
||||||
|
iomcu.write_GPIO(pin, value);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void GPIO::toggle(uint8_t pin)
|
void GPIO::toggle(uint8_t pin)
|
||||||
{
|
{
|
||||||
#if HAL_WITH_IO_MCU
|
|
||||||
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
|
||||||
iomcu.toggle_GPIO(pin);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
struct gpio_entry *g = gpio_by_pin_num(pin);
|
struct gpio_entry *g = gpio_by_pin_num(pin);
|
||||||
if (g) {
|
if (g) {
|
||||||
palToggleLine(g->pal_line);
|
palToggleLine(g->pal_line);
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
#if HAL_WITH_IO_MCU
|
||||||
|
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
||||||
|
iomcu.toggle_GPIO(pin);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Alternative interface: */
|
/* Alternative interface: */
|
||||||
AP_HAL::DigitalSource* GPIO::channel(uint16_t pin)
|
AP_HAL::DigitalSource* GPIO::channel(uint16_t pin)
|
||||||
{
|
{
|
||||||
|
struct gpio_entry *g = gpio_by_pin_num(pin);
|
||||||
|
if (g != nullptr) {
|
||||||
|
return new DigitalSource(g->pal_line);
|
||||||
|
}
|
||||||
#if HAL_WITH_IO_MCU
|
#if HAL_WITH_IO_MCU
|
||||||
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
||||||
return new IOMCU_DigitalSource(pin);
|
return new IOMCU_DigitalSource(pin);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
struct gpio_entry *g = gpio_by_pin_num(pin);
|
|
||||||
if (!g) {
|
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
return new DigitalSource(g->pal_line);
|
|
||||||
}
|
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
@ -526,12 +526,15 @@ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_u
|
||||||
// check if a pin number is valid
|
// check if a pin number is valid
|
||||||
bool GPIO::valid_pin(uint8_t pin) const
|
bool GPIO::valid_pin(uint8_t pin) const
|
||||||
{
|
{
|
||||||
|
if (gpio_by_pin_num(pin) != nullptr) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
#if HAL_WITH_IO_MCU
|
#if HAL_WITH_IO_MCU
|
||||||
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
return gpio_by_pin_num(pin) != nullptr;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// return servo channel associated with GPIO pin. Returns true on success and fills in servo_ch argument
|
// return servo channel associated with GPIO pin. Returns true on success and fills in servo_ch argument
|
||||||
|
|
Loading…
Reference in New Issue