Line data    Source code

       1             : #include "GPIO.h"
       2             : 
       3             : #include <AP_HAL/AP_HAL.h>
       4             : 
       5             : #include <GCS_MAVLink/GCS.h>
       6             : 
       7             : extern const AP_HAL::HAL& hal;
       8             : 
       9        2290 : AP_HAL::PWMSource::~PWMSource()
      10             : {
      11        2290 :     if (interrupt_attached) {
      12             :         // Assume this is always successful
      13           0 :         hal.gpio->detach_interrupt(_pin);
      14           0 :         interrupt_attached = false;
      15             :     }
      16        2290 : }
      17             : 
      18           0 : bool AP_HAL::PWMSource::set_pin(int16_t new_pin, const char *subsystem)
      19             : {
      20           0 :     if (new_pin == _pin) {
      21             :         // we've already tried to attach to this pin
      22           0 :         return interrupt_attached;
      23             :     }
      24             : 
      25           0 :     if (interrupt_attached) {
      26           0 :         if (!hal.gpio->detach_interrupt(_pin)) {
      27           0 :             GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
      28             :                           "%s: Failed to detach interrupt from %d",
      29             :                           subsystem,
      30             :                           _pin);
      31             :         }
      32           0 :         interrupt_attached = false;
      33             :     }
      34             : 
      35             :     // don't want to try to attach more than once:
      36           0 :     _pin = new_pin;
      37             : 
      38           0 :     if (_pin <= 0) {
      39             :         // invalid pin
      40           0 :         return false;
      41             :     }
      42             : 
      43             :     // install interrupt handler on rising and falling edge
      44           0 :     hal.gpio->pinMode(_pin, HAL_GPIO_INPUT);
      45           0 :     if (!hal.gpio->attach_interrupt(
      46           0 :             _pin,
      47             :             FUNCTOR_BIND_MEMBER(&AP_HAL::PWMSource::irq_handler,
      48             :                                 void,
      49             :                                 uint8_t,
      50             :                                 bool,
      51             :                                 uint32_t),
      52             :             AP_HAL::GPIO::INTERRUPT_BOTH)) {
      53             :         // failed to attach interrupt
      54           0 :         GCS_SEND_TEXT(MAV_SEVERITY_WARNING,
      55             :                       "%s: Failed to attach interrupt to %d",
      56             :                       subsystem,
      57             :                       _pin);
      58           0 :         return false;
      59             :     }
      60             : 
      61           0 :     interrupt_attached = true;
      62             : 
      63           0 :     return interrupt_attached;
      64             : }
      65             : 
      66             : // interrupt handler for reading pwm value
      67           0 : void AP_HAL::PWMSource::irq_handler(uint8_t a_pin, bool pin_high, uint32_t timestamp_us)
      68             : {
      69           0 :     if (pin_high) {
      70           0 :         _pulse_start_us = timestamp_us;
      71           0 :         return;
      72             :     }
      73           0 :     if (_pulse_start_us == 0) {
      74             :         // if we miss interrupts we could get two lows in a row.  If
      75             :         // we miss interrupts we're definitely handing back bad
      76             :         // values anyway....
      77           0 :         return;
      78             :     }
      79           0 :     _irq_value_us = timestamp_us - _pulse_start_us;
      80           0 :     _pulse_start_us = 0;
      81             : 
      82             :     // update fields for taking an average reading:
      83           0 :     _irq_value_us_sum += _irq_value_us;
      84           0 :     _irq_value_us_count++;
      85             : }
      86             : 
      87             : 
      88           0 : uint16_t AP_HAL::PWMSource::get_pwm_us()
      89             : {
      90             :     // disable interrupts and grab state
      91           0 :     void *irqstate = hal.scheduler->disable_interrupts_save();
      92           0 :     const uint32_t ret = _irq_value_us;
      93           0 :     _irq_value_us = 0;
      94           0 :     hal.scheduler->restore_interrupts(irqstate);
      95             : 
      96           0 :     return ret;
      97             : }
      98             : 
      99           0 : uint16_t AP_HAL::PWMSource::get_pwm_avg_us()
     100             : {
     101             :     // disable interrupts and grab state
     102           0 :     void *irqstate = hal.scheduler->disable_interrupts_save();
     103             :     uint32_t ret;
     104           0 :     if (_irq_value_us_count == 0) {
     105           0 :         ret = 0;
     106             :     } else {
     107           0 :         ret = _irq_value_us_sum / _irq_value_us_count;
     108             :     }
     109           0 :     _irq_value_us_sum = 0;
     110           0 :     _irq_value_us_count = 0;
     111           0 :     hal.scheduler->restore_interrupts(irqstate);
     112             : 
     113           0 :     return ret;
     114             : }