mirror of https://github.com/ArduPilot/ardupilot
112 lines
2.9 KiB
C++
112 lines
2.9 KiB
C++
#include "GPIO.h"
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#ifndef HAL_BOOTLOADER_BUILD
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#endif
|
|
|
|
#ifndef GCS_SEND_TEXT
|
|
#define GCS_SEND_TEXT(severity, format, args...)
|
|
#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;
|
|
}
|