ardupilot/libraries/AP_HAL/GPIO.cpp

109 lines
2.9 KiB
C++

#include "GPIO.h"
#include <AP_HAL/AP_HAL.h>
#if defined(HAL_NO_GCS) || defined(HAL_BOOTLOADER_BUILD)
#define GCS_SEND_TEXT(severity, format, args...)
#else
#include <GCS_MAVLink/GCS.h>
#endif
extern const AP_HAL::HAL& hal;
bool AP_HAL::PWMSource::set_pin(int16_t new_pin, const char *subsystem)
{
if (new_pin == _pin) {
// we've already tried to attach to this pin
return interrupt_attached;
}
if (interrupt_attached) {
if (!hal.gpio->detach_interrupt(_pin)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
"%s: Failed to detach interrupt from %d",
subsystem,
_pin);
}
interrupt_attached = false;
}
// don't want to try to attach more than once:
_pin = new_pin;
if (_pin <= 0) {
// invalid pin
return false;
}
// install interrupt handler on rising and falling edge
hal.gpio->pinMode(_pin, HAL_GPIO_INPUT);
if (!hal.gpio->attach_interrupt(
_pin,
FUNCTOR_BIND_MEMBER(&AP_HAL::PWMSource::irq_handler,
void,
uint8_t,
bool,
uint32_t),
AP_HAL::GPIO::INTERRUPT_BOTH)) {
// failed to attach interrupt
GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
"%s: Failed to attach interrupt to %d",
subsystem,
_pin);
return false;
}
interrupt_attached = true;
return interrupt_attached;
}
// interrupt handler for reading pwm value
void AP_HAL::PWMSource::irq_handler(uint8_t a_pin, bool pin_high, uint32_t timestamp_us)
{
if (pin_high) {
_pulse_start_us = timestamp_us;
return;
}
if (_pulse_start_us == 0) {
// if we miss interrupts we could get two lows in a row. If
// we miss interrupts we're definitely handing back bad
// values anyway....
return;
}
_irq_value_us = timestamp_us - _pulse_start_us;
_pulse_start_us = 0;
// update fields for taking an average reading:
_irq_value_us_sum += _irq_value_us;
_irq_value_us_count++;
}
uint16_t AP_HAL::PWMSource::get_pwm_us()
{
// disable interrupts and grab state
void *irqstate = hal.scheduler->disable_interrupts_save();
const uint32_t ret = _irq_value_us;
_irq_value_us = 0;
hal.scheduler->restore_interrupts(irqstate);
return ret;
}
uint16_t AP_HAL::PWMSource::get_pwm_avg_us()
{
// disable interrupts and grab state
void *irqstate = hal.scheduler->disable_interrupts_save();
uint32_t ret;
if (_irq_value_us_count == 0) {
ret = 0;
} else {
ret = _irq_value_us_sum / _irq_value_us_count;
}
_irq_value_us_sum = 0;
_irq_value_us_count = 0;
hal.scheduler->restore_interrupts(irqstate);
return ret;
}