mirror of https://github.com/ArduPilot/ardupilot
HAL_ChibiOS: support GPIO get_mode and set_mode
This commit is contained in:
parent
029743e27a
commit
6fa9768eab
|
@ -57,8 +57,9 @@ static struct gpio_entry {
|
||||||
static struct gpio_entry *gpio_by_pin_num(uint8_t pin_num, bool check_enabled=true)
|
static struct gpio_entry *gpio_by_pin_num(uint8_t pin_num, bool check_enabled=true)
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<ARRAY_SIZE(_gpio_tab); i++) {
|
for (uint8_t i=0; i<ARRAY_SIZE(_gpio_tab); i++) {
|
||||||
if (pin_num == _gpio_tab[i].pin_num) {
|
const auto &t = _gpio_tab[i];
|
||||||
if (check_enabled && !_gpio_tab[i].enabled) {
|
if (pin_num == t.pin_num) {
|
||||||
|
if (check_enabled && t.pwm_num != 0 && !t.enabled) {
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
return &_gpio_tab[i];
|
return &_gpio_tab[i];
|
||||||
|
@ -508,6 +509,28 @@ bool GPIO::valid_pin(uint8_t pin) const
|
||||||
return gpio_by_pin_num(pin) != nullptr;
|
return gpio_by_pin_num(pin) != nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) || defined(STM32G4) || defined(STM32L4)
|
||||||
|
|
||||||
|
// allow for save and restore of pin settings
|
||||||
|
bool GPIO::get_mode(uint8_t pin, uint32_t &mode)
|
||||||
|
{
|
||||||
|
auto *p = gpio_by_pin_num(pin);
|
||||||
|
if (!p) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
mode = uint32_t(palReadLineMode(p->pal_line));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void GPIO::set_mode(uint8_t pin, uint32_t mode)
|
||||||
|
{
|
||||||
|
auto *p = gpio_by_pin_num(pin);
|
||||||
|
if (p) {
|
||||||
|
palSetLineMode(p->pal_line, ioline_t(mode));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef IOMCU_FW
|
#ifndef IOMCU_FW
|
||||||
/*
|
/*
|
||||||
timer to setup interrupt quotas for a 100ms period from
|
timer to setup interrupt quotas for a 100ms period from
|
||||||
|
|
|
@ -87,6 +87,12 @@ public:
|
||||||
*/
|
*/
|
||||||
static ioline_t resolve_alt_config(ioline_t base, PERIPH_TYPE ptype, uint8_t instance);
|
static ioline_t resolve_alt_config(ioline_t base, PERIPH_TYPE ptype, uint8_t instance);
|
||||||
|
|
||||||
|
#if defined(STM32F7) || defined(STM32H7) || defined(STM32F4) || defined(STM32F3) || defined(STM32G4) || defined(STM32L4)
|
||||||
|
// allow for save and restore of pin settings
|
||||||
|
bool get_mode(uint8_t pin, uint32_t &mode) override;
|
||||||
|
void set_mode(uint8_t pin, uint32_t mode) override;
|
||||||
|
#endif
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool _usb_connected;
|
bool _usb_connected;
|
||||||
bool _ext_started;
|
bool _ext_started;
|
||||||
|
|
Loading…
Reference in New Issue