mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
AP_HAL_Linux: Export GPIO pin inside of pinMode()
Make sure pinMode() method exports the pin before using it. Otherwise the export method would need to be called, differently from other GPIO implementations. Since now export_pin and export_pins are called only internally, reduce their visibility to protected.
This commit is contained in:
parent
7033e4d8ed
commit
0c0d595b55
@ -78,8 +78,8 @@ void GPIO_Sysfs::pinMode(uint8_t vpin, uint8_t output)
|
||||
{
|
||||
assert_vpin(vpin, n_pins);
|
||||
|
||||
unsigned pin = pin_table[vpin];
|
||||
_pinMode(pin, output);
|
||||
_export_pin(vpin);
|
||||
_pinMode(pin_table[vpin], output);
|
||||
}
|
||||
|
||||
void GPIO_Sysfs::_pinMode(unsigned int pin, uint8_t output)
|
||||
@ -178,7 +178,7 @@ AP_HAL::DigitalSource* GPIO_Sysfs::channel(uint16_t vpin)
|
||||
unsigned pin = pin_table[vpin];
|
||||
int value_fd = -1;
|
||||
|
||||
if (export_pin(vpin)) {
|
||||
if (_export_pin(vpin)) {
|
||||
char value_path[PATH_MAX];
|
||||
snprintf(value_path, PATH_MAX, GPIO_PATH_FORMAT "/value", pin);
|
||||
|
||||
@ -205,13 +205,13 @@ bool GPIO_Sysfs::usb_connected(void)
|
||||
return false;
|
||||
}
|
||||
|
||||
bool GPIO_Sysfs::export_pin(uint8_t vpin)
|
||||
bool GPIO_Sysfs::_export_pin(uint8_t vpin)
|
||||
{
|
||||
uint8_t vpins[] = { vpin };
|
||||
return export_pins(vpins, 1);
|
||||
return _export_pins(vpins, 1);
|
||||
}
|
||||
|
||||
bool GPIO_Sysfs::export_pins(uint8_t vpins[], size_t len)
|
||||
bool GPIO_Sysfs::_export_pins(uint8_t vpins[], size_t len)
|
||||
{
|
||||
int fd = open("/sys/class/gpio/export", O_WRONLY | O_CLOEXEC);
|
||||
if (fd < 0) {
|
||||
|
@ -64,6 +64,10 @@ public:
|
||||
*/
|
||||
bool usb_connected() override;
|
||||
|
||||
protected:
|
||||
void _pinMode(unsigned int pin, uint8_t output);
|
||||
int _open_pin_value(unsigned int pin, int flags);
|
||||
|
||||
/*
|
||||
* Make pin available for use. This function should be called before
|
||||
* calling functions that use the pin number as parameter.
|
||||
@ -72,7 +76,7 @@ public:
|
||||
*
|
||||
* Note: the pin is ignored if already exported.
|
||||
*/
|
||||
static bool export_pin(uint8_t vpin);
|
||||
static bool _export_pin(uint8_t vpin);
|
||||
|
||||
/*
|
||||
* Make pins available for use. This function should be called before
|
||||
@ -83,11 +87,7 @@ public:
|
||||
*
|
||||
* Note: pins already exported are ignored.
|
||||
*/
|
||||
static bool export_pins(uint8_t vpins[], size_t num_vpins);
|
||||
|
||||
protected:
|
||||
void _pinMode(unsigned int pin, uint8_t output);
|
||||
int _open_pin_value(unsigned int pin, int flags);
|
||||
static bool _export_pins(uint8_t vpins[], size_t num_vpins);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
Loading…
Reference in New Issue
Block a user