|
|
|
@ -294,17 +294,13 @@ void Rover::notify_mode(const Mode *mode)
@@ -294,17 +294,13 @@ void Rover::notify_mode(const Mode *mode)
|
|
|
|
|
*/ |
|
|
|
|
uint8_t Rover::check_digital_pin(uint8_t pin) |
|
|
|
|
{ |
|
|
|
|
const int8_t dpin = hal.gpio->analogPinToDigitalPin(pin); |
|
|
|
|
if (dpin == -1) { |
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
// ensure we are in input mode
|
|
|
|
|
hal.gpio->pinMode(dpin, HAL_GPIO_INPUT); |
|
|
|
|
hal.gpio->pinMode(pin, HAL_GPIO_INPUT); |
|
|
|
|
|
|
|
|
|
// enable pullup
|
|
|
|
|
hal.gpio->write(dpin, 1); |
|
|
|
|
hal.gpio->write(pin, 1); |
|
|
|
|
|
|
|
|
|
return hal.gpio->read(dpin); |
|
|
|
|
return hal.gpio->read(pin); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|