Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : #include "AP_RPM_config.h"
      17             : 
      18             : #if AP_RPM_PIN_ENABLED
      19             : 
      20             : #include "RPM_Pin.h"
      21             : 
      22             : #include <AP_HAL/AP_HAL.h>
      23             : #include <AP_HAL/GPIO.h>
      24             : #include <GCS_MAVLink/GCS.h>
      25             : #include <AP_Math/AP_Math.h>
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : AP_RPM_Pin::IrqState AP_RPM_Pin::irq_state[RPM_MAX_INSTANCES];
      29             : 
      30             : /*
      31             :   handle interrupt on an instance
      32             :  */
      33           0 : void AP_RPM_Pin::irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)
      34             : {
      35           0 :     const uint32_t dt = timestamp - irq_state[state.instance].last_pulse_us;
      36           0 :     irq_state[state.instance].last_pulse_us = timestamp;
      37             :     // we don't accept pulses less than 100us. Using an irq for such
      38             :     // high RPM is too inaccurate, and it is probably just bounce of
      39             :     // the signal which we should ignore
      40           0 :     if (dt > 100 && dt < 1000*1000) {
      41           0 :         irq_state[state.instance].dt_sum += dt;
      42           0 :         irq_state[state.instance].dt_count++;
      43             :     }
      44           0 : }
      45             : 
      46           0 : void AP_RPM_Pin::update(void)
      47             : {
      48           0 :     if (last_pin != get_pin()) {
      49             :         // detach from last pin
      50           0 :         if (interrupt_attached) {
      51             :             // ignore this failure of the user may be stuck
      52           0 :             IGNORE_RETURN(hal.gpio->detach_interrupt(last_pin));
      53           0 :             interrupt_attached = false;
      54             :         }
      55           0 :         irq_state[state.instance].dt_count = 0;
      56           0 :         irq_state[state.instance].dt_sum = 0;
      57             :         // attach to new pin
      58           0 :         last_pin = get_pin();
      59           0 :         if (last_pin > 0) {
      60           0 :             hal.gpio->pinMode(last_pin, HAL_GPIO_INPUT);
      61           0 :             if (hal.gpio->attach_interrupt(
      62           0 :                     last_pin,
      63             :                     FUNCTOR_BIND_MEMBER(&AP_RPM_Pin::irq_handler, void, uint8_t, bool, uint32_t),
      64             :                     AP_HAL::GPIO::INTERRUPT_RISING)) {
      65           0 :                 interrupt_attached = true;
      66             :             } else {
      67           0 :                 GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "RPM: Failed to attach to pin %d", last_pin);
      68             :             }
      69             :         }
      70             :     }
      71             : 
      72           0 :     if (irq_state[state.instance].dt_count > 0) {
      73             : 
      74             :         // disable interrupts to prevent race with irq_handler
      75           0 :         void *irqstate = hal.scheduler->disable_interrupts_save();
      76           0 :         const float dt_avg = static_cast<float>(irq_state[state.instance].dt_sum) / irq_state[state.instance].dt_count;
      77           0 :         irq_state[state.instance].dt_count = 0;
      78           0 :         irq_state[state.instance].dt_sum = 0;
      79           0 :         hal.scheduler->restore_interrupts(irqstate);
      80             : 
      81           0 :         const float scaling = ap_rpm._params[state.instance].scaling;
      82           0 :         const float maximum = ap_rpm._params[state.instance].maximum;
      83           0 :         const float minimum = ap_rpm._params[state.instance].minimum;
      84             :         float quality;
      85           0 :         const float rpm = scaling * (1.0e6 / dt_avg) * 60;
      86           0 :         const float filter_value = signal_quality_filter.get();
      87             : 
      88           0 :         state.rate_rpm = signal_quality_filter.apply(rpm);
      89             : 
      90           0 :         if ((maximum <= 0 || rpm <= maximum) && (rpm >= minimum)) {
      91           0 :             if (is_zero(filter_value)){
      92           0 :                 quality = 0;
      93             :             } else {
      94           0 :                 quality = 1 - constrain_float((fabsf(rpm-filter_value))/filter_value, 0.0, 1.0);
      95           0 :                 quality = powf(quality, 2.0);
      96             :             }
      97           0 :             state.last_reading_ms = AP_HAL::millis();
      98             :         } else {
      99           0 :             quality = 0;
     100             :         }
     101           0 :         state.signal_quality = (0.1 * quality) + (0.9 * state.signal_quality);
     102             :     }
     103             : 
     104             :     // assume we get readings at at least 1Hz, otherwise reset quality to zero
     105           0 :     if (AP_HAL::millis() - state.last_reading_ms > 1000) {
     106           0 :         state.signal_quality = 0;
     107           0 :         state.rate_rpm = 0;
     108             :     }
     109           0 : }
     110             : 
     111             : #endif  // AP_RPM_PIN_ENABLED